diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 7c9f6b73afa..082ec35c24f 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.27.24" +version = "0.27.25" # 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 48e522f1f0e..7ff5ef82d4f 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.27.25 (2024-12-11) +~~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Modified :class:`omni.isaac.lab.envs.mdp.actions.DifferentialInverseKinematicsAction` class to use the geometric + Jacobian computed w.r.t. to the root frame of the robot. This helps ensure that root pose does not affect the tracking. + + 0.27.24 (2024-12-09) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py index 96154116c81..a8a1108f509 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py @@ -117,6 +117,19 @@ def raw_actions(self) -> torch.Tensor: def processed_actions(self) -> torch.Tensor: return self._processed_actions + @property + def jacobian_w(self) -> torch.Tensor: + return self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._jacobi_joint_ids] + + @property + def jacobian_b(self) -> torch.Tensor: + jacobian = self.jacobian_w + base_rot = self._asset.data.root_quat_w + base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot)) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + return jacobian + """ Operations. """ @@ -178,7 +191,7 @@ def _compute_frame_jacobian(self): the right Jacobian from the parent body Jacobian. """ # read the parent jacobian - jacobian = self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._jacobi_joint_ids] + jacobian = self.jacobian_b # account for the offset if self.cfg.body_offset is not None: # Modify the jacobian to account for the offset diff --git a/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py b/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py index 4d5877aaa1c..17a069d5b91 100644 --- a/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py +++ b/source/extensions/omni.isaac.lab/test/controllers/test_differential_ik.py @@ -22,7 +22,14 @@ import omni.isaac.lab.sim as sim_utils from omni.isaac.lab.assets import Articulation from omni.isaac.lab.controllers import DifferentialIKController, DifferentialIKControllerCfg -from omni.isaac.lab.utils.math import compute_pose_error, subtract_frame_transforms + +from omni.isaac.lab.utils.math import ( # isort:skip + compute_pose_error, + matrix_from_quat, + quat_inv, + random_yaw_orientation, + subtract_frame_transforms, +) ## # Pre-defined configs @@ -169,6 +176,10 @@ def _run_ik_controller( robot.write_joint_state_to_sim(joint_pos, joint_vel) robot.set_joint_position_target(joint_pos) robot.write_data_to_sim() + # randomize root state yaw, ik should work regardless base rotation + root_state = robot.data.root_state_w.clone() + root_state[:, 3:7] = random_yaw_orientation(self.num_envs, self.sim.device) + robot.write_root_state_to_sim(root_state) robot.reset() # reset actions ee_pose_b_des[:] = self.ee_pose_b_des_set[current_goal_idx] @@ -185,6 +196,10 @@ def _run_ik_controller( jacobian = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] root_pose_w = robot.data.root_state_w[:, 0:7] + base_rot = root_pose_w[:, 3:7] + base_rot_matrix = matrix_from_quat(quat_inv(base_rot)) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) joint_pos = robot.data.joint_pos[:, arm_joint_ids] # compute frame in root frame ee_pos_b, ee_quat_b = subtract_frame_transforms(