From a8f3a2f4b02d2b4aee622d1e3d2d1b255fdfa215 Mon Sep 17 00:00:00 2001 From: Octi Date: Mon, 28 Oct 2024 21:40:01 -0700 Subject: [PATCH] use the geometric jacobian computed w.r.t to the root frame of the robot. --- .../omni.isaac.lab/config/extension.toml | 2 +- .../omni.isaac.lab/docs/CHANGELOG.rst | 4 ++++ .../lab/envs/mdp/actions/task_space_actions.py | 15 ++++++++++++++- .../test/controllers/test_differential_ik.py | 17 ++++++++++++++++- 4 files changed, 35 insertions(+), 3 deletions(-) diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 4a6faf6114e..18485c39069 100644 --- a/source/extensions/omni.isaac.lab/config/extension.toml +++ b/source/extensions/omni.isaac.lab/config/extension.toml @@ -2,7 +2,7 @@ # Note: Semantic Versioning is used: https://semver.org/ -version = "0.27.7" +version = "0.27.8" # 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 ecc04717942..2ee0c24e05e 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,10 @@ Changelog --------- +0.27.8 (2024-10-28) + +* 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.7 (2024-10-28) ~~~~~~~~~~~~~~~~~~~ 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(