Computes Jacobian in the root frame inside the DifferentialInverseKinematicsAction class

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"

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)
 ~~~~~~~~~~~~~~~~~~~

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

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(