[Bug Report] OperationalSpaceControllerAction uses incorrect joint indices for mass_matrix and gravity on floating-base robots

[Vitamin0304]

Describe the bug

When using the OperationalSpaceControllerAction on a floating-base articulation (e.g., a mobile manipulator with a moving chassis), the controller extracts the incorrect subset of the generalized mass matrix and gravity compensation forces from the PhysX view.

This happens because the function _compute_dynamic_quantities() directly uses self._joint_ids instead of accounting for the 6-DoF offset of the floating base. As a result, the dynamic quantities are severely misaligned (e.g., an arm wrist joint might incorrectly receive the mass/inertia of a heavy wheel motor or torso).

I observed this issue directly on my floating-base robot, the Galaxea R1 Pro. This indexing misalignment causes the calculated task-space torques to explode (e.g., outputting > 500 N·m on extremely light joints). The corrupted mass matrix or gravity vector is poisoning the downstream calculations.

Steps to reproduce

1. Configure an OperationalSpaceControllerAction on the Galaxea R1 Pro mobile manipulator and controlling its left arm:

        self.actions.arm_action = OperationalSpaceControllerActionCfg(
            class_type=OperationalSpaceControllerAction,
            asset_name="robot",
            joint_names=["left_arm_joint[1-7]"],
            body_name="left_gripper_link",
            controller_cfg=OperationalSpaceControllerCfg(
                class_type=OperationalSpaceController,
                target_types=["pose_abs"],
                impedance_mode="fixed",
                inertial_dynamics_decoupling=False,
                partial_inertial_dynamics_decoupling=False,
                gravity_compensation=False,
                motion_stiffness_task=50.0,
                motion_damping_ratio_task=1.0,
                motion_stiffness_limits_task=(50.0, 500.0),
                nullspace_control="position",
                nullspace_stiffness=20.0,
                nullspace_damping_ratio=0.8,
            ),
            position_scale=1.0,
            orientation_scale=1.0,
            stiffness_scale=1.0,
            body_offset=OperationalSpaceControllerActionCfg.OffsetCfg(pos=[0, 0.0, -0.05]),
            nullspace_joint_pos_target="default",
        )

2. Read the commanded joint efforts during the simulation step using the following code:

    arm_action_term = env.env.action_manager.get_term("arm_action")
    osc_computed_torques = arm_action_term._joint_efforts.cpu().numpy()[0]
    print("osc_computed_torques=", osc_computed_torques)

3. Expected behavior: Smooth and reasonable torques proportional to the arm's actual inertia:

osc_computed_torques= [-15.905763    -0.02303561   3.7642915    0.21863192   0.12336276  -0.576664    -0.1341066 ]
osc_computed_torques= [-15.906109    -0.02412623   3.7643142    0.21868123   0.12332171  -0.5766963   -0.1340812 ]
osc_computed_torques= [-15.906429    -0.0250241    3.7643566    0.2186842    0.12329902  -0.5767288   -0.13406916]
osc_computed_torques= [-15.906006    -0.02309989   3.764346     0.2188638    0.12333607  -0.57670337  -0.13407342]

4. Actual behavior: The robot violently shakes, and the output torques exhibit absurdly high values because the mass matrix is fetched using unshifted indices (ignoring the first 6 indices of the floating base in the generalized mass matrix):

osc_computed_torques= [  429.9282    136.55771 -1351.9838     68.87749 -1099.508     648.1993   991.29224]
osc_computed_torques= [-424.7126  -193.74025 1230.6783   -75.95021  950.38947 -657.591   -924.3126 ]
osc_computed_torques= [  558.62054    314.82068  -1496.605       41.421825  -221.07108   631.8254     795.9958  ]
osc_computed_torques= [-668.6095   -362.26102   808.4911     68.535095  238.5092   -680.1988 -642.1984  ]

System Info

Describe the characteristic of your environment:

• Commit: f4aa17f
• Isaac Sim Version: 5.10
• OS: Ubuntu 22.04
• GPU: RTX 3090
• CUDA: 12.8
• GPU Driver: 570.211

Additional context

Root Cause & Proposed Fix:
In the __init__ method of OperationalSpaceControllerAction, the 6-DoF offset is correctly handled for the Jacobian indices (self._jacobi_joint_idx):

if self._asset.is_fixed_base:
    # ...
else:
    self._jacobi_ee_body_idx = self._ee_body_idx
    self._jacobi_joint_idx = [i + 6 for i in self._joint_ids] # Correctly shifted

However, in _compute_dynamic_quantities(), it mistakenly falls back to using the unshifted self._joint_ids:

# CURRENT BUGGY IMPLEMENTATION
def _compute_dynamic_quantities(self):
    self._mass_matrix[:] = self._asset.root_physx_view.get_generalized_mass_matrices()[:, self._joint_ids, :][
        :, :, self._joint_ids
    ]
    self._gravity[:] = self._asset.root_physx_view.get_gravity_compensation_forces()[:, self._joint_ids]

The Fix: Replacing self._joint_ids with self._jacobi_joint_idx completely resolves the issue and restores operational space control for floating-base robots. After applying this modification locally, the robot is successfully controlled without torque explosions. Furthermore, enabling inertial_dynamics_decoupling=True and gravity_compensation=True now works perfectly as intended without any indexing misalignment issues.

# PROPOSED FIX
def _compute_dynamic_quantities(self):
    """Computes the dynamic quantities for operational space control."""
    mass_and_grav_indices = self._jacobi_joint_idx
    
    self._mass_matrix[:] = self._asset.root_physx_view.get_generalized_mass_matrices()[:, mass_and_grav_indices, :][
        :, :, mass_and_grav_indices
    ]
    self._gravity[:] = self._asset.root_physx_view.get_gravity_compensation_forces()[:, mass_and_grav_indices]

Checklist

• I have checked that there is no similar issue in the repo (required)
• I have checked that the issue is not in running Isaac Sim itself and is related to the repo
• I have provided a clear and concise description of the bug.
• I have provided a minimal example to reproduce the bug.

Acceptance Criteria

• The _compute_dynamic_quantities() method in OperationalSpaceControllerAction is updated to properly use self._jacobi_joint_idx (or equivalent logic) instead of self._joint_ids to retrieve the mass matrix and gravity vector.

• Operational Space Control runs stably on floating-base articulations (e.g., mobile manipulators) without index misalignment or outputting unbounded/incorrect torques due to fetching the chassis' inertial properties.

[RandomOakForest]

Thank you for posting this. The team will review it.