Merge pull request #2 from aravindskumar-nvidia/aravikumar/ik-qol

Fix Jacobian column offset for Newton floating-base

Author: hujc7

source/isaaclab/isaaclab/assets/articulation/base_articulation.py

@@ -173,6 +173,24 @@ def root_view(self):
         """
         raise NotImplementedError()
 
+    @property
+    def num_jacobi_joints(self) -> int:
+        """Size of the Jacobian's joint axis (its last dimension).
+
+        For most backends this equals :attr:`num_joints`. Some
+        backends prepend the 6 floating-base DoFs to the Jacobian's
+        joint axis without counting them in :attr:`num_joints`; on
+        those, ``num_jacobi_joints`` is ``num_joints + 6`` for
+        floating-base assets. The default returns :attr:`num_joints`;
+        backends that carry extra leading columns override.
+
+        To convert a logical joint index (from :meth:`find_joints`)
+        into the matching Jacobian column index, add the difference
+        ``num_jacobi_joints - num_joints`` — ``0`` for the default
+        convention, ``6`` for the prepend-floating-base convention.
+        """
+        return self.num_joints
+
     def get_jacobians(self) -> wp.array:
         """Per-env geometric Jacobians, referenced at each link origin in world frame.
 
@@ -181,26 +199,30 @@ def get_jacobians(self) -> wp.array:
 
         .. code-block:: text
 
-            J[:, body_idx, 0:3, :] @ q_dot == data.body_link_lin_vel_w[:, body_idx]
-            J[:, body_idx, 3:6, :] @ q_dot == data.body_link_ang_vel_w[:, body_idx]
+            J[:, jacobi_body_idx, 0:3, :] @ q_dot == data.body_link_lin_vel_w[:, body_idx]
+            J[:, jacobi_body_idx, 3:6, :] @ q_dot == data.body_link_ang_vel_w[:, body_idx]
 
         Linear rows ``[0:3]`` give the velocity at the link origin (the
         body's USD prim transform / actor frame) in world frame.
         Angular rows ``[3:6]`` give the body's angular velocity in
         world frame. Both share the contract used by
         :attr:`~isaaclab.assets.ArticulationData.body_link_pos_w` and
         :attr:`~isaaclab.assets.ArticulationData.body_link_lin_vel_w`.
+        For fixed-base articulations, ``jacobi_body_idx`` excludes the
+        fixed root body and is therefore ``body_idx - 1``. For
+        floating-base articulations, ``jacobi_body_idx == body_idx``.
 
         Implementations whose native Jacobian is expressed at a
         different reference point (e.g. body center of mass) MUST shift
         the linear rows to the link origin before returning so the
         contract above holds across backends.
 
-        Floating-base joint-dim convention differs across backends:
-        some prepend the floating-base 6 DoFs to the joint dimension,
-        others fold them into the native joint-DoF count. The total
-        joint dimension is the same; only how :attr:`num_joints` is
-        reported differs.
+        The joint-dimension size is reported by
+        :attr:`num_jacobi_joints`. Backends that prepend floating-base
+        DoFs to the Jacobian columns expose the offset implicitly via
+        ``num_jacobi_joints - num_joints``; callers that index into
+        the joint axis should consult that property rather than
+        hard-coding a constant.
 
         Returns:
             The per-env geometric Jacobian. Shape

source/isaaclab/isaaclab/envs/mdp/actions/rmpflow_task_space_actions.py

@@ -61,10 +61,12 @@ def __init__(self, cfg: rmpflow_actions_cfg.RMPFlowActionCfg, env: ManagerBasedE
         # this means that number of bodies is one less than the articulation's number of bodies
         if self._asset.is_fixed_base:
             self._jacobi_body_idx = self._body_idx - 1
-            self._jacobi_joint_ids = self._joint_ids
         else:
             self._jacobi_body_idx = self._body_idx
-            self._jacobi_joint_ids = [i + 6 for i in self._joint_ids]
+        # See ``DifferentialInverseKinematicsAction.__init__`` for the rationale
+        # behind this offset.
+        jacobi_joint_offset = self._asset.num_jacobi_joints - self._asset.num_joints
+        self._jacobi_joint_ids = [i + jacobi_joint_offset for i in self._joint_ids]
 
         # log info for debugging
         logger.info(

source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py

@@ -77,10 +77,14 @@ def __init__(self, cfg: actions_cfg.DifferentialInverseKinematicsActionCfg, env:
         # this means that number of bodies is one less than the articulation's number of bodies
         if self._asset.is_fixed_base:
             self._jacobi_body_idx = self._body_idx - 1
-            self._jacobi_joint_ids = self._joint_ids
         else:
             self._jacobi_body_idx = self._body_idx
-            self._jacobi_joint_ids = [i + 6 for i in self._joint_ids]
+        # Map logical joint indices to Jacobian column indices. Some backends
+        # (e.g. PhysX, floating-base) prepend the 6 floating-base DoFs to the
+        # Jacobian's joint axis without counting them in ``num_joints``;
+        # ``num_jacobi_joints - num_joints`` is that leading offset (0 or 6).
+        jacobi_joint_offset = self._asset.num_jacobi_joints - self._asset.num_joints
+        self._jacobi_joint_ids = [i + jacobi_joint_offset for i in self._joint_ids]
 
         # log info for debugging
         logger.info(
@@ -305,10 +309,12 @@ def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: Ma
         # this means that number of bodies is one less than the articulation's number of bodies
         if self._asset.is_fixed_base:
             self._jacobi_ee_body_idx = self._ee_body_idx - 1
-            self._jacobi_joint_idx = self._joint_ids
         else:
             self._jacobi_ee_body_idx = self._ee_body_idx
-            self._jacobi_joint_idx = [i + 6 for i in self._joint_ids]
+        # See ``DifferentialInverseKinematicsAction.__init__`` for the rationale
+        # behind this offset.
+        jacobi_joint_offset = self._asset.num_jacobi_joints - self._asset.num_joints
+        self._jacobi_joint_idx = [i + jacobi_joint_offset for i in self._joint_ids]
 
         # log info for debugging
         logger.info(

source/isaaclab_newton/test/assets/test_articulation.py

@@ -18,6 +18,7 @@
 """Rest everything follows."""
 
 import sys
+from copy import deepcopy
 
 import pytest
 import torch
@@ -465,7 +466,7 @@ def sim(request):
     else:
         add_ground_plane = False  # default to no ground plane
     articulation_type = request.getfixturevalue("articulation_type")
-    sim_cfg = SIM_CFGs[articulation_type]
+    sim_cfg = deepcopy(SIM_CFGs[articulation_type])
     sim_cfg.device = device
     # ``gravity_enabled`` is silently ignored by ``build_simulation_context``
     # when an explicit ``sim_cfg`` is also passed; apply it here so the

source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py

@@ -179,6 +179,13 @@ def root_view(self) -> physx.ArticulationView:
         """
         return self._root_view
 
+    @property
+    def num_jacobi_joints(self) -> int:
+        # PhysX's Jacobian prepends 6 floating-base columns for floating-base
+        # articulations; ``num_joints`` does not count them. Fixed-base assets
+        # have no prepended columns.
+        return self.num_joints if self.is_fixed_base else self.num_joints + 6
+
     def get_jacobians(self) -> wp.array:
         return self._root_view.get_jacobians()