Fixed bug with NullSpacePostureTask. Simplified robot articulation

configuration by moving some config parameters to a common configclass.
Removed unused file.

Author: michaellin6

source/isaaclab/docs/CHANGELOG.rst

@@ -19,8 +19,8 @@ Changed
   improvements, our unit tests pass position and orientation accuracy test within **(2 mm, 1 degree)**. Previously, the position accuracy tolerances
   were set to **(30 mm, 10 degrees)**.
 * Including a new config parameter :attr:`fail_on_ik_error` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg`
-  to control whether the IK controller should fail if robot joint limits are exceeded. This adds to stability of the controller and avoids
-  operator experiencing sudden large delays in control.
+  to control whether the IK controller raise an exception if robot joint limits are exceeded. In the case of an exception, the controller will hold the
+  last joint position. This adds to stability of the controller and avoids operator experiencing what is perceived as sudden large delays in robot control.
 
 
 0.45.2 (2025-08-18)

source/isaaclab/isaaclab/controllers/pink_ik/__init__.py

@@ -0,0 +1,13 @@
+# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Pink IK controller package for IsaacLab.
+
+This package provides integration between Pink inverse kinematics solver and IsaacLab.
+"""
+
+from .null_space_posture_task import NullSpacePostureTask
+from .pink_ik import PinkIKController
+from .pink_ik_cfg import PinkIKControllerCfg

source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py

@@ -151,31 +151,29 @@ def compute_error(self, configuration: Configuration) -> np.ndarray:
         _, root_nv = get_root_joint_dim(configuration.model)
 
         # Compute configuration difference
-        diff = pin.difference(
+        return pin.difference(
             configuration.model,
             self.target_q,
             configuration.q,
         )[root_nv:]
 
-        return diff
-
     def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
-        r"""Compute the posture task Jacobian (null space projector).
-
-        This method computes the sum of the left and right null space projectors,
-        each defined as :math:`N(q) = I - J^+J`, where :math:`J^+` is the pseudoinverse
-        of the corresponding end-effector Jacobian. The null space projectors are
-        masked to only affect the selected controlled joints for each side.
-
-        The final Jacobian returned is:
-            :math:`J(q) = N_{\text{left}}(q) + N_{\text{right}}(q)`
-
-        Args:
-            configuration: Robot configuration :math:`q`.
-
-        Returns:
-            Posture task Jacobian :math:`J(q)`, which is the sum of the left and right
-            null space projectors, each masked to their respective controlled joints.
+        r"""Computes the product of left and right null space projectors
+        :math:`N(q) = I - J^+J`, where :math:`J^+` is the pseudoinverse of the
+        corresponding end-effector Jacobian. The null space projector ensures
+        joint velocities in the null space produce zero end-effector velocity
+        (:math:`J \cdot \dot{q}_{\text{null}} = 0`), allowing posture control
+        without affecting end-effector motion.
+
+            The final Jacobian returned is:
+                :math:`J(q) = N_{\text{task_1}}(q) * N_{\text{task_2}}(q)`
+
+            Args:
+                configuration: Robot configuration :math:`q`.
+
+            Returns:
+                Posture task Jacobian :math:`J(q)`, which is the sum of the left and right
+                null space projectors, each masked to their respective controlled joints.
         """
         # Initialize joint mapping if needed
         if self._joint_name_to_index is None:
@@ -199,19 +197,23 @@ def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
             self._jacobian_dim = J_frame_tasks[0].shape[1]
 
         # Initialize null space task matrix
-        null_space_task = np.zeros((self._jacobian_dim, self._jacobian_dim))
+        null_space_task = np.eye(self._jacobian_dim)
 
-        # Compute null space projectors efficiently
-        for i, (J_frame_task, selected_indices) in enumerate(zip(J_frame_tasks, self._selected_joint_indices or [])):
-            # Create mask for selected joints
-            mask = np.zeros(J_frame_task.shape[1], dtype=bool)
+        # Create mask for all controlled joints
+        mask = np.zeros(self._jacobian_dim, dtype=bool)
+        for selected_indices in self._selected_joint_indices or []:
             mask[selected_indices] = True
 
+        # Compute null space projectors efficiently
+        for J_frame_task, selected_indices in zip(J_frame_tasks, self._selected_joint_indices or []):
             # Compute pseudoinverse and null space projector
             J_pinv = np.linalg.pinv(J_frame_task)
             null_space_full = np.eye(J_frame_task.shape[1]) - J_pinv @ J_frame_task
 
             # Only update rows corresponding to selected joints
-            null_space_task[mask, :] += null_space_full[mask, :]
+            null_space_task @= null_space_full
+
+        # Apply mask to the final result
+        null_space_task = null_space_task * mask[:, np.newaxis]
 
         return null_space_task

source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py

@@ -59,4 +59,6 @@ class PinkIKControllerCfg:
     """Show warning if IK solver fails to find a solution."""
 
     fail_on_joint_limit_violation: bool = True
-    """Fail the IK solver if a joint limit is violated."""
+    """If True, the Pink IK solver will fail and raise an error if any joint limit is violated during optimization. PinkIKController
+    will handle the error by setting the last joint positions. If False, the solver will ignore joint limit violations and return the
+    closest solution found."""

source/isaaclab/test/controllers/test_pink_ik.py

@@ -360,3 +360,4 @@ def print_debug_info(errors, test_counter):
     for hand in ["left", "right"]:
         print(f"Measured {hand} hand position error:", errors[f"{hand}_pos_error"])
         print(f"Measured {hand} hand rotation error:", errors[f"{hand}_rot_error"])
+        print(f"Measured {hand} hand PD error:", errors[f"{hand}_pd_error"])

source/isaaclab_assets/isaaclab_assets/robots/fourier.py

@@ -8,6 +8,7 @@
 The following configuration parameters are available:
 
 * :obj:`GR1T2_CFG`: The GR1T2 humanoid.
+* :obj:`GR1T2_PICK_PLACE_CFG`: The GR1T2 humanoid configured for pick-place manipulation tasks.
 
 Reference: https://www.fftai.com/products-gr1
 """
@@ -123,3 +124,54 @@
     },
 )
 """Configuration for the GR1T2 Humanoid robot."""
+
+
+GR1T2_PICK_PLACE_CFG = GR1T2_CFG.replace(
+    actuators={
+        "trunk": ImplicitActuatorCfg(
+            joint_names_expr=[
+                "waist_.*",
+            ],
+            effort_limit=None,
+            velocity_limit=None,
+            stiffness=4400,
+            damping=40.0,
+            armature=0.01,
+        ),
+        "right-arm": ImplicitActuatorCfg(
+            joint_names_expr=[
+                "right_shoulder_.*",
+                "right_elbow_.*",
+                "right_wrist_.*",
+            ],
+            stiffness=4400.0,
+            damping=40.0,
+            armature=0.01,
+        ),
+        "left-arm": ImplicitActuatorCfg(
+            joint_names_expr=[
+                "left_shoulder_.*",
+                "left_elbow_.*",
+                "left_wrist_.*",
+            ],
+            stiffness=4400.0,
+            damping=40.0,
+            armature=0.01,
+        ),
+        "right-hand": ImplicitActuatorCfg(
+            joint_names_expr=[
+                "R_.*",
+            ],
+            stiffness=None,
+            damping=None,
+        ),
+        "left-hand": ImplicitActuatorCfg(
+            joint_names_expr=[
+                "L_.*",
+            ],
+            stiffness=None,
+            damping=None,
+        ),
+    },
+)
+"""Configuration for the GR1T2 Humanoid robot configured for pick-place manipulation tasks."""

source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py

@@ -39,7 +39,7 @@ def target_eef_pose_to_action(
         target_eef_pose_dict: dict,
         gripper_action_dict: dict,
         action_noise_dict: dict | None = None,
-        env_id: int = 0,
+        env_id: int = 0,  # Unused, but required to conform to interface
     ) -> torch.Tensor:
         """
         Takes a target pose and gripper action for the end effector controller and returns an action
@@ -49,8 +49,8 @@ def target_eef_pose_to_action(
         Args:
             target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector.
             gripper_action_dict: Dictionary of gripper actions for each end-effector.
-            noise: Noise to add to the action. If None, no noise is added.
-            env_id: Environment index to get the action for.
+            action_noise_dict: Noise to add to the action. If None, no noise is added.
+            env_id: Environment index to get the action for. (Unused in this implementation, present for interface compatibility.)
 
         Returns:
             An action torch.Tensor that's compatible with env.step().

source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py

@@ -17,7 +17,7 @@ def __post_init__(self):
         super().__post_init__()
 
         # Override the existing values
-        self.datagen_config.name = "demo_src_gr1t2_demo_task_D0"
+        self.datagen_config.name = "gr1t2_pick_place_D0"
         self.datagen_config.generation_guarantee = True
         self.datagen_config.generation_keep_failed = False
         self.datagen_config.generation_num_trials = 1000

source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env.py

@@ -19,7 +19,7 @@ def __post_init__(self):
         super().__post_init__()
 
         # Override the existing values
-        self.datagen_config.name = "demo_src_gr1t2_demo_task_D0"
+        self.datagen_config.name = "gr1t2_pick_place_waist_enabled_D0"
         self.datagen_config.generation_guarantee = True
         self.datagen_config.generation_keep_failed = False
         self.datagen_config.generation_num_trials = 1000