features: Implement locomanipulation system with improved IK control

- Added locomotion and locomanipulation policies with agile observation
- Enhanced IK controller with gravity compensation and tuned gains
- Expanded test coverage for ik
- Added unit tests for new pink kinematics config class, and new custom
  pink frame task

Author: michaellin6

.gitattributes

@@ -10,3 +10,4 @@
 *.pt filter=lfs diff=lfs merge=lfs -text
 *.jit filter=lfs diff=lfs merge=lfs -text
 *.hdf5 filter=lfs diff=lfs merge=lfs -text
+*.npz filter=lfs diff=lfs merge=lfs -text

source/isaaclab/docs/CHANGELOG.rst

@@ -7,7 +7,7 @@ Changelog
 Added
 ^^^^^
 
-* Added teleoperation environments for Unitree G1. This includes an environment with lower body fixed and upper body 
+* Added teleoperation environments for Unitree G1. This includes an environment with lower body fixed and upper body
   controller by IK, and an environment with the lower body controlled by a policy and the upper body controlled by IK.
 
 

source/isaaclab/isaaclab/controllers/local_frame_task.py

@@ -1,24 +1,30 @@
-from typing import Union, Sequence
+# 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
+from collections.abc import Sequence
 
 import pinocchio as pin
-
-from pink.tasks.frame_task import FrameTask
 from pink.exceptions import TargetNotSet
+from pink.tasks.frame_task import FrameTask
 
 from isaaclab.controllers.pink_kinematics_configuration import PinkKinematicsConfiguration
 
+
 class LocalFrameTask(FrameTask):
     """
     A task that computes error in a local (custom) frame.
     Inherits from FrameTask but overrides compute_error.
     """
 
