Migrates Pink IK to Newton branch

source/isaaclab/isaaclab/assets/articulation/articulation_cfg.py

@@ -57,5 +57,17 @@ class InitialStateCfg(AssetBaseCfg.InitialStateCfg):
     The soft joint position limits are accessible through the :attr:`ArticulationData.soft_joint_pos_limits` attribute.
     """
 
+    gravity_compensation: list[str] | None = None
+    """List of body name patterns for gravity compensation.
+
+    Each string is a regular expression pattern matching body names that should have
+    gravity compensation applied (full compensation, i.e., no gravity effect).
+    Defaults to None (no compensation).
+    """
+
+    gravity_compensation_root_link_index: str | None = None
+    """Root link index for gravity compensation. Defaults to None (no compensation).
+    """
+
     actuators: dict[str, ActuatorBaseCfg] = MISSING
     """Actuators for the robot with corresponding joint names."""

source/isaaclab/isaaclab/cloner/cloner_utils.py

@@ -67,7 +67,9 @@ def clone_from_template(stage: Usd.Stage, num_clones: int, template_clone_cfg: T
         # If all prototypes map to env_0, clone whole env_0 to all envs; else clone per-object
         if torch.all(proto_idx == 0):
             # args: src_paths, dest_paths, env_ids, mask
-            replicate_args = [clone_path_fmt.format(0)], [clone_path_fmt], world_indices, clone_masking
+            # When cloning entire env_0, we use a single source so need a single-row mask
+            single_source_mask = torch.ones((1, num_clones), device=cfg.device, dtype=torch.bool)
+            replicate_args = [clone_path_fmt.format(0)], [clone_path_fmt], world_indices, single_source_mask
             get_pos = lambda path: stage.GetPrimAtPath(path).GetAttribute("xformOp:translate").Get()  # noqa: E731
             positions = torch.tensor([get_pos(clone_path_fmt.format(i)) for i in world_indices])
             if cfg.clone_physics:

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

@@ -0,0 +1,13 @@
+# Copyright (c) 2022-2026, 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/local_frame_task.py

@@ -0,0 +1,116 @@
+# Copyright (c) 2022-2026, 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_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,
+        frame: str,
+        base_link_frame_name: str,
+        position_cost: float | Sequence[float],
+        orientation_cost: float | Sequence[float],
+        lm_damping: float = 0.0,
+        gain: float = 1.0,
+    ):
+        """
+        Initialize the LocalFrameTask with configuration.
+
+        This task computes pose errors in a local (custom) frame rather than the world frame,
+        allowing for more flexible control strategies where the reference frame can be
+        specified independently.
+
+        Args:
+            frame: Name of the frame to control (end-effector or target frame).
+            base_link_frame_name: Name of the base link frame used as reference frame
+                for computing transforms and errors.
+            position_cost: Cost weight(s) for position error. Can be a single float
+                for uniform weighting or a sequence of 3 floats for per-axis weighting.
+            orientation_cost: Cost weight(s) for orientation error. Can be a single float
+                for uniform weighting or a sequence of 3 floats for per-axis weighting.
+            lm_damping: Levenberg-Marquardt damping factor for numerical stability.
+                Defaults to 0.0 (no damping).
+            gain: Task gain factor that scales the overall task contribution.
+                Defaults to 1.0.
+        """
+        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.
+
+        Args:
+            transform_target_to_world: Transform from the task target frame to
+                the world frame.
+        """
+        self.transform_target_to_base = transform_target_to_base.copy()
+
+    def set_target_from_configuration(self, configuration: PinkKinematicsConfiguration) -> None:
+        """Set task target pose from a robot configuration.
+
+        Args:
+            configuration: Robot configuration.
+        """
+        if not isinstance(configuration, PinkKinematicsConfiguration):
+            raise ValueError("configuration must be a PinkKinematicsConfiguration")
+        self.set_target(configuration.get_transform(self.frame, self.base_link_frame_name))
+
+    def compute_error(self, configuration: PinkKinematicsConfiguration) -> np.ndarray:
+        """
+        Compute the error between current and target pose in a local frame.
+        """
+        if not isinstance(configuration, PinkKinematicsConfiguration):
+            raise ValueError("configuration must be a PinkKinematicsConfiguration")
+        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)
+
+        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.
+
+        The task Jacobian :math:`J(q) \in \mathbb{R}^{6 \times n_v}` is the
+        derivative of the task error :math:`e(q) \in \mathbb{R}^6` with respect
+        to the configuration :math:`q`. The formula for the frame task is:
+
+        .. math::
+
+            J(q) = -\text{Jlog}_6(T_{tb}) {}_b J_{0b}(q)
+
+        The derivation of the formula for this Jacobian is detailed in
+        [Caron2023]_. See also
+        :func:`pink.tasks.task.Task.compute_jacobian` for more context on task
+        Jacobians.
+
+        Args:
+            configuration: Robot configuration :math:`q`.
+
+        Returns:
+            Jacobian matrix :math:`J`, expressed locally in the frame.
+        """
+        if self.transform_target_to_base is None:
+            raise Exception(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)
+        jacobian_in_frame = configuration.get_frame_jacobian(self.frame)
+        J = -pin.Jlog6(transform_frame_to_target) @ jacobian_in_frame
+        return J