From 6c3d62444b0c583ac63f9d3fb30b54f2bd6f9604 Mon Sep 17 00:00:00 2001 From: DorsaRoh Date: Sat, 19 Oct 2024 14:35:13 -0400 Subject: [PATCH 1/4] Fix: Position Threshold Check for State Transitions --- CONTRIBUTORS.md | 1 + .../state_machine/lift_cube_sm.py | 50 +++++++++++-------- 2 files changed, 30 insertions(+), 21 deletions(-) diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 3c345a9d31..727a75e120 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -42,6 +42,7 @@ Guidelines for modifications: * Calvin Yu * Chenyu Yang * David Yang +* Dorsa Rohani * Gary Lvov * Giulio Romualdi * HoJin Jeon diff --git a/source/standalone/environments/state_machine/lift_cube_sm.py b/source/standalone/environments/state_machine/lift_cube_sm.py index bd14dcaf0d..9fd982678b 100644 --- a/source/standalone/environments/state_machine/lift_cube_sm.py +++ b/source/standalone/environments/state_machine/lift_cube_sm.py @@ -81,6 +81,12 @@ class PickSmWaitTime: LIFT_OBJECT = wp.constant(1.0) +@wp.func +def distance_below_threshold(current_pos: wp.vec3, + desired_pos: wp.vec3, + threshold: float) -> bool: + return wp.length(current_pos - desired_pos) < threshold + @wp.kernel def infer_state_machine( dt: wp.array(dtype=float), @@ -92,6 +98,7 @@ def infer_state_machine( des_ee_pose: wp.array(dtype=wp.transform), gripper_state: wp.array(dtype=float), offset: wp.array(dtype=wp.transform), + position_threshold:float ): # retrieve thread id tid = wp.tid() @@ -109,21 +116,20 @@ def infer_state_machine( elif state == PickSmState.APPROACH_ABOVE_OBJECT: des_ee_pose[tid] = wp.transform_multiply(offset[tid], object_pose[tid]) gripper_state[tid] = GripperState.OPEN - # TODO: error between current and desired ee pose below threshold - # wait for a while - if sm_wait_time[tid] >= PickSmWaitTime.APPROACH_OBJECT: - # move to next state and reset wait time - sm_state[tid] = PickSmState.APPROACH_OBJECT - sm_wait_time[tid] = 0.0 + if distance_below_threshold(wp.transform_get_translation(ee_pose[tid]), wp.transform_get_translation(des_ee_pose[tid]), position_threshold): + # wait for a while + if sm_wait_time[tid] >= PickSmWaitTime.APPROACH_OBJECT: + # move to next state and reset wait time + sm_state[tid] = PickSmState.APPROACH_OBJECT + sm_wait_time[tid] = 0.0 elif state == PickSmState.APPROACH_OBJECT: des_ee_pose[tid] = object_pose[tid] gripper_state[tid] = GripperState.OPEN - # TODO: error between current and desired ee pose below threshold - # wait for a while - if sm_wait_time[tid] >= PickSmWaitTime.APPROACH_OBJECT: - # move to next state and reset wait time - sm_state[tid] = PickSmState.GRASP_OBJECT - sm_wait_time[tid] = 0.0 + if distance_below_threshold(wp.transform_get_translation(ee_pose[tid]), wp.transform_get_translation(des_ee_pose[tid]), position_threshold): + if sm_wait_time[tid] >= PickSmWaitTime.APPROACH_OBJECT: + # move to next state and reset wait time + sm_state[tid] = PickSmState.GRASP_OBJECT + sm_wait_time[tid] = 0.0 elif state == PickSmState.GRASP_OBJECT: des_ee_pose[tid] = object_pose[tid] gripper_state[tid] = GripperState.CLOSE @@ -135,12 +141,12 @@ def infer_state_machine( elif state == PickSmState.LIFT_OBJECT: des_ee_pose[tid] = des_object_pose[tid] gripper_state[tid] = GripperState.CLOSE - # TODO: error between current and desired ee pose below threshold - # wait for a while - if sm_wait_time[tid] >= PickSmWaitTime.LIFT_OBJECT: - # move to next state and reset wait time - sm_state[tid] = PickSmState.LIFT_OBJECT - sm_wait_time[tid] = 0.0 + if distance_below_threshold(wp.transform_get_translation(ee_pose[tid]), wp.transform_get_translation(des_ee_pose[tid]), position_threshold): + # wait for a while + if sm_wait_time[tid] >= PickSmWaitTime.LIFT_OBJECT: + # move to next state and reset wait time + sm_state[tid] = PickSmState.LIFT_OBJECT + sm_wait_time[tid] = 0.0 # increment wait time sm_wait_time[tid] = sm_wait_time[tid] + dt[tid] @@ -160,7 +166,7 @@ class PickAndLiftSm: 5. LIFT_OBJECT: The robot lifts the object to the desired pose. This is the final state. """ - def __init__(self, dt: float, num_envs: int, device: torch.device | str = "cpu"): + def __init__(self, dt: float, num_envs: int, device: torch.device | str = "cpu", position_threshold=0.01): """Initialize the state machine. Args: @@ -172,6 +178,7 @@ def __init__(self, dt: float, num_envs: int, device: torch.device | str = "cpu") self.dt = float(dt) self.num_envs = num_envs self.device = device + self.position_threshold = position_threshold # initialize state machine self.sm_dt = torch.full((self.num_envs,), self.dt, device=self.device) self.sm_state = torch.full((self.num_envs,), 0, dtype=torch.int32, device=self.device) @@ -201,7 +208,7 @@ def reset_idx(self, env_ids: Sequence[int] = None): self.sm_state[env_ids] = 0 self.sm_wait_time[env_ids] = 0.0 - def compute(self, ee_pose: torch.Tensor, object_pose: torch.Tensor, des_object_pose: torch.Tensor): + def compute(self, ee_pose: torch.Tensor, object_pose: torch.Tensor, des_object_pose: torch.Tensor) -> torch.Tensor: """Compute the desired state of the robot's end-effector and the gripper.""" # convert all transformations from (w, x, y, z) to (x, y, z, w) ee_pose = ee_pose[:, [0, 1, 2, 4, 5, 6, 3]] @@ -227,6 +234,7 @@ def compute(self, ee_pose: torch.Tensor, object_pose: torch.Tensor, des_object_p self.des_ee_pose_wp, self.des_gripper_state_wp, self.offset_wp, + self.position_threshold, ], device=self.device, ) @@ -257,7 +265,7 @@ def main(): desired_orientation = torch.zeros((env.unwrapped.num_envs, 4), device=env.unwrapped.device) desired_orientation[:, 1] = 1.0 # create state machine - pick_sm = PickAndLiftSm(env_cfg.sim.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device) + pick_sm = PickAndLiftSm(env_cfg.sim.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device, position_threshold=0.01) while simulation_app.is_running(): # run everything in inference mode From ddc3ef168e739c3582a840d9504925dc6d3069a8 Mon Sep 17 00:00:00 2001 From: DorsaRoh Date: Sat, 19 Oct 2024 14:50:51 -0400 Subject: [PATCH 2/4] update version in extension.toml --- source/extensions/omni.isaac.lab/config/extension.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index fa76c37685..3d3baeb5c2 100644 --- a/source/extensions/omni.isaac.lab/config/extension.toml +++ b/source/extensions/omni.isaac.lab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.26.0" +version = "0.26.1" # Description title = "Isaac Lab framework for Robot Learning" From 5844d4a508604662a4249299e353b67bacb0ff9d Mon Sep 17 00:00:00 2001 From: Kelly Guo Date: Sun, 15 Dec 2024 00:05:10 -0500 Subject: [PATCH 3/4] fix spacing --- source/extensions/omni.isaac.lab/docs/CHANGELOG.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index 1f951534aa..dea595b330 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -10,7 +10,6 @@ Changed * Added check for error below threshold in state machines to ensure the state has been reached. - 0.27.27 (2024-12-13) ~~~~~~~~~~~~~~~~~~~~ From dc43bb318b62b0789b835dc46cac5b1606fc7e71 Mon Sep 17 00:00:00 2001 From: Kelly Guo Date: Sun, 15 Dec 2024 01:26:01 -0500 Subject: [PATCH 4/4] fix version --- source/extensions/omni.isaac.lab/config/extension.toml | 2 +- source/extensions/omni.isaac.lab/docs/CHANGELOG.rst | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 6786e5ecd3..ff6bb6a617 100644 --- a/source/extensions/omni.isaac.lab/config/extension.toml +++ b/source/extensions/omni.isaac.lab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.28.1" +version = "0.27.28" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index dea595b330..bb9698c9c6 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,8 +1,8 @@ Changelog --------- -0.28.1 (2024-12-14) -~~~~~~~~~~~~~~~~~~~ +0.27.28 (2024-12-14) +~~~~~~~~~~~~~~~~~~~~ Changed ^^^^^^^