Enhances Pink IK controller with null-space posture control and improv… (#3149)

# Description

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link:
https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html
-->

Enhance Pink IK Controller with Null Space Posture Control

This PR improves the Pink IK controller integration for better humanoid
robot control and more natural postures.

**Note**: Original this PR was staged in the internal repo (#547). It
has been moved here due to new Github workflow.


## Key Changes
### New Null Space Posture Task
- Added NullSpacePostureTask to enforce postural constraints on
shoulder/waist joints while prioritizing end-effector tasks
- Maintains natural robot poses during manipulation
### Controller Improvements
- Tuned low level PD controller gains
- Support mixed task types (FrameTask + NullSpacePostureTask)
### Testing & Environment Updates
- Redesigned pink controller test script to use JSON-based
configurations to program test motions.
- Updated all environments (PickPlace, NutPour, ExhaustPipe) with null
space control, damping tasks, and improved tracking
- Added `Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0` env that is
identical to `Isaac-PickPlace-GR1T2-Abs-v0` but enables the Waist DOFs.
- Added target_eef_link_names mapping for clearer link specification

Fixes # (issue)
These changes help fix the following problems from [VDR
feedback](https://docs.google.com/document/d/1saB1QA5r_WlD1l17q7C04WWNltnBW-K0ydI2UB8jxAs/edit?tab=t.0)
- [Enable Waist DOF](https://nvbugspro.nvidia.com/bug/5235527)
- Discourage elbow flare
- Make controller low-latency and low-jerk. **We improved the unit test
for the pink controller and reduced our position and rotation accuracy
tolerance from 30 mm, 10 degrees to 1 mm, 1 degree.**
- Develop metric for controller performance
- Added a flag to disable failure due to joint limits. Previously, any
commanded pose that ended in joint limit violation would result in no
solution and the controlled robot freezing in place. This change gets
the solver to still provide a solution and instead issue a warning for
joint limit violations.

## Screenshots

These controller changes have been tested through the Mimic pipeline
(teleop_se3_agent.py, record/replay_demos.py). Here are videos showing
teleoperation of all three environments working.

### PickPlace-GR1T2-Abs
![IK Improvements - Pick Place
Wheel](https://github.com/user-attachments/assets/98bd5a70-e5fc-4b5b-954a-848c8dbe85d4)

### NutPour-GR1T2
![IK Improvements -
NutPour](https://github.com/user-attachments/assets/b3603dd4-73cb-4ee7-9963-c68a32dffc60)


### ExhaustPipe-GR1T2
![IK Improvements -
ExhaustPipe](https://github.com/user-attachments/assets/28cd1a4b-29cc-402c-9ec4-7082b2c64d98)


### Successfully Trained Robomimic Model Rollout on PickPlace task
For the two robomimic tasks: `Isaac-PickPlace-GR1T2-Abs-v0` and
`Isaac-NutPour-GR1T2-Pink-IK-Abs-v0`, if we collect a new dataset, we
achieve a success rate of 96 and 92% respectively.
![IK Improvements - GR1T2 Waist Enabled Model Rollout
Trimmed](https://github.com/user-attachments/assets/d270e8a8-ed72-41f3-84ac-bdc2c02d190d)
![IK Improvements - GR1T2 Nut Pour Model Rollout
Trimmed](https://github.com/user-attachments/assets/1434721a-5dce-4b76-845a-6ac1379982f5)


## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->

---------
Co-authored-by: Kelly Guo <kellyg@nvidia.com>

CONTRIBUTORS.md

@@ -70,6 +70,7 @@ Guidelines for modifications:
 * HoJin Jeon
 * Hongwei Xiong
 * Hongyu Li
+* Huihua Zhao
 * Iretiayo Akinola
 * Jack Zeng
 * Jan Kerner
@@ -94,6 +95,7 @@ Guidelines for modifications:
 * Maurice Rahme
 * Michael Gussert
 * Michael Noseworthy
+* Michael Lin
 * Miguel Alonso Jr
 * Mingyu Lee
 * Muhong Guo
@@ -147,4 +149,5 @@ Guidelines for modifications:
 * Gavriel State
 * Hammad Mazhar
 * Marco Hutter
+* Yan Chang
 * Yashraj Narang

docs/source/api/lab/isaaclab.controllers.rst

@@ -11,6 +11,9 @@
     DifferentialIKControllerCfg
     OperationalSpaceController
     OperationalSpaceControllerCfg
+    PinkIKController
+    PinkIKControllerCfg
+    pink_ik.NullSpacePostureTask
 
 Differential Inverse Kinematics
 -------------------------------
@@ -39,3 +42,13 @@ Operational Space controllers
     :inherited-members:
     :show-inheritance:
     :exclude-members: __init__, class_type
+
+Differential Inverse Kinematics Controllers (Based on Pink)
+-----------------------------------------------------------
+
+For detailed documentation of Pink IK controllers and tasks, see:
+
+.. toctree::
+   :maxdepth: 1
+
+   isaaclab.controllers.pink_ik

docs/source/overview/environments.rst

@@ -132,6 +132,9 @@ for the lift-cube environment:
     +--------------------+-------------------------+-----------------------------------------------------------------------------+
     | |gr1_pick_place|   | |gr1_pick_place-link|   | Pick up and place an object in a basket with a GR-1 humanoid robot          |
     +--------------------+-------------------------+-----------------------------------------------------------------------------+
+    | |gr1_pp_waist|     | |gr1_pp_waist-link|     | Pick up and place an object in a basket with a GR-1 humanoid robot          |
+    |                    |                         | with waist degrees-of-freedom enables that provides a wider reach space.    |
+    +--------------------+-------------------------+-----------------------------------------------------------------------------+
 
 .. |reach-franka| image:: ../_static/tasks/manipulation/franka_reach.jpg
 .. |reach-ur10| image:: ../_static/tasks/manipulation/ur10_reach.jpg
@@ -141,6 +144,7 @@ for the lift-cube environment:
 .. |cube-shadow| image:: ../_static/tasks/manipulation/shadow_cube.jpg
 .. |stack-cube| image:: ../_static/tasks/manipulation/franka_stack.jpg
 .. |gr1_pick_place| image:: ../_static/tasks/manipulation/gr-1_pick_place.jpg
+.. |gr1_pp_waist| image:: ../_static/tasks/manipulation/gr-1_pick_place_waist.jpg
 
 .. |reach-franka-link| replace:: `Isaac-Reach-Franka-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/joint_pos_env_cfg.py>`__
 .. |reach-ur10-link| replace:: `Isaac-Reach-UR10-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/joint_pos_env_cfg.py>`__
@@ -154,6 +158,7 @@ for the lift-cube environment:
 .. |stack-cube-link| replace:: `Isaac-Stack-Cube-Franka-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_joint_pos_env_cfg.py>`__
 .. |stack-cube-bp-link| replace:: `Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_blueprint_env_cfg.py>`__
 .. |gr1_pick_place-link| replace:: `Isaac-PickPlace-GR1T2-Abs-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py>`__
+.. |gr1_pp_waist-link| replace:: `Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py>`__
 
 .. |cube-shadow-link| replace:: `Isaac-Repose-Cube-Shadow-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py>`__
 .. |cube-shadow-ff-link| replace:: `Isaac-Repose-Cube-Shadow-OpenAI-FF-Direct-v0 <https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_env_cfg.py>`__

docs/source/overview/teleop_imitation.rst

@@ -333,7 +333,7 @@ Collect human demonstrations
 
    Data collection for the GR-1 humanoid robot environment requires use of an Apple Vision Pro headset. If you do not have access to
    an Apple Vision Pro, you may skip this step and continue on to the next step: `Generate the dataset`_.
-   A pre-recorded annotated dataset is provided in the next step .
+   A pre-recorded annotated dataset is provided in the next step.
 
 .. tip::
    The GR1 scene utilizes the wrist poses from the Apple Vision Pro (AVP) as setpoints for a differential IK controller (Pink-IK).
@@ -380,6 +380,9 @@ Collect five demonstrations by running the following command:
    --dataset_file ./datasets/dataset_gr1.hdf5 \
    --num_demos 5 --enable_pinocchio
 
+.. note::
+   We also provide a GR-1 pick and place task with waist degrees-of-freedom enabled ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0`` (see :ref:`environments` for details on the available environments, including the GR1 Waist Enabled variant). The same command above applies but with the task name changed to ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0``.
+
 .. tip::
    If a demo fails during data collection, the environment can be reset using the teleoperation controls panel in the XR teleop client
    on the Apple Vision Pro or via voice control by saying "reset". See :ref:`teleoperate-apple-vision-pro` for more details.

source/isaaclab/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.45.7"
+version = "0.45.8"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/isaaclab/docs/CHANGELOG.rst

 Changelog
 ---------
 
+0.45.8 (2025-07-25)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Created :attr:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg.target_eef_link_names` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg`
+  to specify the target end-effector link names for the pink inverse kinematics controller.
+
+Changed
+^^^^^^^
+
+* Updated pink inverse kinematics controller configuration for the following tasks (Isaac-PickPlace-GR1T2, Isaac-NutPour-GR1T2, Isaac-ExhaustPipe-GR1T2)
+  to increase end-effector tracking accuracy and speed. Also added a null-space regularizer that enables turning on of waist degrees-of-freedom.
+* Improved the test_pink_ik script to more comprehensive test on controller accuracy. Also, migrated to use pytest. With the current IK controller
+  improvements, our unit tests pass position and orientation accuracy test within **(1 mm, 1 degree)**. Previously, the position accuracy tolerances
+  were set to **(30 mm, 10 degrees)**.
+* Included a new config parameter :attr:`fail_on_ik_error` to :class:`~isaaclab.controllers.pink_ik.PinkIKControllerCfg`
+  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.7 (2025-08-21)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab/isaaclab/controllers/__init__.py

@@ -15,3 +15,4 @@ from .differential_ik import DifferentialIKController
 from .differential_ik_cfg import DifferentialIKControllerCfg
 from .operational_space import OperationalSpaceController
 from .operational_space_cfg import OperationalSpaceControllerCfg
+from .pink_ik import NullSpacePostureTask, PinkIKController, PinkIKControllerCfg

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

+# 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

+# 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
+
+import numpy as np
+
+import pinocchio as pin
+from pink.configuration import Configuration
+from pink.tasks import Task
+
+
+class NullSpacePostureTask(Task):
+    r"""Pink-based task that adds a posture objective that is in the null space projection of other tasks.
+
+    This task implements posture control in the null space of higher priority tasks
+    (typically end-effector pose tasks) within the Pink inverse kinematics framework.
+
+    **Mathematical Formulation:**
+
+    For details on Pink Inverse Kinematics optimization formulation visit: https://github.com/stephane-caron/pink
+
+    **Null Space Posture Task Implementation:**
+
+    This task consists of two components:
+
+    1. **Error Function**: The posture error is computed as:
+
+    .. math::
+
+        \mathbf{e}(\mathbf{q}) = \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q})
+
+    where:
+        - :math:`\mathbf{q}^*` is the target joint configuration
+        - :math:`\mathbf{q}` is the current joint configuration
+        - :math:`\mathbf{M}` is a joint selection mask matrix
+
+    2. **Jacobian Matrix**: The task Jacobian is the null space projector:
+
+    .. math::
+
+        \mathbf{J}_{\text{posture}}(\mathbf{q}) = \mathbf{N}(\mathbf{q}) = \mathbf{I} - \mathbf{J}_{\text{primary}}^+ \mathbf{J}_{\text{primary}}
+
+    where:
+        - :math:`\mathbf{J}_{\text{primary}}` is the combined Jacobian of all higher priority tasks
+        - :math:`\mathbf{J}_{\text{primary}}^+` is the pseudoinverse of the primary task Jacobian
+        - :math:`\mathbf{N}(\mathbf{q})` is the null space projector matrix
+
+    For example, if there are two frame tasks (e.g., controlling the pose of two end-effectors), the combined Jacobian
+    :math:`\mathbf{J}_{\text{primary}}` is constructed by stacking the individual Jacobians for each frame vertically:
+
+    .. math::
+
+        \mathbf{J}_{\text{primary}} =
+        \begin{bmatrix}
+            \mathbf{J}_1(\mathbf{q}) \\
+            \mathbf{J}_2(\mathbf{q})
+        \end{bmatrix}
+
+    where :math:`\mathbf{J}_1(\mathbf{q})` and :math:`\mathbf{J}_2(\mathbf{q})` are the Jacobians for the first and second frame tasks, respectively.
+
+    The null space projector ensures that joint velocities in the null space produce zero velocity
+    for the primary tasks: :math:`\mathbf{J}_{\text{primary}} \cdot \dot{\mathbf{q}}_{\text{null}} = \mathbf{0}`.
+
+    **Task Integration:**
+
+    When integrated into the Pink framework, this task contributes to the optimization as:
+
+    .. math::
+
+        \left\| \mathbf{N}(\mathbf{q}) \mathbf{v} + \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q}) \right\|_{W_{\text{posture}}}^2
+
+    This formulation allows the robot to maintain a desired posture while respecting the constraints
+    imposed by higher priority tasks (e.g., end-effector positioning).
+
+    """
+
+    def __init__(
+        self,
+        cost: float,
+        lm_damping: float = 0.0,
+        gain: float = 1.0,
+        controlled_frames: list[str] | None = None,
+        controlled_joints: list[str] | None = None,
+    ) -> None:
+        r"""Initialize the null space posture task.
+
+        This task maintains a desired joint posture in the null space of higher-priority
+        frame tasks. Joint selection allows excluding specific joints (e.g., wrist joints
+        in humanoid manipulation) to prevent large rotational ranges from overwhelming
+        errors in critical joints like shoulders and waist.
+
+        Args:
+            cost: Task weighting factor in the optimization objective.
+                Units: :math:`[\text{cost}] / [\text{rad}]`.
+            lm_damping: Levenberg-Marquardt regularization scale (unitless). Defaults to 0.0.
+            gain: Task gain :math:`\alpha \in [0, 1]` for low-pass filtering.
+                Defaults to 1.0 (no filtering).
+            controlled_frames: Frame names whose Jacobians define the primary tasks for
+                null space projection. If None or empty, no projection is applied.
+            controlled_joints: Joint names to control in the posture task. If None or
+                empty, all actuated joints are controlled.
+        """
+        super().__init__(cost=cost, gain=gain, lm_damping=lm_damping)
+        self.target_q: np.ndarray | None = None
+        self.controlled_frames: list[str] = controlled_frames or []
+        self.controlled_joints: list[str] = controlled_joints or []
+        self._joint_mask: np.ndarray | None = None
+        self._frame_names: list[str] | None = None
+
+    def __repr__(self) -> str:
+        """Human-readable representation of the task."""
+        return (
+            f"NullSpacePostureTask(cost={self.cost}, gain={self.gain}, lm_damping={self.lm_damping},"
+            f" controlled_frames={self.controlled_frames}, controlled_joints={self.controlled_joints})"
+        )
+
+    def _build_joint_mapping(self, configuration: Configuration) -> None:
+        """Build joint mask and cache frequently used values.
+
+        Creates a binary mask that selects which joints should be controlled
+        in the posture task.
+
+        Args:
+            configuration: Robot configuration containing the model and joint information.
+        """
+        # Create joint mask for full configuration size
+        self._joint_mask = np.zeros(configuration.model.nq)
+
+        # Create dictionary for joint names to indices (exclude root joint)
+        joint_names = configuration.model.names.tolist()[1:]
+
+        # Build joint mask efficiently
+        for i, joint_name in enumerate(joint_names):
+            if joint_name in self.controlled_joints:
+                self._joint_mask[i] = 1.0
+
+        # Cache frame names for performance
+        self._frame_names = list(self.controlled_frames)
+
+    def set_target(self, target_q: np.ndarray) -> None:
+        """Set target posture configuration.
+
+        Args:
+            target_q: Target vector in the configuration space. If the model
+                has a floating base, then this vector should include
+                floating-base coordinates (although they have no effect on the
+                posture task since only actuated joints are controlled).
+        """
+        self.target_q = target_q.copy()
+
+    def set_target_from_configuration(self, configuration: Configuration) -> None:
+        """Set target posture from a robot configuration.
+
+        Args:
+            configuration: Robot configuration whose joint angles will be used
+                as the target posture.
+        """
+        self.set_target(configuration.q)
+
+    def compute_error(self, configuration: Configuration) -> np.ndarray:
+        r"""Compute posture task error.
+
+        The error computation follows:
+
+        .. math::
+
+            \mathbf{e}(\mathbf{q}) = \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q})
+
+        where :math:`\mathbf{M}` is the joint selection mask and :math:`\mathbf{q}^* - \mathbf{q}`
+        is computed using Pinocchio's difference function to handle joint angle wrapping.
+
+        Args:
+            configuration: Robot configuration :math:`\mathbf{q}`.
+
+        Returns:
+            Posture task error :math:`\mathbf{e}(\mathbf{q})` with the same dimension
+            as the configuration vector, but with zeros for non-controlled joints.
+
+        Raises:
+            ValueError: If no posture target has been set.
+        """
+        if self.target_q is None:
+            raise ValueError("No posture target has been set. Call set_target() first.")
+
+        # Initialize joint mapping if needed
+        if self._joint_mask is None:
+            self._build_joint_mapping(configuration)
+
+        # Compute configuration difference using Pinocchio's difference function
+        # This handles joint angle wrapping correctly
+        err = pin.difference(
+            configuration.model,
+            self.target_q,
+            configuration.q,
+        )
+
+        # Apply pre-computed joint mask to select only controlled joints
+        return self._joint_mask * err
+
+    def compute_jacobian(self, configuration: Configuration) -> np.ndarray:
+        r"""Compute the null space projector Jacobian.
+
+        The null space projector is defined as:
+
+        .. math::
+
+            \mathbf{N}(\mathbf{q}) = \mathbf{I} - \mathbf{J}_{\text{primary}}^+ \mathbf{J}_{\text{primary}}
+
+        where:
+            - :math:`\mathbf{J}_{\text{primary}}` is the combined Jacobian of all controlled frames
+            - :math:`\mathbf{J}_{\text{primary}}^+` is the pseudoinverse of the primary task Jacobian
+            - :math:`\mathbf{I}` is the identity matrix
+
+        The null space projector ensures that joint velocities in the null space produce
+        zero velocity for the primary tasks: :math:`\mathbf{J}_{\text{primary}} \cdot \dot{\mathbf{q}}_{\text{null}} = \mathbf{0}`.
+
+        If no controlled frames are specified, returns the identity matrix.
+
+        Args:
+            configuration: Robot configuration :math:`\mathbf{q}`.
+
+        Returns:
+            Null space projector matrix :math:`\mathbf{N}(\mathbf{q})` with dimensions
+            :math:`n_q \times n_q` where :math:`n_q` is the number of configuration variables.
+        """
+        # Initialize joint mapping if needed
+        if self._frame_names is None:
+            self._build_joint_mapping(configuration)
+
+        # If no frame tasks are defined, return identity matrix (no null space projection)
+        if not self._frame_names:
+            return np.eye(configuration.model.nq)
+
+        # Get Jacobians for all frame tasks and combine them
+        J_frame_tasks = [configuration.get_frame_jacobian(frame_name) for frame_name in self._frame_names]
+        J_combined = np.concatenate(J_frame_tasks, axis=0)
+
+        # Compute null space projector: N = I - J^+ * J
+        N_combined = np.eye(J_combined.shape[1]) - np.linalg.pinv(J_combined) @ J_combined
+
+        return N_combined

source/isaaclab/isaaclab/controllers/pink_ik.py → source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py

@@ -7,30 +7,58 @@
 
 This module provides integration between Pink inverse kinematics solver and IsaacLab.
 Pink is a differentiable inverse kinematics solver framework that provides task-space control capabilities.
+
+Reference:
+    Pink IK Solver: https://github.com/stephane-caron/pink
 """
 
+from __future__ import annotations
+
 import numpy as np
 import torch
+from typing import TYPE_CHECKING
 
 from pink import solve_ik
 from pink.configuration import Configuration
+from pink.tasks import FrameTask
 from pinocchio.robot_wrapper import RobotWrapper
 
-from .pink_ik_cfg import PinkIKControllerCfg
+from isaaclab.assets import ArticulationCfg
+from isaaclab.utils.string import resolve_matching_names_values
+
+from .null_space_posture_task import NullSpacePostureTask
+
+if TYPE_CHECKING:
+    from .pink_ik_cfg import PinkIKControllerCfg
 
 
 class PinkIKController:
     """Integration of Pink IK controller with Isaac Lab.
 
-    The Pink IK controller is available at: https://github.com/stephane-caron/pink
+    The Pink IK controller solves differential inverse kinematics through weighted tasks. Each task is defined
+    by a residual function e(q) that is driven to zero (e.g., e(q) = p_target - p_ee(q) for end-effector positioning).
+    The controller computes joint velocities v satisfying J_e(q)v = -αe(q), where J_e(q) is the task Jacobian.
+    Multiple tasks are resolved through weighted optimization, formulating a quadratic program that minimizes
+    weighted task errors while respecting joint velocity limits.
+
+    It supports user defined tasks, and we have provided a NullSpacePostureTask for maintaining desired joint configurations.
+
+    Reference:
+        Pink IK Solver: https://github.com/stephane-caron/pink
     """
 
-    def __init__(self, cfg: PinkIKControllerCfg, device: str):
+    def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str):
         """Initialize the Pink IK Controller.
 
         Args:
-            cfg: The configuration for the controller.
-            device: The device to use for computations (e.g., 'cuda:0').
+            cfg: The configuration for the Pink IK controller containing task definitions, solver parameters,
+                and joint configurations.
+            robot_cfg: The robot articulation configuration containing initial joint positions and robot
+                specifications.
+            device: The device to use for computations (e.g., 'cuda:0', 'cpu').
+
+        Raises:
+            KeyError: When Pink joint names cannot be matched to robot configuration joint positions.
         """
         # Initialize the robot model from URDF and mesh files
         self.robot_wrapper = RobotWrapper.BuildFromURDF(cfg.urdf_path, cfg.mesh_path, root_joint=None)
@@ -38,47 +66,69 @@ class PinkIKController:
             self.robot_wrapper.model, self.robot_wrapper.data, self.robot_wrapper.q0
         )
 
+        # Find the initial joint positions by matching Pink's joint names to robot_cfg.init_state.joint_pos,
+        # where the joint_pos keys may be regex patterns and the values are the initial positions.
+        # We want to assign to each Pink joint name the value from the first matching regex key in joint_pos.
+        pink_joint_names = self.pink_configuration.model.names.tolist()[1:]
+        joint_pos_dict = robot_cfg.init_state.joint_pos
+
+        # Use resolve_matching_names_values to match Pink joint names to joint_pos values
+        indices, names, values = resolve_matching_names_values(
+            joint_pos_dict, pink_joint_names, preserve_order=False, strict=False
+        )
+        if len(indices) != len(pink_joint_names):
+            unmatched = [name for name in pink_joint_names if name not in names]
+            raise KeyError(
+                "Could not find a match for all Pink joint names in robot_cfg.init_state.joint_pos. "
+                f"Unmatched: {unmatched}, Expected: {pink_joint_names}"
+            )
+        self.init_joint_positions = np.array(values)
+
         # Set the default targets for each task from the configuration
         for task in cfg.variable_input_tasks:
+            # If task is a NullSpacePostureTask, set the target to the initial joint positions
+            if isinstance(task, NullSpacePostureTask):
+                task.set_target(self.init_joint_positions)
+                continue
             task.set_target_from_configuration(self.pink_configuration)
         for task in cfg.fixed_input_tasks:
             task.set_target_from_configuration(self.pink_configuration)
 
         # Map joint names from Isaac Lab to Pink's joint conventions
-        pink_joint_names = self.robot_wrapper.model.names.tolist()[1:]  # Skip the root and universal joints
-        isaac_lab_joint_names = cfg.joint_names
+        self.pink_joint_names = self.robot_wrapper.model.names.tolist()[1:]  # Skip the root and universal joints
+        self.isaac_lab_joint_names = cfg.joint_names
+        assert cfg.joint_names is not None, "cfg.joint_names cannot be None"
+
+        # Frame task link names
+        self.frame_task_link_names = []
+        for task in cfg.variable_input_tasks:
+            if isinstance(task, FrameTask):
+                self.frame_task_link_names.append(task.frame)
 
         # Create reordering arrays for joint indices
-        self.isaac_lab_to_pink_ordering = [isaac_lab_joint_names.index(pink_joint) for pink_joint in pink_joint_names]
-        self.pink_to_isaac_lab_ordering = [
-            pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_joint_names
-        ]
+        self.isaac_lab_to_pink_ordering = np.array(
+            [self.isaac_lab_joint_names.index(pink_joint) for pink_joint in self.pink_joint_names]
+        )
+        self.pink_to_isaac_lab_ordering = np.array(
+            [self.pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in self.isaac_lab_joint_names]
+        )
 
         self.cfg = cfg
         self.device = device
 
-    """
-    Operations.
-    """
+    def update_null_space_joint_targets(self, curr_joint_pos: np.ndarray):
+        """Update the null space joint targets.
 
-    def reorder_array(self, input_array: list[float], reordering_array: list[int]) -> list[float]:
-        """Reorder the input array based on the provided ordering.
+        This method updates the target joint positions for null space posture tasks based on the current
+        joint configuration. This is useful for maintaining desired joint configurations when the primary
+        task allows redundancy.
 
         Args:
-            input_array: The array to reorder.
-            reordering_array: The indices to use for reordering.
-
-        Returns:
-            Reordered array.
+            curr_joint_pos: The current joint positions of shape (num_joints,).
         """
-        return [input_array[i] for i in reordering_array]
-
-    def initialize(self):
-        """Initialize the internals of the controller.
-
-        This method is called during setup but before the first compute call.
-        """
-        pass
+        for task in self.cfg.variable_input_tasks:
+            if isinstance(task, NullSpacePostureTask):
+                task.set_target(curr_joint_pos)
 
     def compute(
         self,
@@ -87,15 +137,20 @@ class PinkIKController:
     ) -> torch.Tensor:
         """Compute the target joint positions based on current state and tasks.
 
+        Performs inverse kinematics using the Pink solver to compute target joint positions that satisfy
+        the defined tasks. The solver uses quadratic programming to find optimal joint velocities that
+        minimize task errors while respecting constraints.
+
         Args:
-            curr_joint_pos: The current joint positions.
-            dt: The time step for computing joint position changes.
+            curr_joint_pos: The current joint positions of shape (num_joints,).
+            dt: The time step for computing joint position changes in seconds.
 
         Returns:
-            The target joint positions as a tensor.
+            The target joint positions as a tensor of shape (num_joints,) on the specified device.
+            If the IK solver fails, returns the current joint positions unchanged to maintain stability.
         """
-        # Initialize joint positions for Pink, including the root and universal joints
-        joint_positions_pink = np.array(self.reorder_array(curr_joint_pos, self.isaac_lab_to_pink_ordering))
+        # Initialize joint positions for Pink, change from isaac_lab to pink/pinocchio joint ordering.
+        joint_positions_pink = curr_joint_pos[self.isaac_lab_to_pink_ordering]
 
         # Update Pink's robot configuration with the current joint positions
         self.pink_configuration.update(joint_positions_pink)
@@ -103,15 +158,20 @@ class PinkIKController:
         # pink.solve_ik can raise an exception if the solver fails
         try:
             velocity = solve_ik(
-                self.pink_configuration, self.cfg.variable_input_tasks + self.cfg.fixed_input_tasks, dt, solver="osqp"
+                self.pink_configuration,
+                self.cfg.variable_input_tasks + self.cfg.fixed_input_tasks,
+                dt,
+                solver="osqp",
+                safety_break=self.cfg.fail_on_joint_limit_violation,
             )
             Delta_q = velocity * dt
-        except (AssertionError, Exception):
+        except (AssertionError, Exception) as e:
             # Print warning and return the current joint positions as the target
             # Not using omni.log since its not available in CI during docs build
             if self.cfg.show_ik_warnings:
                 print(
-                    "Warning: IK quadratic solver could not find a solution! Did not update the target joint positions."
+                    "Warning: IK quadratic solver could not find a solution! Did not update the target joint"
+                    f" positions.\nError: {e}"
                 )
             return torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32)
 
@@ -120,7 +180,7 @@ class PinkIKController:
 
         # Reorder the joint angle changes back to Isaac Lab conventions
         joint_vel_isaac_lab = torch.tensor(
-            self.reorder_array(pink_joint_angle_changes, self.pink_to_isaac_lab_ordering),
+            pink_joint_angle_changes[self.pink_to_isaac_lab_ordering],
             device=self.device,
             dtype=torch.float,
         )

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

@@ -57,3 +57,8 @@ class PinkIKControllerCfg:
 
     show_ik_warnings: bool = True
     """Show warning if IK solver fails to find a solution."""
+
+    fail_on_joint_limit_violation: bool = True
+    """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/isaaclab/envs/mdp/actions/pink_actions_cfg.py

@@ -5,7 +5,7 @@
 
 from dataclasses import MISSING
 
-from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg
+from isaaclab.controllers.pink_ik import PinkIKControllerCfg
 from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg
 from isaaclab.utils import configclass
 
@@ -34,3 +34,10 @@ class PinkInverseKinematicsActionCfg(ActionTermCfg):
 
     controller: PinkIKControllerCfg = MISSING
     """Configuration for the Pink IK controller that will be used to solve the inverse kinematics."""
+
+    target_eef_link_names: dict[str, str] = MISSING
+    """Dictionary mapping task names to controlled link names for the Pink IK controller.
+
+    This dictionary should map the task names (e.g., 'left_wrist', 'right_wrist') to the
+    corresponding link names in the URDF that will be controlled by the IK solver.
+    """

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

@@ -10,6 +10,8 @@ import torch
 from collections.abc import Sequence
 from typing import TYPE_CHECKING
 
+from pink.tasks import FrameTask
+
 import isaaclab.utils.math as math_utils
 from isaaclab.assets.articulation import Articulation
 from isaaclab.controllers.pink_ik import PinkIKController
@@ -57,7 +59,9 @@ class PinkInverseKinematicsAction(ActionTerm):
         assert env.num_envs > 0, "Number of environments specified are less than 1."
         self._ik_controllers = []
         for _ in range(env.num_envs):
-            self._ik_controllers.append(PinkIKController(cfg=copy.deepcopy(self.cfg.controller), device=self.device))
+            self._ik_controllers.append(
+                PinkIKController(cfg=self.cfg.controller.copy(), robot_cfg=env.scene.cfg.robot, device=self.device)
+            )
 
         # Create tensors to store raw and processed actions
         self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device)
@@ -113,8 +117,11 @@ class PinkInverseKinematicsAction(ActionTerm):
     @property
     def action_dim(self) -> int:
         """Dimension of the action space (based on number of tasks and pose dimension)."""
-        # The tasks for all the controllers are the same, hence just using the first one to calculate the action_dim
-        return len(self._ik_controllers[0].cfg.variable_input_tasks) * self.pose_dim + self.hand_joint_dim
+        # Count only FrameTask instances in variable_input_tasks
+        frame_tasks_count = sum(
+            1 for task in self._ik_controllers[0].cfg.variable_input_tasks if isinstance(task, FrameTask)
+        )
+        return frame_tasks_count * self.pose_dim + self.hand_joint_dim
 
     @property
     def raw_actions(self) -> torch.Tensor:
@@ -163,7 +170,6 @@ class PinkInverseKinematicsAction(ActionTerm):
         """
         # Store the raw actions
         self._raw_actions[:] = actions
-        self._processed_actions[:] = self.raw_actions
 
         # Make a copy of actions before modifying so that raw actions are not modified
         actions_clone = actions.clone()
@@ -204,10 +210,11 @@ class PinkInverseKinematicsAction(ActionTerm):
         # Loop through each task and set the target
         for env_index, ik_controller in enumerate(self._ik_controllers):
             for task_index, task in enumerate(ik_controller.cfg.variable_input_tasks):
-                target = task.transform_target_to_world
-                target.translation = controlled_frame_in_base_link_frame_pos[task_index, env_index, :].cpu().numpy()
-                target.rotation = controlled_frame_in_base_link_frame_mat[task_index, env_index, :].cpu().numpy()
-                task.set_target(target)
+                if isinstance(task, FrameTask):
+                    target = task.transform_target_to_world
+                    target.translation = controlled_frame_in_base_link_frame_pos[task_index, env_index, :].cpu().numpy()
+                    target.rotation = controlled_frame_in_base_link_frame_mat[task_index, env_index, :].cpu().numpy()
+                    task.set_target(target)
 
     def apply_actions(self):
         # start_time = time.time()  # Capture the time before the step

source/isaaclab/isaaclab/utils/string.py

@@ -272,7 +272,10 @@ def resolve_matching_names(
 
 
 def resolve_matching_names_values(
-    data: dict[str, Any], list_of_strings: Sequence[str], preserve_order: bool = False
+    data: dict[str, Any],
+    list_of_strings: Sequence[str],
+    preserve_order: bool = False,
+    strict: bool = True,
 ) -> tuple[list[int], list[str], list[Any]]:
     """Match a list of regular expressions in a dictionary against a list of strings and return
     the matched indices, names, and values.
@@ -293,6 +296,7 @@ def resolve_matching_names_values(
         data: A dictionary of regular expressions and values to match the strings in the list.
         list_of_strings: A list of strings to match.
         preserve_order: Whether to preserve the order of the query keys in the returned values. Defaults to False.
+        strict: Whether to require that all keys in the dictionary get matched. Defaults to True.
 
     Returns:
         A tuple of lists containing the matched indices, names, and values.
@@ -300,7 +304,7 @@ def resolve_matching_names_values(
     Raises:
         TypeError: When the input argument :attr:`data` is not a dictionary.
         ValueError: When multiple matches are found for a string in the dictionary.
-        ValueError: When not all regular expressions in the data keys are matched.
+        ValueError: When not all regular expressions in the data keys are matched (if strict is True).
     """
     # check valid input
     if not isinstance(data, dict):
@@ -354,7 +358,7 @@ def resolve_matching_names_values(
         names_list = names_list_reorder
         values_list = values_list_reorder
     # check that all regular expressions are matched
-    if not all(keys_match_found):
+    if strict and not all(keys_match_found):
         # make this print nicely aligned for debugging
         msg = "\n"
         for key, value in zip(data.keys(), keys_match_found):

source/isaaclab/test/controllers/simplified_test_robot.urdf

+<?xml version="1.0" encoding="UTF-8"?>
+<robot name="simplified_robot">
+  <!-- Links -->
+  <link name="base_link"/>
+  <link name="waist_yaw_link"/>
+  <link name="waist_pitch_link"/>
+  <link name="waist_roll_link"/>
+  <link name="left_upper_arm_pitch_link"/>
+  <link name="left_upper_arm_roll_link"/>
+  <link name="left_upper_arm_yaw_link"/>
+  <link name="left_lower_arm_pitch_link"/>
+  <link name="left_hand_yaw_link"/>
+  <link name="left_hand_roll_link"/>
+  <link name="left_hand_pitch_link"/>
+  <link name="right_upper_arm_pitch_link"/>
+  <link name="right_upper_arm_roll_link"/>
+  <link name="right_upper_arm_yaw_link"/>
+  <link name="right_lower_arm_pitch_link"/>
+  <link name="right_hand_yaw_link"/>
+  <link name="right_hand_roll_link"/>
+  <link name="right_hand_pitch_link"/>
+  <link name="head_roll_link"/>
+  <link name="head_pitch_link"/>
+  <link name="head_yaw_link"/>
+
+  <!-- Joints -->
+  <!-- Waist joints -->
+  <joint name="waist_yaw_joint" type="revolute">
+    <origin xyz="0. 0. 0." rpy="0. 0. 0."/>
+    <parent link="base_link"/>
+    <child link="waist_yaw_link"/>
+    <axis xyz="0. 0. 1."/>
+    <limit lower="-1.05" upper="1.05" effort="50." velocity="16.76"/>
+  </joint>
+
+  <joint name="waist_pitch_joint" type="revolute">
+    <origin xyz="0. 0. 0.0869" rpy="0. 0. 0."/>
+    <parent link="waist_yaw_link"/>
+    <child link="waist_pitch_link"/>
+    <axis xyz="0. 1. 0."/>
+    <limit lower="-0.52" upper="1.22" effort="50." velocity="16.76"/>
+  </joint>
+
+  <joint name="waist_roll_joint" type="revolute">
+    <origin xyz="0.00525 -0.0005 0.081" rpy="0. 0. 0."/>
+    <parent link="waist_pitch_link"/>
+    <child link="waist_roll_link"/>
+    <axis xyz="1. 0. 0."/>
+    <limit lower="-0.7" upper="0.7" effort="50." velocity="16.76"/>
+  </joint>
+
+  <!-- Left arm -->
+  <joint name="left_shoulder_pitch_joint" type="revolute">
+    <origin xyz="-0.00225 0.1018544 0.207001" rpy="0.43275 0. 0."/>
+    <parent link="waist_roll_link"/>
+    <child link="left_upper_arm_pitch_link"/>
+    <axis xyz="0. 1. 0."/>
+    <limit lower="-2.79" upper="1.92" effort="38." velocity="9.11"/>
+  </joint>
+
+  <joint name="left_shoulder_roll_joint" type="revolute">
+    <origin xyz="0. 0.09515 0." rpy="-0.4327501 0. 0."/>
+    <parent link="left_upper_arm_pitch_link"/>
+    <child link="left_upper_arm_roll_link"/>
+    <axis xyz="1. 0. 0."/>
+    <limit lower="-0.57" upper="2.53" effort="38." velocity="9.11"/>
+  </joint>
+
+  <joint name="left_shoulder_yaw_joint" type="revolute">
+    <origin xyz="0. 0.04 -0.0709" rpy="0.0000001 0. 0."/>
+    <parent link="left_upper_arm_roll_link"/>
+    <child link="left_upper_arm_yaw_link"/>
+    <axis xyz="0. 0. 1."/>
+    <limit lower="-2.97" upper="2.97" effort="30." velocity="7.33"/>
+  </joint>
+
+  <joint name="left_elbow_pitch_joint" type="revolute">
+    <origin xyz="0. 0.0010503 -0.1786" rpy="0. 0. 0."/>
+    <parent link="left_upper_arm_yaw_link"/>
+    <child link="left_lower_arm_pitch_link"/>
+    <axis xyz="0. 1. 0."/>
+    <limit lower="-2.27" upper="2.27" effort="30." velocity="7.33"/>
+  </joint>
+
+  <joint name="left_wrist_yaw_joint" type="revolute">
+    <origin xyz="0. 0. -0.05945" rpy="0. 0. 0."/>
+    <parent link="left_lower_arm_pitch_link"/>
+    <child link="left_hand_yaw_link"/>
+    <axis xyz="0. 0. 1."/>
+    <limit lower="-2.97" upper="2.97" effort="10.2" velocity="24.4"/>
+  </joint>
+
+  <joint name="left_wrist_roll_joint" type="revolute">
+    <origin xyz="0. 0. -0.19115" rpy="0. 0. 0."/>
+    <parent link="left_hand_yaw_link"/>
+    <child link="left_hand_roll_link"/>
+    <axis xyz="1. 0. 0."/>
+    <limit lower="-0.87" upper="0.96" effort="3.95" velocity="27.96"/>
+  </joint>
+
+  <joint name="left_wrist_pitch_joint" type="revolute">
+    <origin xyz="0. 0. 0." rpy="0. 0. 0."/>
+    <parent link="left_hand_roll_link"/>
+    <child link="left_hand_pitch_link"/>
+    <axis xyz="0. 1. 0."/>
+    <limit lower="-0.61" upper="0.61" effort="3.95" velocity="27.96"/>
+  </joint>
+
+  <!-- Right arm -->
+  <joint name="right_shoulder_pitch_joint" type="revolute">
+    <origin xyz="-0.00225 -0.1007056 0.207001" rpy="-0.43275 0. 0."/>
+    <parent link="waist_roll_link"/>
+    <child link="right_upper_arm_pitch_link"/>
+    <axis xyz="0. 1. 0."/>
+    <limit lower="-2.79" upper="1.92" effort="38." velocity="9.11"/>
+  </joint>
+
+  <joint name="right_shoulder_roll_joint" type="revolute">
+    <origin xyz="0. -0.09515 0." rpy="0.4327501 0. 0."/>
+    <parent link="right_upper_arm_pitch_link"/>
+    <child link="right_upper_arm_roll_link"/>
+    <axis xyz="1. 0. 0."/>
+    <limit lower="-2.53" upper="0.57" effort="38." velocity="9.11"/>
+  </joint>
+
+  <joint name="right_shoulder_yaw_joint" type="revolute">
+    <origin xyz="0. -0.04 -0.0709" rpy="-0.0000001 0. 0."/>
+    <parent link="right_upper_arm_roll_link"/>
+    <child link="right_upper_arm_yaw_link"/>
+    <axis xyz="0. 0. 1."/>
+    <limit lower="-2.97" upper="2.97" effort="30." velocity="7.33"/>
+  </joint>
+
+  <joint name="right_elbow_pitch_joint" type="revolute">
+    <origin xyz="0. -0.0010503 -0.1786" rpy="0. 0. 0."/>
+    <parent link="right_upper_arm_yaw_link"/>
+    <child link="right_lower_arm_pitch_link"/>
+    <axis xyz="0. 1. 0."/>
+    <limit lower="-2.27" upper="2.27" effort="30." velocity="7.33"/>
+  </joint>
+
+  <joint name="right_wrist_yaw_joint" type="revolute">
+    <origin xyz="0. 0. -0.05945" rpy="0. 0. 0."/>
+    <parent link="right_lower_arm_pitch_link"/>
+    <child link="right_hand_yaw_link"/>
+    <axis xyz="0. 0. 1."/>
+    <limit lower="-2.97" upper="2.97" effort="10.2" velocity="24.4"/>
+  </joint>
+
+  <joint name="right_wrist_roll_joint" type="revolute">
+    <origin xyz="0. 0. -0.19115" rpy="0. 0. 0."/>
+    <parent link="right_hand_yaw_link"/>
+    <child link="right_hand_roll_link"/>
+    <axis xyz="1. 0. 0."/>
+    <limit lower="-0.96" upper="0.87" effort="3.95" velocity="27.96"/>
+  </joint>
+
+  <joint name="right_wrist_pitch_joint" type="revolute">
+    <origin xyz="0. 0. 0." rpy="0. 0. 0."/>
+    <parent link="right_hand_roll_link"/>
+    <child link="right_hand_pitch_link"/>
+    <axis xyz="0. 1. 0."/>
+    <limit lower="-0.61" upper="0.61" effort="3.95" velocity="27.96"/>
+  </joint>
+
+  <!-- Head -->
+  <joint name="head_roll_joint" type="revolute">
+    <origin xyz="-0.00025 0.0010928 0.305" rpy="0. 0. 0."/>
+    <parent link="waist_roll_link"/>
+    <child link="head_roll_link"/>
+    <axis xyz="1. 0. 0."/>
+    <limit lower="-0.35" upper="0.35" effort="25." velocity="27.96"/>
+  </joint>
+
+  <joint name="head_pitch_joint" type="revolute">
+    <origin xyz="0. 0. 0." rpy="0. 0. 0."/>
+    <parent link="head_roll_link"/>
+    <child link="head_pitch_link"/>
+    <axis xyz="0. 1. 0."/>
+    <limit lower="-0.87" upper="0.87" effort="25." velocity="27.96"/>
+  </joint>
+
+  <joint name="head_yaw_joint" type="revolute">
+    <origin xyz="0. 0. 0.0513" rpy="0. 0. 0."/>
+    <parent link="head_pitch_link"/>
+    <child link="head_yaw_link"/>
+    <axis xyz="0. 0. 1."/>
+    <limit lower="-2.71" upper="2.71" effort="25." velocity="27.96"/>
+  </joint>
+
+</robot>

source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json

+{
+    "tolerances": {
+        "position": 0.001,
+        "pd_position": 0.001,
+        "rotation": 0.02,
+        "check_errors": true
+    },
+    "tests": {
+        "stay_still": {
+            "left_hand_pose": [
+                [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "right_hand_pose": [
+                [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "allowed_steps_per_motion": 10,
+            "repeat": 2
+        },
+        "vertical_movement": {
+            "left_hand_pose": [
+                [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [-0.23, 0.32, 1.2, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "right_hand_pose": [
+                [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [0.23, 0.32, 1.2, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "allowed_steps_per_motion": 15,
+            "repeat": 2
+        },
+        "horizontal_movement": {
+            "left_hand_pose": [
+                [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [-0.13, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "right_hand_pose": [
+                [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [0.13, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "allowed_steps_per_motion": 15,
+            "repeat": 2
+        },
+        "horizontal_small_movement": {
+            "left_hand_pose": [
+                [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [-0.22, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "right_hand_pose": [
+                [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [0.22, 0.32, 1.1, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "allowed_steps_per_motion": 15,
+            "repeat": 2
+        },
+        "forward_waist_bending_movement": {
+            "left_hand_pose": [
+                [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [-0.23, 0.5, 1.05, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "right_hand_pose": [
+                [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [0.23, 0.5, 1.05, 0.5, 0.5, -0.5, 0.5]
+            ],
+            "allowed_steps_per_motion": 30,
+            "repeat": 3
+        },
+        "rotation_movements": {
+            "left_hand_pose": [
+                [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [-0.23, 0.32, 1.1, 0.7071, 0.7071, 0.0, 0.0],
+                [-0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [-0.23, 0.32, 1.1, 0.0000, 0.0000, -0.7071, 0.7071]
+            ],
+            "right_hand_pose": [
+                [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [0.23, 0.32, 1.1, 0.0000, 0.0000, -0.7071, 0.7071],
+                [0.23, 0.28, 1.1, 0.5, 0.5, -0.5, 0.5],
+                [0.23, 0.32, 1.1, 0.7071, 0.7071, 0.0, 0.0]
+            ],
+            "allowed_steps_per_motion": 20,
+            "repeat": 2
+        }
+    }
+}

source/isaaclab/test/controllers/test_null_space_posture_task.py

+# 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
+"""Launch Isaac Sim Simulator first."""
+# Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim
+# pinocchio is required by the Pink IK controller
+import sys
+
+if sys.platform != "win32":
+    import pinocchio  # noqa: F401
+    import pinocchio as pin  # noqa: F401
+else:
+    import pinocchio  # noqa: F401
+    import pinocchio as pin  # noqa: F401
+
+from isaaclab.app import AppLauncher
+
+# launch omniverse app
+simulation_app = AppLauncher(headless=True).app
+
+"""Unit tests for NullSpacePostureTask with simplified robot configuration using Pink library directly."""
+
+import numpy as np
+
+import pytest
+from pink.configuration import Configuration
+from pink.tasks import FrameTask
+from pinocchio.robot_wrapper import RobotWrapper
+
+from isaaclab.controllers.pink_ik.null_space_posture_task import NullSpacePostureTask
+
+
+class TestNullSpacePostureTaskSimplifiedRobot:
+    """Test cases for NullSpacePostureTask with simplified robot configuration."""
+
+    @pytest.fixture
+    def num_joints(self):
+        """Number of joints in the simplified robot."""
+        return 20
+
+    @pytest.fixture
+    def joint_configurations(self):
+        """Pre-generated joint configurations for testing."""
+        # Set random seed for reproducible tests
+        np.random.seed(42)
+
+        return {
+            "random": np.random.uniform(-0.5, 0.5, 20),
+            "controlled_only": np.array([0.5] * 5 + [0.0] * 15),  # Non-zero for controlled joints only
+            "sequential": np.linspace(0.1, 2.0, 20),
+        }
+
+    @pytest.fixture
+    def robot_urdf(self):
+        """Load the simplified test robot URDF file."""
+        import os
+
+        current_dir = os.path.dirname(os.path.abspath(__file__))
+        urdf_path = os.path.join(current_dir, "simplified_test_robot.urdf")
+        return urdf_path
+
+    @pytest.fixture
+    def robot_configuration(self, robot_urdf):
+        """Simplified robot wrapper."""
+        wrapper = RobotWrapper.BuildFromURDF(robot_urdf, None, root_joint=None)
+        return Configuration(wrapper.model, wrapper.data, wrapper.q0)
+
+    @pytest.fixture
+    def tasks(self):
+        """pink tasks."""
+        return [
+            FrameTask("left_hand_pitch_link", position_cost=1.0, orientation_cost=1.0),
+            NullSpacePostureTask(
+                cost=1.0,
+                controlled_frames=["left_hand_pitch_link"],
+                controlled_joints=[
+                    "waist_yaw_joint",
+                    "waist_pitch_joint",
+                    "waist_roll_joint",
+                    "left_shoulder_pitch_joint",
+                    "left_shoulder_roll_joint",
+                ],
+            ),
+        ]
+
+    def test_null_space_jacobian_zero_end_effector_velocity(
+        self, robot_configuration, tasks, joint_configurations, num_joints
+    ):
+        """Test that velocities projected through null space Jacobian result in zero end-effector velocity."""
+        # Set specific joint configuration
+        robot_configuration.q = joint_configurations["random"]
+
+        # Set frame task target to a specific position in workspace
+        frame_task = tasks[0]
+        # Create pin.SE3 from position and quaternion
+        position = np.array([0.5, 0.3, 0.8])  # x, y, z
+        quaternion = pin.Quaternion(1.0, 0.0, 0.0, 0.0)  # w, x, y, z (identity quaternion)
+        target_pose = pin.SE3(quaternion, position)
+        frame_task.set_target(target_pose)
+
+        # Set null space posture task target
+        null_space_task = tasks[1]
+        target_posture = np.zeros(num_joints)
+        null_space_task.set_target(target_posture)
+
+        # Get the null space Jacobian
+        null_space_jacobian = null_space_task.compute_jacobian(robot_configuration)
+
+        # Get the end-effector Jacobian
+        frame_task_jacobian = frame_task.compute_jacobian(robot_configuration)
+
+        # Test multiple random velocities in null space
+        for _ in range(10):
+            # Generate random joint velocity
+            random_velocity = np.random.randn(num_joints) * 0.1
+
+            # Project through null space Jacobian
+            null_space_velocity = null_space_jacobian @ random_velocity
+
+            # Compute resulting end-effector velocity
+            ee_velocity = frame_task_jacobian @ null_space_velocity
+
+            # The end-effector velocity should be approximately zero
+            assert np.allclose(ee_velocity, np.zeros(6), atol=1e-7), f"End-effector velocity not zero: {ee_velocity}"
+
+    def test_null_space_jacobian_properties(self, robot_configuration, tasks, joint_configurations, num_joints):
+        """Test mathematical properties of the null space Jacobian."""
+        # Set specific joint configuration
+        robot_configuration.q = joint_configurations["random"]
+
+        # Set frame task target
+        frame_task = tasks[0]
+        # Create pin.SE3 from position and quaternion
+        position = np.array([0.3, 0.4, 0.6])
+        quaternion = pin.Quaternion(0.707, 0.0, 0.0, 0.707)  # w, x, y, z (90-degree rotation around X)
+        target_pose = pin.SE3(quaternion, position)
+        frame_task.set_target(target_pose)
+
+        # Set null space posture task target
+        null_space_task = tasks[1]
+        target_posture = np.zeros(num_joints)
+        target_posture[0:5] = [0.1, -0.1, 0.2, -0.2, 0.0]  # Set first 5 joints (controlled joints)
+        null_space_task.set_target(target_posture)
+
+        # Get Jacobians
+        null_space_jacobian = null_space_task.compute_jacobian(robot_configuration)
+        ee_jacobian = robot_configuration.get_frame_jacobian("left_hand_pitch_link")
+
+        # Test: N * J^T should be approximately zero (null space property)
+        # where N is the null space projector and J is the end-effector Jacobian
+        null_space_projection = null_space_jacobian @ ee_jacobian.T
+        assert np.allclose(
+            null_space_projection, np.zeros_like(null_space_projection), atol=1e-7
+        ), f"Null space projection of end-effector Jacobian not zero: {null_space_projection}"
+
+    def test_null_space_jacobian_identity_when_no_frame_tasks(
+        self, robot_configuration, joint_configurations, num_joints
+    ):
+        """Test that null space Jacobian is identity when no frame tasks are defined."""
+        # Create null space task without frame task controlled joints
+        null_space_task = NullSpacePostureTask(cost=1.0, controlled_frames=[], controlled_joints=[])
+
+        # Set specific joint configuration
+        robot_configuration.q = joint_configurations["sequential"]
+
+        # Set target
+        target_posture = np.zeros(num_joints)
+        null_space_task.set_target(target_posture)
+
+        # Get the null space Jacobian
+        null_space_jacobian = null_space_task.compute_jacobian(robot_configuration)
+
+        # Should be identity matrix
+        expected_identity = np.eye(num_joints)
+        assert np.allclose(
+            null_space_jacobian, expected_identity
+        ), f"Null space Jacobian should be identity when no frame tasks defined: {null_space_jacobian}"
+
+    def test_null_space_jacobian_consistency_across_configurations(
+        self, robot_configuration, tasks, joint_configurations, num_joints
+    ):
+        """Test that null space Jacobian is consistent across different joint configurations."""
+        # Test multiple joint configurations
+        test_configs = [
+            np.zeros(num_joints),  # Zero configuration
+            joint_configurations["controlled_only"],  # Non-zero for controlled joints
+            joint_configurations["random"],  # Random configuration
+        ]
+
+        # Set frame task target
+        frame_task = tasks[0]
+        # Create pin.SE3 from position and quaternion
+        position = np.array([0.3, 0.3, 0.5])
+        quaternion = pin.Quaternion(1.0, 0.0, 0.0, 0.0)  # w, x, y, z (identity quaternion)
+        target_pose = pin.SE3(quaternion, position)
+        frame_task.set_target(target_pose)
+
+        # Set null space posture task target
+        null_space_task = tasks[1]
+        target_posture = np.zeros(num_joints)
+        null_space_task.set_target(target_posture)
+
+        jacobians = []
+        for config in test_configs:
+            robot_configuration.q = config
+            jacobian = null_space_task.compute_jacobian(robot_configuration)
+            jacobians.append(jacobian)
+
+            # Verify that velocities through this Jacobian result in zero end-effector velocity
+            ee_jacobian = robot_configuration.get_frame_jacobian("left_hand_pitch_link")
+
+            # Test with random velocity
+            random_velocity = np.random.randn(num_joints) * 0.1
+            null_space_velocity = jacobian @ random_velocity
+            ee_velocity = ee_jacobian @ null_space_velocity
+
+            assert np.allclose(
+                ee_velocity, np.zeros(6), atol=1e-7
+            ), f"End-effector velocity not zero for configuration {config}: {ee_velocity}"
+
+    def test_compute_error_without_target(self, robot_configuration, joint_configurations):
+        """Test that compute_error raises ValueError when no target is set."""
+        null_space_task = NullSpacePostureTask(
+            cost=1.0,
+            controlled_frames=["left_hand_pitch_link"],
+            controlled_joints=["waist_yaw_joint", "waist_pitch_joint"],
+        )
+
+        robot_configuration.q = joint_configurations["sequential"]
+
+        # Should raise ValueError when no target is set
+        with pytest.raises(ValueError, match="No posture target has been set"):
+            null_space_task.compute_error(robot_configuration)
+
+    def test_joint_masking(self, robot_configuration, joint_configurations, num_joints):
+        """Test that joint mask correctly filters only controlled joints."""
+
+        controlled_joint_names = ["waist_pitch_joint", "left_shoulder_pitch_joint", "left_elbow_pitch_joint"]
+
+        # Create task with specific controlled joints
+        null_space_task = NullSpacePostureTask(
+            cost=1.0, controlled_frames=["left_hand_pitch_link"], controlled_joints=controlled_joint_names
+        )
+
+        # Find the joint indexes in robot_configuration.model.names.tolist()[1:]
+        joint_names = robot_configuration.model.names.tolist()[1:]
+        joint_indexes = [joint_names.index(jn) for jn in controlled_joint_names]
+
+        # Set configurations
+        current_config = joint_configurations["sequential"]
+        target_config = np.zeros(num_joints)
+
+        robot_configuration.q = current_config
+        null_space_task.set_target(target_config)
+
+        # Compute error
+        error = null_space_task.compute_error(robot_configuration)
+
+        # Only controlled joints should have non-zero error
+        # Joint indices: waist_yaw_joint=0, waist_pitch_joint=1, waist_roll_joint=2, left_shoulder_pitch_joint=3, left_shoulder_roll_joint=4, etc.
+        expected_error = np.zeros(num_joints)
+        for i in joint_indexes:
+            expected_error[i] = current_config[i]
+
+        assert np.allclose(
+            error, expected_error, atol=1e-7
+        ), f"Joint mask not working correctly: expected {expected_error}, got {error}"
+
+    def test_empty_controlled_joints(self, robot_configuration, joint_configurations, num_joints):
+        """Test behavior when controlled_joints is empty."""
+        null_space_task = NullSpacePostureTask(
+            cost=1.0, controlled_frames=["left_hand_pitch_link"], controlled_joints=[]
+        )
+
+        current_config = joint_configurations["sequential"]
+        target_config = np.zeros(num_joints)
+
+        robot_configuration.q = current_config
+        null_space_task.set_target(target_config)
+
+        # Error should be all zeros
+        error = null_space_task.compute_error(robot_configuration)
+        expected_error = np.zeros(num_joints)
+        assert np.allclose(error, expected_error), f"Error should be zero when no joints controlled: {error}"
+
+    def test_set_target_from_configuration(self, robot_configuration, joint_configurations):
+        """Test set_target_from_configuration method."""
+        null_space_task = NullSpacePostureTask(
+            cost=1.0,
+            controlled_frames=["left_hand_pitch_link"],
+            controlled_joints=["waist_yaw_joint", "waist_pitch_joint"],
+        )
+
+        # Set a specific configuration
+        test_config = joint_configurations["sequential"]
+        robot_configuration.q = test_config
+
+        # Set target from configuration
+        null_space_task.set_target_from_configuration(robot_configuration)
+
+        # Verify target was set correctly
+        assert null_space_task.target_q is not None
+        assert np.allclose(null_space_task.target_q, test_config)
+
+    def test_multiple_frame_tasks(self, robot_configuration, joint_configurations, num_joints):
+        """Test null space projection with multiple frame tasks."""
+        # Create task with multiple controlled frames
+        null_space_task = NullSpacePostureTask(
+            cost=1.0,
+            controlled_frames=["left_hand_pitch_link", "right_hand_pitch_link"],
+            controlled_joints=["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"],
+        )
+
+        current_config = joint_configurations["sequential"]
+        robot_configuration.q = current_config
+
+        # Get null space Jacobian
+        null_space_jacobian = null_space_task.compute_jacobian(robot_configuration)
+
+        # Get Jacobians for both frames
+        jacobian_left_hand = robot_configuration.get_frame_jacobian("left_hand_pitch_link")
+        jacobian_right_hand = robot_configuration.get_frame_jacobian("right_hand_pitch_link")
+
+        # Test that null space velocities result in zero velocity for both frames
+        for _ in range(5):
+            random_velocity = np.random.randn(num_joints) * 0.1
+            null_space_velocity = null_space_jacobian @ random_velocity
+
+            # Check both frames
+            ee_velocity_left = jacobian_left_hand @ null_space_velocity
+            ee_velocity_right = jacobian_right_hand @ null_space_velocity
+
+            assert np.allclose(
+                ee_velocity_left, np.zeros(6), atol=1e-7
+            ), f"Left hand velocity not zero: {ee_velocity_left}"
+            assert np.allclose(
+                ee_velocity_right, np.zeros(6), atol=1e-7
+            ), f"Right hand velocity not zero: {ee_velocity_right}"

source/isaaclab/test/controllers/test_pink_ik.py

@@ -4,10 +4,10 @@
 # SPDX-License-Identifier: BSD-3-Clause
 
 """Launch Isaac Sim Simulator first."""
-import sys
-
 # Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim
 # pinocchio is required by the Pink IK controller
+import sys
+
 if sys.platform != "win32":
     import pinocchio  # noqa: F401
 
@@ -20,9 +20,13 @@ simulation_app = AppLauncher(headless=True).app
 
 import contextlib
 import gymnasium as gym
+import json
+import numpy as np
+import os
 import torch
 
 import pytest
+from pink.configuration import Configuration
 
 from isaaclab.utils.math import axis_angle_from_quat, matrix_from_quat, quat_from_matrix, quat_inv
 
@@ -31,179 +35,329 @@ import isaaclab_tasks.manager_based.manipulation.pick_place  # noqa: F401
 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg
 
 
+@pytest.fixture(scope="module")
+def test_cfg():
+    """Load test configuration."""
+    config_path = os.path.join(os.path.dirname(__file__), "test_configs", "pink_ik_gr1_test_configs.json")
+    with open(config_path) as f:
+        return json.load(f)
+
+
+@pytest.fixture(scope="module")
+def test_params(test_cfg):
+    """Set up test parameters."""
+    return {
+        "position_tolerance": test_cfg["tolerances"]["position"],
+        "rotation_tolerance": test_cfg["tolerances"]["rotation"],
+        "pd_position_tolerance": test_cfg["tolerances"]["pd_position"],
+        "check_errors": test_cfg["tolerances"]["check_errors"],
+    }
+
+
+def create_test_env(num_envs):
+    """Create a test environment with the Pink IK controller."""
+    env_name = "Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0"
+    device = "cuda:0"
+
+    try:
+        env_cfg = parse_env_cfg(env_name, device=device, num_envs=num_envs)
+        # Modify scene config to not spawn the packing table to avoid collision with the robot
+        del env_cfg.scene.packing_table
+        del env_cfg.terminations.object_dropping
+        del env_cfg.terminations.time_out
+        return gym.make(env_name, cfg=env_cfg).unwrapped, env_cfg
+    except Exception as e:
+        print(f"Failed to create environment: {str(e)}")
+        raise
+
+
+@pytest.fixture(scope="module")
+def env_and_cfg():
+    """Create environment and configuration for tests."""
+    env, env_cfg = create_test_env(num_envs=1)
+
+    # Set up camera view
+    env.sim.set_camera_view(eye=[2.5, 2.5, 2.5], target=[0.0, 0.0, 1.0])
+
+    return env, env_cfg
+
+
 @pytest.fixture
-def pink_ik_test_config():
-    """Test configuration for Pink IK controller tests."""
-    # End effector position mean square error tolerance in meters
-    pos_tolerance = 0.03  # 3 cm
-    # End effector orientation mean square error tolerance in radians
-    rot_tolerance = 0.17  # 10 degrees
-
-    # Number of environments
-    num_envs = 1
-
-    # Number of joints in the 2 robot hands
-    num_joints_in_robot_hands = 22
-
-    # Number of steps to wait for controller convergence
-    num_steps_controller_convergence = 25
-
-    num_times_to_move_hands_up = 3
-    num_times_to_move_hands_down = 3
-
-    # Create starting setpoints with respect to the env origin frame
-    # These are the setpoints for the forward kinematics result of the
-    # InitialStateCfg specified in `PickPlaceGR1T2EnvCfg`
-    y_axis_z_axis_90_rot_quaternion = [0.5, 0.5, -0.5, 0.5]
-    left_hand_roll_link_pos = [-0.23, 0.28, 1.1]
-    left_hand_roll_link_pose = left_hand_roll_link_pos + y_axis_z_axis_90_rot_quaternion
-    right_hand_roll_link_pos = [0.23, 0.28, 1.1]
-    right_hand_roll_link_pose = right_hand_roll_link_pos + y_axis_z_axis_90_rot_quaternion
+def test_setup(env_and_cfg):
+    """Set up test case - runs before each test."""
+    env, env_cfg = env_and_cfg
+
+    num_joints_in_robot_hands = env_cfg.actions.pink_ik_cfg.controller.num_hand_joints
+
+    # Get Action Term and IK controller
+    action_term = env.action_manager.get_term(name="pink_ik_cfg")
+    pink_controllers = action_term._ik_controllers
+    articulation = action_term._asset
+
+    # Initialize Pink Configuration for forward kinematics
+    kinematics_model = Configuration(
+        pink_controllers[0].robot_wrapper.model,
+        pink_controllers[0].robot_wrapper.data,
+        pink_controllers[0].robot_wrapper.q0,
+    )
+    left_target_link_name = env_cfg.actions.pink_ik_cfg.target_eef_link_names["left_wrist"]
+    right_target_link_name = env_cfg.actions.pink_ik_cfg.target_eef_link_names["right_wrist"]
 
     return {
-        "pos_tolerance": pos_tolerance,
-        "rot_tolerance": rot_tolerance,
-        "num_envs": num_envs,
+        "env": env,
+        "env_cfg": env_cfg,
         "num_joints_in_robot_hands": num_joints_in_robot_hands,
-        "num_steps_controller_convergence": num_steps_controller_convergence,
-        "num_times_to_move_hands_up": num_times_to_move_hands_up,
-        "num_times_to_move_hands_down": num_times_to_move_hands_down,
-        "left_hand_roll_link_pose": left_hand_roll_link_pose,
-        "right_hand_roll_link_pose": right_hand_roll_link_pose,
+        "action_term": action_term,
+        "pink_controllers": pink_controllers,
+        "articulation": articulation,
+        "kinematics_model": kinematics_model,
+        "left_target_link_name": left_target_link_name,
+        "right_target_link_name": right_target_link_name,
     }
 
 
-def test_gr1t2_ik_pose_abs(pink_ik_test_config):
-    """Test IK controller for GR1T2 humanoid.
+def test_stay_still(test_setup, test_cfg):
+    """Test staying still."""
+    print("Running stay still test...")
+    run_movement_test(test_setup, test_cfg["tests"]["stay_still"], test_cfg)
 
-    This test validates that the Pink IK controller can accurately track commanded
-    end-effector poses for a humanoid robot. It specifically:
 
-    1. Creates a GR1T2 humanoid robot with the Pink IK controller
-    2. Sends target pose commands to the left and right hand roll links
-    3. Checks that the observed poses of the links match the target poses within tolerance
-    4. Tests adaptability by moving the hands up and down multiple times
+def test_vertical_movement(test_setup, test_cfg):
+    """Test vertical movement of robot hands."""
+    print("Running vertical movement test...")
+    run_movement_test(test_setup, test_cfg["tests"]["vertical_movement"], test_cfg)
 
-    The test succeeds when the controller can accurately converge to each new target
-    position, demonstrating both accuracy and adaptability to changing targets.
-    """
 
-    env_name = "Isaac-PickPlace-GR1T2-Abs-v0"
-    device = "cuda:0"
-    env_cfg = parse_env_cfg(env_name, device=device, num_envs=pink_ik_test_config["num_envs"])
+def test_horizontal_movement(test_setup, test_cfg):
+    """Test horizontal movement of robot hands."""
+    print("Running horizontal movement test...")
+    run_movement_test(test_setup, test_cfg["tests"]["horizontal_movement"], test_cfg)
 
-    # create environment from loaded config
-    env = gym.make(env_name, cfg=env_cfg).unwrapped
 
-    # reset before starting
-    obs, _ = env.reset()
+def test_horizontal_small_movement(test_setup, test_cfg):
+    """Test small horizontal movement of robot hands."""
+    print("Running horizontal small movement test...")
+    run_movement_test(test_setup, test_cfg["tests"]["horizontal_small_movement"], test_cfg)
 
-    num_runs = 0  # Counter for the number of runs
 
-    move_hands_up = True
-    test_counter = 0
+def test_forward_waist_bending_movement(test_setup, test_cfg):
+    """Test forward waist bending movement of robot hands."""
+    print("Running forward waist bending movement test...")
+    run_movement_test(test_setup, test_cfg["tests"]["forward_waist_bending_movement"], test_cfg)
+
+
+def test_rotation_movements(test_setup, test_cfg):
+    """Test rotation movements of robot hands."""
+    print("Running rotation movements test...")
+    run_movement_test(test_setup, test_cfg["tests"]["rotation_movements"], test_cfg)
+
+
+def run_movement_test(test_setup, test_config, test_cfg, aux_function=None):
+    """Run a movement test with the given configuration."""
+    env = test_setup["env"]
+    num_joints_in_robot_hands = test_setup["num_joints_in_robot_hands"]
 
-    # Get poses from config
-    left_hand_roll_link_pose = pink_ik_test_config["left_hand_roll_link_pose"].copy()
-    right_hand_roll_link_pose = pink_ik_test_config["right_hand_roll_link_pose"].copy()
+    left_hand_poses = np.array(test_config["left_hand_pose"], dtype=np.float32)
+    right_hand_poses = np.array(test_config["right_hand_pose"], dtype=np.float32)
+
+    curr_pose_idx = 0
+    test_counter = 0
+    num_runs = 0
 
-    # simulate environment -- run everything in inference mode
     with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode():
-        while simulation_app.is_running() and not simulation_app.is_exiting():
+        obs, _ = env.reset()
 
+        while simulation_app.is_running() and not simulation_app.is_exiting():
             num_runs += 1
-            setpoint_poses = left_hand_roll_link_pose + right_hand_roll_link_pose
-            actions = setpoint_poses + [0.0] * pink_ik_test_config["num_joints_in_robot_hands"]
-            actions = torch.tensor(actions, device=device)
-            actions = torch.stack([actions for _ in range(env.num_envs)])
 
-            obs, _, _, _, _ = env.step(actions)
+            # Call auxiliary function if provided
+            if aux_function is not None:
+                aux_function(num_runs)
 
-            left_hand_roll_link_pose_obs = obs["policy"]["robot_links_state"][
-                :, env.scene["robot"].data.body_names.index("left_hand_roll_link"), :7
-            ]
-            right_hand_roll_link_pose_obs = obs["policy"]["robot_links_state"][
-                :, env.scene["robot"].data.body_names.index("right_hand_roll_link"), :7
-            ]
-
-            # The setpoints are wrt the env origin frame
-            # The observations are also wrt the env origin frame
-            left_hand_roll_link_feedback = left_hand_roll_link_pose_obs
-            left_hand_roll_link_setpoint = (
-                torch.tensor(left_hand_roll_link_pose, device=device).unsqueeze(0).repeat(env.num_envs, 1)
-            )
-            left_hand_roll_link_pos_error = left_hand_roll_link_setpoint[:, :3] - left_hand_roll_link_feedback[:, :3]
-            left_hand_roll_link_rot_error = axis_angle_from_quat(
-                quat_from_matrix(
-                    matrix_from_quat(left_hand_roll_link_setpoint[:, 3:])
-                    * matrix_from_quat(quat_inv(left_hand_roll_link_feedback[:, 3:]))
-                )
-            )
-
-            right_hand_roll_link_feedback = right_hand_roll_link_pose_obs
-            right_hand_roll_link_setpoint = (
-                torch.tensor(right_hand_roll_link_pose, device=device).unsqueeze(0).repeat(env.num_envs, 1)
-            )
-            right_hand_roll_link_pos_error = right_hand_roll_link_setpoint[:, :3] - right_hand_roll_link_feedback[:, :3]
-            right_hand_roll_link_rot_error = axis_angle_from_quat(
-                quat_from_matrix(
-                    matrix_from_quat(right_hand_roll_link_setpoint[:, 3:])
-                    * matrix_from_quat(quat_inv(right_hand_roll_link_feedback[:, 3:]))
-                )
-            )
-
-            if num_runs % pink_ik_test_config["num_steps_controller_convergence"] == 0:
-                # Check if the left hand roll link is at the target position
-                torch.testing.assert_close(
-                    torch.mean(torch.abs(left_hand_roll_link_pos_error), dim=1),
-                    torch.zeros(env.num_envs, device="cuda:0"),
-                    rtol=0.0,
-                    atol=pink_ik_test_config["pos_tolerance"],
-                )
+            # Create actions from hand poses and joint positions
+            setpoint_poses = np.concatenate([left_hand_poses[curr_pose_idx], right_hand_poses[curr_pose_idx]])
+            actions = np.concatenate([setpoint_poses, np.zeros(num_joints_in_robot_hands)])
+            actions = torch.tensor(actions, device=env.device, dtype=torch.float32)
+            actions = actions.repeat(env.num_envs, 1)
 
-                # Check if the right hand roll link is at the target position
-                torch.testing.assert_close(
-                    torch.mean(torch.abs(right_hand_roll_link_pos_error), dim=1),
-                    torch.zeros(env.num_envs, device="cuda:0"),
-                    rtol=0.0,
-                    atol=pink_ik_test_config["pos_tolerance"],
-                )
+            # Step environment
+            obs, _, _, _, _ = env.step(actions)
 
-                # Check if the left hand roll link is at the target orientation
-                torch.testing.assert_close(
-                    torch.mean(torch.abs(left_hand_roll_link_rot_error), dim=1),
-                    torch.zeros(env.num_envs, device="cuda:0"),
-                    rtol=0.0,
-                    atol=pink_ik_test_config["rot_tolerance"],
+            # Check convergence and verify errors
+            if num_runs % test_config["allowed_steps_per_motion"] == 0:
+                print("Computing errors...")
+                errors = compute_errors(
+                    test_setup, env, left_hand_poses[curr_pose_idx], right_hand_poses[curr_pose_idx]
                 )
+                print_debug_info(errors, test_counter)
+                if test_cfg["tolerances"]["check_errors"]:
+                    verify_errors(errors, test_setup, test_cfg["tolerances"])
+
+                curr_pose_idx = (curr_pose_idx + 1) % len(left_hand_poses)
+                if curr_pose_idx == 0:
+                    test_counter += 1
+                    if test_counter > test_config["repeat"]:
+                        print("Test completed successfully")
+                        break
+
+
+def get_link_pose(env, link_name):
+    """Get the position and orientation of a link."""
+    link_index = env.scene["robot"].data.body_names.index(link_name)
+    link_states = env.scene._articulations["robot"]._data.body_link_state_w
+    link_pose = link_states[:, link_index, :7]
+    return link_pose[:, :3], link_pose[:, 3:7]
+
+
+def calculate_rotation_error(current_rot, target_rot):
+    """Calculate the rotation error between current and target orientations in axis-angle format."""
+    if isinstance(target_rot, torch.Tensor):
+        target_rot_tensor = (
+            target_rot.unsqueeze(0).expand(current_rot.shape[0], -1) if target_rot.dim() == 1 else target_rot
+        )
+    else:
+        target_rot_tensor = torch.tensor(target_rot, device=current_rot.device)
+        if target_rot_tensor.dim() == 1:
+            target_rot_tensor = target_rot_tensor.unsqueeze(0).expand(current_rot.shape[0], -1)
+
+    return axis_angle_from_quat(
+        quat_from_matrix(matrix_from_quat(target_rot_tensor) * matrix_from_quat(quat_inv(current_rot)))
+    )
+
+
+def compute_errors(test_setup, env, left_target_pose, right_target_pose):
+    """Compute all error metrics for the current state."""
+    action_term = test_setup["action_term"]
+    pink_controllers = test_setup["pink_controllers"]
+    articulation = test_setup["articulation"]
+    kinematics_model = test_setup["kinematics_model"]
+    left_target_link_name = test_setup["left_target_link_name"]
+    right_target_link_name = test_setup["right_target_link_name"]
+    num_joints_in_robot_hands = test_setup["num_joints_in_robot_hands"]
+
+    # Get current hand positions and orientations
+    left_hand_pos, left_hand_rot = get_link_pose(env, left_target_link_name)
+    right_hand_pos, right_hand_rot = get_link_pose(env, right_target_link_name)
+
+    # Create setpoint tensors
+    device = env.device
+    num_envs = env.num_envs
+    left_hand_pose_setpoint = torch.tensor(left_target_pose, device=device).unsqueeze(0).repeat(num_envs, 1)
+    right_hand_pose_setpoint = torch.tensor(right_target_pose, device=device).unsqueeze(0).repeat(num_envs, 1)
+    # compensate for the hand rotational offset
+    # nominal_hand_pose_rotmat = matrix_from_quat(torch.tensor(env_cfg.actions.pink_ik_cfg.controller.hand_rotational_offset, device=env.device))
+    left_hand_pose_setpoint[:, 3:7] = quat_from_matrix(matrix_from_quat(left_hand_pose_setpoint[:, 3:7]))
+    right_hand_pose_setpoint[:, 3:7] = quat_from_matrix(matrix_from_quat(right_hand_pose_setpoint[:, 3:7]))
+
+    # Calculate position and rotation errors
+    left_pos_error = left_hand_pose_setpoint[:, :3] - left_hand_pos
+    right_pos_error = right_hand_pose_setpoint[:, :3] - right_hand_pos
+    left_rot_error = calculate_rotation_error(left_hand_rot, left_hand_pose_setpoint[:, 3:])
+    right_rot_error = calculate_rotation_error(right_hand_rot, right_hand_pose_setpoint[:, 3:])
+
+    # Calculate PD controller errors
+    ik_controller = pink_controllers[0]
+    pink_controlled_joint_ids = action_term._pink_controlled_joint_ids
+
+    # Get current and target positions
+    curr_joints = articulation.data.joint_pos[:, pink_controlled_joint_ids].cpu().numpy()[0]
+    target_joints = action_term.processed_actions[:, :num_joints_in_robot_hands].cpu().numpy()[0]
+
+    # Reorder joints for Pink IK
+    curr_joints = np.array(curr_joints)[ik_controller.isaac_lab_to_pink_ordering]
+    target_joints = np.array(target_joints)[ik_controller.isaac_lab_to_pink_ordering]
+
+    # Run forward kinematics
+    kinematics_model.update(curr_joints)
+    left_curr_pos = kinematics_model.get_transform_frame_to_world(
+        frame="GR1T2_fourier_hand_6dof_left_hand_pitch_link"
+    ).translation
+    right_curr_pos = kinematics_model.get_transform_frame_to_world(
+        frame="GR1T2_fourier_hand_6dof_right_hand_pitch_link"
+    ).translation
+
+    kinematics_model.update(target_joints)
+    left_target_pos = kinematics_model.get_transform_frame_to_world(
+        frame="GR1T2_fourier_hand_6dof_left_hand_pitch_link"
+    ).translation
+    right_target_pos = kinematics_model.get_transform_frame_to_world(
+        frame="GR1T2_fourier_hand_6dof_right_hand_pitch_link"
+    ).translation
+
+    # Calculate PD errors
+    left_pd_error = (
+        torch.tensor(left_target_pos - left_curr_pos, device=device, dtype=torch.float32)
+        .unsqueeze(0)
+        .repeat(num_envs, 1)
+    )
+    right_pd_error = (
+        torch.tensor(right_target_pos - right_curr_pos, device=device, dtype=torch.float32)
+        .unsqueeze(0)
+        .repeat(num_envs, 1)
+    )
+
+    return {
+        "left_pos_error": left_pos_error,
+        "right_pos_error": right_pos_error,
+        "left_rot_error": left_rot_error,
+        "right_rot_error": right_rot_error,
+        "left_pd_error": left_pd_error,
+        "right_pd_error": right_pd_error,
+    }
 
-                # Check if the right hand roll link is at the target orientation
-                torch.testing.assert_close(
-                    torch.mean(torch.abs(right_hand_roll_link_rot_error), dim=1),
-                    torch.zeros(env.num_envs, device="cuda:0"),
-                    rtol=0.0,
-                    atol=pink_ik_test_config["rot_tolerance"],
-                )
 
-                # Change the setpoints to move the hands up and down as per the counter
-                test_counter += 1
-                if move_hands_up and test_counter > pink_ik_test_config["num_times_to_move_hands_up"]:
-                    move_hands_up = False
-                elif not move_hands_up and test_counter > (
-                    pink_ik_test_config["num_times_to_move_hands_down"]
-                    + pink_ik_test_config["num_times_to_move_hands_up"]
-                ):
-                    # Test is done after moving the hands up and down
-                    break
-                if move_hands_up:
-                    left_hand_roll_link_pose[1] += 0.05
-                    left_hand_roll_link_pose[2] += 0.05
-                    right_hand_roll_link_pose[1] += 0.05
-                    right_hand_roll_link_pose[2] += 0.05
-                else:
-                    left_hand_roll_link_pose[1] -= 0.05
-                    left_hand_roll_link_pose[2] -= 0.05
-                    right_hand_roll_link_pose[1] -= 0.05
-                    right_hand_roll_link_pose[2] -= 0.05
-
-    env.close()
+def verify_errors(errors, test_setup, tolerances):
+    """Verify that all error metrics are within tolerance."""
+    env = test_setup["env"]
+    device = env.device
+    num_envs = env.num_envs
+    zero_tensor = torch.zeros(num_envs, device=device)
+
+    for hand in ["left", "right"]:
+        # Check PD controller errors
+        pd_error_norm = torch.norm(errors[f"{hand}_pd_error"], dim=1)
+        torch.testing.assert_close(
+            pd_error_norm,
+            zero_tensor,
+            rtol=0.0,
+            atol=tolerances["pd_position"],
+            msg=(
+                f"{hand.capitalize()} hand PD controller error ({pd_error_norm.item():.6f}) exceeds tolerance"
+                f" ({tolerances['pd_position']:.6f})"
+            ),
+        )
+
+        # Check IK position errors
+        pos_error_norm = torch.norm(errors[f"{hand}_pos_error"], dim=1)
+        torch.testing.assert_close(
+            pos_error_norm,
+            zero_tensor,
+            rtol=0.0,
+            atol=tolerances["position"],
+            msg=(
+                f"{hand.capitalize()} hand IK position error ({pos_error_norm.item():.6f}) exceeds tolerance"
+                f" ({tolerances['position']:.6f})"
+            ),
+        )
+
+        # Check rotation errors
+        rot_error_max = torch.max(errors[f"{hand}_rot_error"])
+        torch.testing.assert_close(
+            rot_error_max,
+            torch.zeros_like(rot_error_max),
+            rtol=0.0,
+            atol=tolerances["rotation"],
+            msg=(
+                f"{hand.capitalize()} hand IK rotation error ({rot_error_max.item():.6f}) exceeds tolerance"
+                f" ({tolerances['rotation']:.6f})"
+            ),
+        )
+
+
+def print_debug_info(errors, test_counter):
+    """Print debug information about the current state."""
+    print(f"\nTest iteration {test_counter + 1}:")
+    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/test/utils/test_string.py

@@ -164,6 +164,27 @@ def test_resolve_matching_names_values_with_basic_strings():
         _ = string_utils.resolve_matching_names_values(query_names, target_names)
 
 
+def test_resolve_matching_names_values_with_strict_false():
+    """Test resolving matching names with strict=False parameter."""
+    # list of strings
+    target_names = ["a", "b", "c", "d", "e"]
+    # test strict=False
+    data = {"a|c": 1, "b": 2, "f": 3}
+    index_list, names_list, values_list = string_utils.resolve_matching_names_values(data, target_names, strict=False)
+    assert index_list == [0, 1, 2]
+    assert names_list == ["a", "b", "c"]
+    assert values_list == [1, 2, 1]
+
+    # test failure case: multiple matches for a string (should still raise ValueError even with strict=False)
+    data = {"a|c": 1, "a": 2, "b": 3}
+    with pytest.raises(ValueError, match="Multiple matches for 'a':"):
+        _ = string_utils.resolve_matching_names_values(data, target_names, strict=False)
+
+    # test failure case: invalid input type (should still raise TypeError even with strict=False)
+    with pytest.raises(TypeError, match="Input argument `data` should be a dictionary"):
+        _ = string_utils.resolve_matching_names_values("not_a_dict", target_names, strict=False)
+
+
 def test_resolve_matching_names_values_with_basic_strings_and_preserved_order():
     """Test resolving matching names with a basic expression."""
     # list of strings

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_HIGH_PD_CFG`: The GR1T2 humanoid configured with high PD gains on upper body joints for pick-place manipulation tasks.
 
 Reference: https://www.fftai.com/products-gr1
 """
@@ -123,3 +124,40 @@ GR1T2_CFG = ArticulationCfg(
     },
 )
 """Configuration for the GR1T2 Humanoid robot."""
+
+
+GR1T2_HIGH_PD_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 with high PD gains for pick-place manipulation tasks."""

source/isaaclab_mimic/config/extension.toml

 [package]
 
 # Semantic Versioning is used: https://semver.org/
-version = "1.0.12"
+version = "1.0.13"
 
 # Description
 category = "isaaclab"

source/isaaclab_mimic/docs/CHANGELOG.rst

 Changelog
 ---------
 
+1.0.13 (2025-08-14)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added :class:`PickPlaceGR1T2WaistEnabledEnvCfg` and :class:`PickPlaceGR1T2WaistEnabledMimicEnvCfg` for GR1T2 robot manipulation tasks with waist joint control enabled.
+
 1.0.12 (2025-07-31)
 ~~~~~~~~~~~~~~~~~~~
 

source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py

@@ -11,6 +11,7 @@ from .exhaustpipe_gr1t2_mimic_env_cfg import ExhaustPipeGR1T2MimicEnvCfg
 from .nutpour_gr1t2_mimic_env_cfg import NutPourGR1T2MimicEnvCfg
 from .pickplace_gr1t2_mimic_env import PickPlaceGR1T2MimicEnv
 from .pickplace_gr1t2_mimic_env_cfg import PickPlaceGR1T2MimicEnvCfg
+from .pickplace_gr1t2_waist_enabled_mimic_env_cfg import PickPlaceGR1T2WaistEnabledMimicEnvCfg
 
 gym.register(
     id="Isaac-PickPlace-GR1T2-Abs-Mimic-v0",
@@ -21,6 +22,15 @@ gym.register(
     disable_env_checker=True,
 )
 
+gym.register(
+    id="Isaac-PickPlace-GR1T2-WaistEnabled-Abs-Mimic-v0",
+    entry_point="isaaclab_mimic.envs.pinocchio_envs:PickPlaceGR1T2MimicEnv",
+    kwargs={
+        "env_cfg_entry_point": pickplace_gr1t2_waist_enabled_mimic_env_cfg.PickPlaceGR1T2WaistEnabledMimicEnvCfg,
+    },
+    disable_env_checker=True,
+)
+
 gym.register(
     id="Isaac-NutPour-GR1T2-Pink-IK-Abs-Mimic-v0",
     entry_point="isaaclab_mimic.envs.pinocchio_envs:PickPlaceGR1T2MimicEnv",

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

@@ -39,7 +39,7 @@ class PickPlaceGR1T2MimicEnv(ManagerBasedRLMimicEnv):
         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,7 +49,7 @@ class PickPlaceGR1T2MimicEnv(ManagerBasedRLMimicEnv):
         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.
+            action_noise_dict: Noise to add to the action. If None, no noise is added.
             env_id: Environment index to get the action for.
 
         Returns:

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

@@ -17,7 +17,7 @@ class PickPlaceGR1T2MimicEnvCfg(PickPlaceGR1T2EnvCfg, MimicEnvCfg):
         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_cfg.py

+# Copyright (c) 2024-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
+# All rights reserved.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig
+from isaaclab.utils import configclass
+
+from isaaclab_tasks.manager_based.manipulation.pick_place.pickplace_gr1t2_waist_enabled_env_cfg import (
+    PickPlaceGR1T2WaistEnabledEnvCfg,
+)
+
+
+@configclass
+class PickPlaceGR1T2WaistEnabledMimicEnvCfg(PickPlaceGR1T2WaistEnabledEnvCfg, MimicEnvCfg):
+
+    def __post_init__(self):
+        # Calling post init of parents
+        super().__post_init__()
+
+        # Override the existing values
+        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
+        self.datagen_config.generation_select_src_per_subtask = False
+        self.datagen_config.generation_select_src_per_arm = False
+        self.datagen_config.generation_relative = False
+        self.datagen_config.generation_joint_pos = False
+        self.datagen_config.generation_transform_first_robot_pose = False
+        self.datagen_config.generation_interpolate_from_last_target_pose = True
+        self.datagen_config.max_num_failures = 25
+        self.datagen_config.num_demo_to_render = 10
+        self.datagen_config.num_fail_demo_to_render = 25
+        self.datagen_config.seed = 1
+
+        # The following are the subtask configurations for the stack task.
+        subtask_configs = []
+        subtask_configs.append(
+            SubTaskConfig(
+                # Each subtask involves manipulation with respect to a single object frame.
+                object_ref="object",
+                # This key corresponds to the binary indicator in "datagen_info" that signals
+                # when this subtask is finished (e.g., on a 0 to 1 edge).
+                subtask_term_signal="idle_right",
+                first_subtask_start_offset_range=(0, 0),
+                # Randomization range for starting index of the first subtask
+                subtask_term_offset_range=(0, 0),
+                # Selection strategy for the source subtask segment during data generation
+                # selection_strategy="nearest_neighbor_object",
+                selection_strategy="nearest_neighbor_object",
+                # Optional parameters for the selection strategy function
+                selection_strategy_kwargs={"nn_k": 3},
+                # Amount of action noise to apply during this subtask
+                action_noise=0.003,
+                # Number of interpolation steps to bridge to this subtask segment
+                num_interpolation_steps=0,
+                # Additional fixed steps for the robot to reach the necessary pose
+                num_fixed_steps=0,
+                # If True, apply action noise during the interpolation phase and execution
+                apply_noise_during_interpolation=False,
+            )
+        )
+        subtask_configs.append(
+            SubTaskConfig(
+                # Each subtask involves manipulation with respect to a single object frame.
+                object_ref="object",
+                # Corresponding key for the binary indicator in "datagen_info" for completion
+                subtask_term_signal=None,
+                # Time offsets for data generation when splitting a trajectory
+                subtask_term_offset_range=(0, 0),
+                # Selection strategy for source subtask segment
+                selection_strategy="nearest_neighbor_object",
+                # Optional parameters for the selection strategy function
+                selection_strategy_kwargs={"nn_k": 3},
+                # Amount of action noise to apply during this subtask
+                action_noise=0.003,
+                # Number of interpolation steps to bridge to this subtask segment
+                num_interpolation_steps=3,
+                # Additional fixed steps for the robot to reach the necessary pose
+                num_fixed_steps=0,
+                # If True, apply action noise during the interpolation phase and execution
+                apply_noise_during_interpolation=False,
+            )
+        )
+        self.subtask_configs["right"] = subtask_configs
+
+        subtask_configs = []
+        subtask_configs.append(
+            SubTaskConfig(
+                # Each subtask involves manipulation with respect to a single object frame.
+                object_ref="object",
+                # Corresponding key for the binary indicator in "datagen_info" for completion
+                subtask_term_signal=None,
+                # Time offsets for data generation when splitting a trajectory
+                subtask_term_offset_range=(0, 0),
+                # Selection strategy for source subtask segment
+                selection_strategy="nearest_neighbor_object",
+                # Optional parameters for the selection strategy function
+                selection_strategy_kwargs={"nn_k": 3},
+                # Amount of action noise to apply during this subtask
+                action_noise=0.003,
+                # Number of interpolation steps to bridge to this subtask segment
+                num_interpolation_steps=0,
+                # Additional fixed steps for the robot to reach the necessary pose
+                num_fixed_steps=0,
+                # If True, apply action noise during the interpolation phase and execution
+                apply_noise_during_interpolation=False,
+            )
+        )
+        self.subtask_configs["left"] = subtask_configs

source/isaaclab_tasks/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.10.46"
+version = "0.10.47"
 
 # Description
 title = "Isaac Lab Environments"

source/isaaclab_tasks/docs/CHANGELOG.rst

 Changelog
 ---------
 
+0.10.47 (2025-07-25)
+~~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* New ``Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0`` environment that enables the waist degrees-of-freedom for the GR1T2 robot.
+
+
+Changed
+^^^^^^^
+
+* Updated pink inverse kinematics controller configuration for the following tasks (``Isaac-PickPlace-GR1T2``, ``Isaac-NutPour-GR1T2``, ``Isaac-ExhaustPipe-GR1T2``)
+  to increase end-effector tracking accuracy and speed. Also added a null-space regularizer that enables turning on of waist degrees-of-freedom without
+  the robot control drifting to a bending posture.
+* Tuned the pink inverse kinematics controller and joint PD controllers for the following tasks (``Isaac-PickPlace-GR1T2``, ``Isaac-NutPour-GR1T2``, ``Isaac-ExhaustPipe-GR1T2``)
+  to improve the end-effector tracking accuracy and speed. Achieving position and orientation accuracy test within **(2 mm, 1 degree)**.
+
+
 0.10.46 (2025-08-16)
 ~~~~~~~~~~~~~~~~~~~~
 

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py

@@ -6,7 +6,13 @@
 import gymnasium as gym
 import os
 
-from . import agents, exhaustpipe_gr1t2_pink_ik_env_cfg, nutpour_gr1t2_pink_ik_env_cfg, pickplace_gr1t2_env_cfg
+from . import (
+    agents,
+    exhaustpipe_gr1t2_pink_ik_env_cfg,
+    nutpour_gr1t2_pink_ik_env_cfg,
+    pickplace_gr1t2_env_cfg,
+    pickplace_gr1t2_waist_enabled_env_cfg,
+)
 
 gym.register(
     id="Isaac-PickPlace-GR1T2-Abs-v0",
@@ -37,3 +43,13 @@ gym.register(
     },
     disable_env_checker=True,
 )
+
+gym.register(
+    id="Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0",
+    entry_point="isaaclab.envs:ManagerBasedRLEnv",
+    kwargs={
+        "env_cfg_entry_point": pickplace_gr1t2_waist_enabled_env_cfg.PickPlaceGR1T2WaistEnabledEnvCfg,
+        "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"),
+    },
+    disable_env_checker=True,
+)

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py

@@ -263,6 +263,9 @@ class ExhaustPipeGR1T2BaseEnvCfg(ManagerBasedRLEnvCfg):
         anchor_rot=(1.0, 0.0, 0.0, 0.0),
     )
 
+    # OpenXR hand tracking has 26 joints per hand
+    NUM_OPENXR_HAND_JOINTS = 26
+
     # Temporary directory for URDF files
     temp_urdf_dir = tempfile.gettempdir()
 

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py

@@ -3,10 +3,10 @@
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
-from pink.tasks import FrameTask
+from pink.tasks import DampingTask, FrameTask
 
 import isaaclab.controllers.utils as ControllerUtils
-from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg
+from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg
 from isaaclab.devices import DevicesCfg
 from isaaclab.devices.openxr import OpenXRDeviceCfg
 from isaaclab.devices.openxr.retargeters import GR1T2RetargeterCfg
@@ -108,6 +108,10 @@ class ExhaustPipeGR1T2PinkIKEnvCfg(ExhaustPipeGR1T2BaseEnvCfg):
                 "L_thumb_distal_joint",
                 "R_thumb_distal_joint",
             ],
+            target_eef_link_names={
+                "left_wrist": "left_hand_pitch_link",
+                "right_wrist": "right_hand_pitch_link",
+            },
             # the robot in the sim scene we are controlling
             asset_name="robot",
             # Configuration for the IK controller
@@ -118,20 +122,45 @@ class ExhaustPipeGR1T2PinkIKEnvCfg(ExhaustPipeGR1T2BaseEnvCfg):
                 base_link_name="base_link",
                 num_hand_joints=22,
                 show_ik_warnings=False,
+                fail_on_joint_limit_violation=False,  # Determines whether to pink solver will fail due to a joint limit violation
                 variable_input_tasks=[
                     FrameTask(
                         "GR1T2_fourier_hand_6dof_left_hand_pitch_link",
-                        position_cost=1.0,  # [cost] / [m]
+                        position_cost=8.0,  # [cost] / [m]
                         orientation_cost=1.0,  # [cost] / [rad]
                         lm_damping=10,  # dampening for solver for step jumps
-                        gain=0.1,
+                        gain=0.5,
                     ),
                     FrameTask(
                         "GR1T2_fourier_hand_6dof_right_hand_pitch_link",
-                        position_cost=1.0,  # [cost] / [m]
+                        position_cost=8.0,  # [cost] / [m]
                         orientation_cost=1.0,  # [cost] / [rad]
                         lm_damping=10,  # dampening for solver for step jumps
-                        gain=0.1,
+                        gain=0.5,
+                    ),
+                    DampingTask(
+                        cost=0.5,  # [cost] * [s] / [rad]
+                    ),
+                    NullSpacePostureTask(
+                        cost=0.2,
+                        lm_damping=1,
+                        controlled_frames=[
+                            "GR1T2_fourier_hand_6dof_left_hand_pitch_link",
+                            "GR1T2_fourier_hand_6dof_right_hand_pitch_link",
+                        ],
+                        controlled_joints=[
+                            "left_shoulder_pitch_joint",
+                            "left_shoulder_roll_joint",
+                            "left_shoulder_yaw_joint",
+                            "left_elbow_pitch_joint",
+                            "right_shoulder_pitch_joint",
+                            "right_shoulder_roll_joint",
+                            "right_shoulder_yaw_joint",
+                            "right_elbow_pitch_joint",
+                            "waist_yaw_joint",
+                            "waist_pitch_joint",
+                            "waist_roll_joint",
+                        ],
                     ),
                 ],
                 fixed_input_tasks=[
@@ -162,8 +191,8 @@ class ExhaustPipeGR1T2PinkIKEnvCfg(ExhaustPipeGR1T2BaseEnvCfg):
                     retargeters=[
                         GR1T2RetargeterCfg(
                             enable_visualization=True,
-                            # OpenXR hand tracking has 26 joints per hand
-                            num_open_xr_hand_joints=2 * 26,
+                            # number of joints in both hands
+                            num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS,
                             sim_device=self.sim.device,
                             hand_joint_names=self.actions.gr1_action.hand_joint_names,
                         ),

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py

@@ -298,6 +298,9 @@ class NutPourGR1T2BaseEnvCfg(ManagerBasedRLEnvCfg):
         anchor_rot=(1.0, 0.0, 0.0, 0.0),
     )
 
+    # OpenXR hand tracking has 26 joints per hand
+    NUM_OPENXR_HAND_JOINTS = 26
+
     # Temporary directory for URDF files
     temp_urdf_dir = tempfile.gettempdir()
 

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py

@@ -3,10 +3,10 @@
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
-from pink.tasks import FrameTask
+from pink.tasks import DampingTask, FrameTask
 
 import isaaclab.controllers.utils as ControllerUtils
-from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg
+from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg
 from isaaclab.devices import DevicesCfg
 from isaaclab.devices.openxr import OpenXRDeviceCfg
 from isaaclab.devices.openxr.retargeters import GR1T2RetargeterCfg
@@ -106,6 +106,10 @@ class NutPourGR1T2PinkIKEnvCfg(NutPourGR1T2BaseEnvCfg):
                 "L_thumb_distal_joint",
                 "R_thumb_distal_joint",
             ],
+            target_eef_link_names={
+                "left_wrist": "left_hand_pitch_link",
+                "right_wrist": "right_hand_pitch_link",
+            },
             # the robot in the sim scene we are controlling
             asset_name="robot",
             # Configuration for the IK controller
@@ -116,20 +120,45 @@ class NutPourGR1T2PinkIKEnvCfg(NutPourGR1T2BaseEnvCfg):
                 base_link_name="base_link",
                 num_hand_joints=22,
                 show_ik_warnings=False,
+                fail_on_joint_limit_violation=False,  # Determines whether to pink solver will fail due to a joint limit violation
                 variable_input_tasks=[
                     FrameTask(
                         "GR1T2_fourier_hand_6dof_left_hand_pitch_link",
-                        position_cost=1.0,  # [cost] / [m]
+                        position_cost=8.0,  # [cost] / [m]
                         orientation_cost=1.0,  # [cost] / [rad]
                         lm_damping=10,  # dampening for solver for step jumps
-                        gain=0.1,
+                        gain=0.5,
                     ),
                     FrameTask(
                         "GR1T2_fourier_hand_6dof_right_hand_pitch_link",
-                        position_cost=1.0,  # [cost] / [m]
+                        position_cost=8.0,  # [cost] / [m]
                         orientation_cost=1.0,  # [cost] / [rad]
                         lm_damping=10,  # dampening for solver for step jumps
-                        gain=0.1,
+                        gain=0.5,
+                    ),
+                    DampingTask(
+                        cost=0.5,  # [cost] * [s] / [rad]
+                    ),
+                    NullSpacePostureTask(
+                        cost=0.2,
+                        lm_damping=1,
+                        controlled_frames=[
+                            "GR1T2_fourier_hand_6dof_left_hand_pitch_link",
+                            "GR1T2_fourier_hand_6dof_right_hand_pitch_link",
+                        ],
+                        controlled_joints=[
+                            "left_shoulder_pitch_joint",
+                            "left_shoulder_roll_joint",
+                            "left_shoulder_yaw_joint",
+                            "left_elbow_pitch_joint",
+                            "right_shoulder_pitch_joint",
+                            "right_shoulder_roll_joint",
+                            "right_shoulder_yaw_joint",
+                            "right_elbow_pitch_joint",
+                            "waist_yaw_joint",
+                            "waist_pitch_joint",
+                            "waist_roll_joint",
+                        ],
                     ),
                 ],
                 fixed_input_tasks=[
@@ -160,8 +189,8 @@ class NutPourGR1T2PinkIKEnvCfg(NutPourGR1T2BaseEnvCfg):
                     retargeters=[
                         GR1T2RetargeterCfg(
                             enable_visualization=True,
-                            # OpenXR hand tracking has 26 joints per hand
-                            num_open_xr_hand_joints=2 * 26,
+                            # number of joints in both hands
+                            num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS,
                             sim_device=self.sim.device,
                             hand_joint_names=self.actions.gr1_action.hand_joint_names,
                         ),

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py

@@ -6,13 +6,13 @@
 import tempfile
 import torch
 
-from pink.tasks import FrameTask
+from pink.tasks import DampingTask, FrameTask
 
 import isaaclab.controllers.utils as ControllerUtils
 import isaaclab.envs.mdp as base_mdp
 import isaaclab.sim as sim_utils
 from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
-from isaaclab.controllers.pink_ik_cfg import PinkIKControllerCfg
+from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg
 from isaaclab.devices.device_base import DevicesCfg
 from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg
 from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2RetargeterCfg
@@ -30,7 +30,7 @@ from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR
 
 from . import mdp
 
-from isaaclab_assets.robots.fourier import GR1T2_CFG  # isort: skip
+from isaaclab_assets.robots.fourier import GR1T2_HIGH_PD_CFG  # isort: skip
 
 
 ##
@@ -59,8 +59,8 @@ class ObjectTableSceneCfg(InteractiveSceneCfg):
         ),
     )
 
-    # Humanoid robot w/ arms higher
-    robot: ArticulationCfg = GR1T2_CFG.replace(
+    # Humanoid robot configured for pick-place manipulation tasks
+    robot: ArticulationCfg = GR1T2_HIGH_PD_CFG.replace(
         prim_path="/World/envs/env_.*/Robot",
         init_state=ArticulationCfg.InitialStateCfg(
             pos=(0, 0, 0.93),
@@ -199,6 +199,10 @@ class ActionsCfg:
             "L_thumb_distal_joint",
             "R_thumb_distal_joint",
         ],
+        target_eef_link_names={
+            "left_wrist": "left_hand_pitch_link",
+            "right_wrist": "right_hand_pitch_link",
+        },
         # the robot in the sim scene we are controlling
         asset_name="robot",
         # Configuration for the IK controller
@@ -209,30 +213,48 @@ class ActionsCfg:
             base_link_name="base_link",
             num_hand_joints=22,
             show_ik_warnings=False,
+            fail_on_joint_limit_violation=False,  # Determines whether to pink solver will fail due to a joint limit violation
             variable_input_tasks=[
                 FrameTask(
                     "GR1T2_fourier_hand_6dof_left_hand_pitch_link",
-                    position_cost=1.0,  # [cost] / [m]
+                    position_cost=8.0,  # [cost] / [m]
                     orientation_cost=1.0,  # [cost] / [rad]
                     lm_damping=10,  # dampening for solver for step jumps
-                    gain=0.1,
+                    gain=0.5,
                 ),
                 FrameTask(
                     "GR1T2_fourier_hand_6dof_right_hand_pitch_link",
-                    position_cost=1.0,  # [cost] / [m]
+                    position_cost=8.0,  # [cost] / [m]
                     orientation_cost=1.0,  # [cost] / [rad]
                     lm_damping=10,  # dampening for solver for step jumps
-                    gain=0.1,
+                    gain=0.5,
+                ),
+                DampingTask(
+                    cost=0.5,  # [cost] * [s] / [rad]
+                ),
+                NullSpacePostureTask(
+                    cost=0.5,
+                    lm_damping=1,
+                    controlled_frames=[
+                        "GR1T2_fourier_hand_6dof_left_hand_pitch_link",
+                        "GR1T2_fourier_hand_6dof_right_hand_pitch_link",
+                    ],
+                    controlled_joints=[
+                        "left_shoulder_pitch_joint",
+                        "left_shoulder_roll_joint",
+                        "left_shoulder_yaw_joint",
+                        "left_elbow_pitch_joint",
+                        "right_shoulder_pitch_joint",
+                        "right_shoulder_roll_joint",
+                        "right_shoulder_yaw_joint",
+                        "right_elbow_pitch_joint",
+                        "waist_yaw_joint",
+                        "waist_pitch_joint",
+                        "waist_roll_joint",
+                    ],
                 ),
             ],
-            fixed_input_tasks=[
-                # COMMENT OUT IF LOCKING WAIST/HEAD
-                # FrameTask(
-                #     "GR1T2_fourier_hand_6dof_head_yaw_link",
-                #     position_cost=1.0,  # [cost] / [m]
-                #     orientation_cost=0.05,  # [cost] / [rad]
-                # ),
-            ],
+            fixed_input_tasks=[],
         ),
     )
 
@@ -331,6 +353,9 @@ class PickPlaceGR1T2EnvCfg(ManagerBasedRLEnvCfg):
         anchor_rot=(1.0, 0.0, 0.0, 0.0),
     )
 
+    # OpenXR hand tracking has 26 joints per hand
+    NUM_OPENXR_HAND_JOINTS = 26
+
     # Temporary directory for URDF files
     temp_urdf_dir = tempfile.gettempdir()
 
@@ -403,8 +428,8 @@ class PickPlaceGR1T2EnvCfg(ManagerBasedRLEnvCfg):
                     retargeters=[
                         GR1T2RetargeterCfg(
                             enable_visualization=True,
-                            # OpenXR hand tracking has 26 joints per hand
-                            num_open_xr_hand_joints=2 * 26,
+                            # number of joints in both hands
+                            num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS,
                             sim_device=self.sim.device,
                             hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names,
                         ),

source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py

+# 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
+
+import tempfile
+
+import isaaclab.controllers.utils as ControllerUtils
+from isaaclab.devices.device_base import DevicesCfg
+from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg
+from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2RetargeterCfg
+from isaaclab.envs import ManagerBasedRLEnvCfg
+from isaaclab.utils import configclass
+
+from .pickplace_gr1t2_env_cfg import ActionsCfg, EventCfg, ObjectTableSceneCfg, ObservationsCfg, TerminationsCfg
+
+
+@configclass
+class PickPlaceGR1T2WaistEnabledEnvCfg(ManagerBasedRLEnvCfg):
+    """Configuration for the GR1T2 environment."""
+
+    # Scene settings
+    scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True)
+    # Basic settings
+    observations: ObservationsCfg = ObservationsCfg()
+    actions: ActionsCfg = ActionsCfg()
+    # MDP settings
+    terminations: TerminationsCfg = TerminationsCfg()
+    events = EventCfg()
+
+    # Unused managers
+    commands = None
+    rewards = None
+    curriculum = None
+
+    # Position of the XR anchor in the world frame
+    xr: XrCfg = XrCfg(
+        anchor_pos=(0.0, 0.0, 0.0),
+        anchor_rot=(1.0, 0.0, 0.0, 0.0),
+    )
+
+    # OpenXR hand tracking has 26 joints per hand
+    NUM_OPENXR_HAND_JOINTS = 26
+
+    # Temporary directory for URDF files
+    temp_urdf_dir = tempfile.gettempdir()
+
+    def __post_init__(self):
+        """Post initialization."""
+        # general settings
+        self.decimation = 6
+        self.episode_length_s = 20.0
+        # simulation settings
+        self.sim.dt = 1 / 120  # 120Hz
+        self.sim.render_interval = 2
+
+        # Add waist joint to pink_ik_cfg
+        waist_joint_names = ["waist_yaw_joint", "waist_pitch_joint", "waist_roll_joint"]
+        for joint_name in waist_joint_names:
+            self.actions.pink_ik_cfg.pink_controlled_joint_names.append(joint_name)
+            self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names.remove(joint_name)
+
+        # Convert USD to URDF and change revolute joints to fixed
+        temp_urdf_output_path, temp_urdf_meshes_output_path = ControllerUtils.convert_usd_to_urdf(
+            self.scene.robot.spawn.usd_path, self.temp_urdf_dir, force_conversion=True
+        )
+        ControllerUtils.change_revolute_to_fixed(
+            temp_urdf_output_path, self.actions.pink_ik_cfg.ik_urdf_fixed_joint_names
+        )
+
+        # Set the URDF and mesh paths for the IK controller
+        self.actions.pink_ik_cfg.controller.urdf_path = temp_urdf_output_path
+        self.actions.pink_ik_cfg.controller.mesh_path = temp_urdf_meshes_output_path
+
+        self.teleop_devices = DevicesCfg(
+            devices={
+                "handtracking": OpenXRDeviceCfg(
+                    retargeters=[
+                        GR1T2RetargeterCfg(
+                            enable_visualization=True,
+                            # number of joints in both hands
+                            num_open_xr_hand_joints=2 * self.NUM_OPENXR_HAND_JOINTS,
+                            sim_device=self.sim.device,
+                            hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names,
+                        ),
+                    ],
+                    sim_device=self.sim.device,
+                    xr_cfg=self.xr,
+                ),
+            }
+        )