-    def __init__(self, 
+    def __init__(
+        self,
         frame: str,
         base_link_frame_name: str,
-        position_cost: Union[float, Sequence[float]],
-        orientation_cost: Union[float, Sequence[float]],
+        position_cost: float | Sequence[float],
+        orientation_cost: float | Sequence[float],
         lm_damping: float = 0.0,
         gain: float = 1.0,
     ):
@@ -30,6 +36,7 @@ def __init__(self,
         """
         super().__init__(frame, position_cost, orientation_cost, lm_damping, gain)
         self.base_link_frame_name = base_link_frame_name
+        self.transform_target_to_base = None
 
     def set_target(self, transform_target_to_base: pin.SE3) -> None:
         """Set task target pose in the world frame.
@@ -40,9 +47,7 @@ def set_target(self, transform_target_to_base: pin.SE3) -> None:
         """
         self.transform_target_to_base = transform_target_to_base.copy()
 
-    def set_target_from_configuration(
-        self, configuration: PinkKinematicsConfiguration
-    ) -> None:
+    def set_target_from_configuration(self, configuration: PinkKinematicsConfiguration) -> None:
         """Set task target pose from a robot configuration.
 
         Args:
@@ -61,18 +66,12 @@ def compute_error(self, configuration: PinkKinematicsConfiguration) -> np.ndarra
         if self.transform_target_to_base is None:
             raise ValueError(f"no target set for frame '{self.frame}'")
 
-        transform_frame_to_base = configuration.get_transform(
-            self.frame,
-            self.base_link_frame_name
-        )
-        transform_target_to_frame = transform_frame_to_base.actInv(
-            self.transform_target_to_base
-        )
+        transform_frame_to_base = configuration.get_transform(self.frame, self.base_link_frame_name)
+        transform_target_to_frame = transform_frame_to_base.actInv(self.transform_target_to_base)
 
         error_in_frame: np.ndarray = pin.log(transform_target_to_frame).vector
         return error_in_frame
 
-
     def compute_jacobian(self, configuration: PinkKinematicsConfiguration) -> np.ndarray:
         r"""Compute the frame task Jacobian.
 
@@ -97,13 +96,8 @@ def compute_jacobian(self, configuration: PinkKinematicsConfiguration) -> np.nda
         """
         if self.transform_target_to_base is None:
             raise TargetNotSet(f"no target set for frame '{self.frame}'")
-        transform_frame_to_base = configuration.get_transform(
-            self.frame,
-            self.base_link_frame_name
-        )
-        transform_frame_to_target = self.transform_target_to_base.actInv(
-            transform_frame_to_base
-        )
+        transform_frame_to_base = configuration.get_transform(self.frame, self.base_link_frame_name)
+        transform_frame_to_target = self.transform_target_to_base.actInv(transform_frame_to_base)
         jacobian_in_frame = configuration.get_frame_jacobian(self.frame)
         J = -pin.Jlog6(transform_frame_to_target) @ jacobian_in_frame
         return J

source/isaaclab/isaaclab/controllers/pink_ik.py

@@ -13,10 +13,12 @@
 import torch
 
 from pink import solve_ik
+
 from isaaclab.assets import ArticulationCfg
 from isaaclab.controllers.null_space_posture_task import NullSpacePostureTask
 from isaaclab.controllers.pink_kinematics_configuration import PinkKinematicsConfiguration
 from isaaclab.utils.string import resolve_matching_names_values
+
 from .pink_ik_cfg import PinkIKControllerCfg
 
 
@@ -77,7 +79,7 @@ def _setup_joint_ordering_mappings(self):
         """Setup joint ordering mappings between Isaac Lab and Pink conventions."""
         pink_joint_names = self.pink_configuration.all_joint_names_pinocchio_order
         isaac_lab_joint_names = self.cfg.all_joint_names
-        
+
         # Type assertions for linter clarity
         assert pink_joint_names is not None, "pink_joint_names should not be None"
         assert isaac_lab_joint_names is not None, "isaac_lab_joint_names should not be None"
@@ -93,7 +95,7 @@ def _setup_joint_ordering_mappings(self):
         # Create reordering arrays for controlled joints only
         pink_controlled_joint_names = self.pink_configuration.controlled_joint_names_pinocchio_order
         isaac_lab_controlled_joint_names = self.cfg.controlled_joint_names
-        
+
         # Type assertions for linter clarity
         assert pink_controlled_joint_names is not None, "pink_controlled_joint_names should not be None"
         assert isaac_lab_controlled_joint_names is not None, "isaac_lab_controlled_joint_names should not be None"
@@ -105,7 +107,6 @@ def _setup_joint_ordering_mappings(self):
             [pink_controlled_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_controlled_joint_names]
         )
 
-
     """
     Operations.
     """
@@ -168,9 +169,9 @@ def compute(
         # Solve IK using Pink's solver
         try:
             velocity = solve_ik(
-                self.pink_configuration, 
-                self.cfg.variable_input_tasks + self.cfg.fixed_input_tasks, 
-                dt, 
+                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,
             )
@@ -194,8 +195,7 @@ def compute(
 
         # Add the velocity changes to the current joint positions to get the target joint positions
         target_joint_pos = torch.add(
-            joint_vel_isaac_lab, 
-            torch.tensor(curr_controlled_joint_pos, device=self.device, dtype=torch.float32)
+            joint_vel_isaac_lab, torch.tensor(curr_controlled_joint_pos, device=self.device, dtype=torch.float32)
         )
 
         return target_joint_pos

source/isaaclab/isaaclab/controllers/pink_ik_cfg.py

@@ -70,7 +70,7 @@ class PinkIKControllerCfg:
     """Fail the IK solver if a joint limit is violated."""
     hand_rotational_offset: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)
     """Rotational offset quaternion (w, x, y, z) between USD hand orientation and nominal orientation.
-    
+
     The nominal orientation is defined as thumbs pointing up and fingers pointing forward.
     This offset is applied to align the USD hand orientation with the expected nominal pose.
     Default is identity quaternion (1, 0, 0, 0) representing no rotation.

source/isaaclab/isaaclab/controllers/pink_kinematics_configuration.py

@@ -1,11 +1,15 @@
-from typing import Optional
+# 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 pinocchio.robot_wrapper import RobotWrapper
 
+import pinocchio as pin
 from pink.configuration import Configuration
 from pink.exceptions import FrameNotFound
+from pinocchio.robot_wrapper import RobotWrapper
+
 
 class PinkKinematicsConfiguration(Configuration):
     """
@@ -64,14 +68,21 @@ def __init__(
             if joint_name not in self._controlled_joint_names:
                 joints_to_lock.append(self.full_model.getJointId(joint_name))
 
-        self.controlled_model = pin.buildReducedModel(self.full_model, joints_to_lock, self.full_q)
-        self.controlled_data = self.controlled_model.createData()
-        self.controlled_q = self.full_q[self._controlled_joint_indices]
+
+        if len(joints_to_lock) == 0:
+            # No joints to lock, controlled model is the same as full model
+            self.controlled_model = self.full_model
+            self.controlled_data = self.full_data
+            self.controlled_q = self.full_q
+        else:
+            self.controlled_model = pin.buildReducedModel(self.full_model, joints_to_lock, self.full_q)
+            self.controlled_data = self.controlled_model.createData()
+            self.controlled_q = self.full_q[self._controlled_joint_indices]
 
         # Pink will should only have the controlled model
         super().__init__(self.controlled_model, self.controlled_data, self.controlled_q, copy_data, forward_kinematics)
 
-    def update(self, q: Optional[np.ndarray] = None) -> None:
+    def update(self, q: np.ndarray | None = None) -> None:
         """Update configuration to a new vector.
 
         Calling this function runs forward kinematics and computes
@@ -84,6 +95,10 @@ def update(self, q: Optional[np.ndarray] = None) -> None:
             raise ValueError("q must have the same length as the number of joints in the model")
         if q is not None:
             super().update(q[self._controlled_joint_indices])
+
+            q_readonly = q.copy()
+            q_readonly.setflags(write=False)
+            self.full_q = q_readonly
             pin.computeJointJacobians(self.full_model, self.full_data, q)
             pin.updateFramePlacements(self.full_model, self.full_data)
         else:
@@ -121,14 +136,12 @@ def get_frame_jacobian(self, frame: str) -> np.ndarray:
         if not self.full_model.existFrame(frame):
             raise FrameNotFound(frame, self.full_model.frames)
         frame_id = self.full_model.getFrameId(frame)
-        J: np.ndarray = pin.getFrameJacobian(
-            self.full_model, self.full_data, frame_id, pin.ReferenceFrame.LOCAL
-        )
+        J: np.ndarray = pin.getFrameJacobian(self.full_model, self.full_data, frame_id, pin.ReferenceFrame.LOCAL)
         return J[:, self._controlled_joint_indices]
 
     def get_transform_frame_to_world(self, frame: str) -> pin.SE3:
         """Get the pose of a frame in the current configuration.
-        We override this method from the super class to solve the issue that in the default 
+        We override this method from the super class to solve the issue that in the default
         Pink implementation, the frame placements do not take into account the non-controlled joints
         being not at initial pose (which is a bad assumption when they are controlled by other controllers like a lower body controller).
 
@@ -155,4 +168,4 @@ def controlled_joint_names_pinocchio_order(self) -> list[str]:
     @property
     def all_joint_names_pinocchio_order(self) -> list[str]:
         """Get the names of all joints in the order of the pinocchio model."""
-        return self._all_joint_names
+        return self._all_joint_names

source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/data/configs/dex-retargeting/g1_hand_left_dexpilot.yml

@@ -1,3 +1,8 @@
+# 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
+
 retargeting:
   finger_tip_link_names:
   - thumb_tip

source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/data/configs/dex-retargeting/g1_hand_right_dexpilot.yml

@@ -1,3 +1,8 @@
+# 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
+
 retargeting:
   finger_tip_link_names:
   - thumb_tip

source/isaaclab/isaaclab/devices/teleop_device_factory.py

@@ -15,22 +15,18 @@
 from isaaclab.devices.gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg
 from isaaclab.devices.keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg
 from isaaclab.devices.openxr.retargeters import (
-    GR1T2Retargeter,
-    GR1T2RetargeterCfg,
-    G1UpperBodyRetargeter,
-    G1UpperBodyRetargeterCfg,
     G1LowerBodyRetargeter,
     G1LowerBodyRetargeterCfg,
+    G1UpperBodyRetargeter,
+    G1UpperBodyRetargeterCfg,
+    GR1T2Retargeter,
+    GR1T2RetargeterCfg,
     GripperRetargeter,
     GripperRetargeterCfg,
     Se3AbsRetargeter,
     Se3AbsRetargeterCfg,
     Se3RelRetargeter,
     Se3RelRetargeterCfg,
-    G1UpperBodyRetargeter,
-    G1UpperBodyRetargeterCfg,
-    G1LowerBodyRetargeter,
-    G1LowerBodyRetargeterCfg,
 )
 from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg
 from isaaclab.devices.spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg