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_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/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,
+                ),
+            }
+        )