diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 17cd64b4db39..bde83712b642 100644 --- a/CONTRIBUTORS.md +++ b/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 diff --git a/docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.jpg b/docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.jpg new file mode 100644 index 000000000000..1f99cb727419 Binary files /dev/null and b/docs/source/_static/tasks/manipulation/gr-1_pick_place_waist.jpg differ diff --git a/docs/source/api/lab/isaaclab.controllers.rst b/docs/source/api/lab/isaaclab.controllers.rst index 07adbfcc0cda..24bfa0b7f836 100644 --- a/docs/source/api/lab/isaaclab.controllers.rst +++ b/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 diff --git a/docs/source/overview/environments.rst b/docs/source/overview/environments.rst index 79e220a27f43..7e3e2668e7b6 100644 --- a/docs/source/overview/environments.rst +++ b/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 `__ .. |reach-ur10-link| replace:: `Isaac-Reach-UR10-v0 `__ @@ -154,6 +158,7 @@ for the lift-cube environment: .. |stack-cube-link| replace:: `Isaac-Stack-Cube-Franka-v0 `__ .. |stack-cube-bp-link| replace:: `Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0 `__ .. |gr1_pick_place-link| replace:: `Isaac-PickPlace-GR1T2-Abs-v0 `__ +.. |gr1_pp_waist-link| replace:: `Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0 `__ .. |cube-shadow-link| replace:: `Isaac-Repose-Cube-Shadow-Direct-v0 `__ .. |cube-shadow-ff-link| replace:: `Isaac-Repose-Cube-Shadow-OpenAI-FF-Direct-v0 `__ diff --git a/docs/source/overview/teleop_imitation.rst b/docs/source/overview/teleop_imitation.rst index a8c3c347f9b9..67cddf03ec18 100644 --- a/docs/source/overview/teleop_imitation.rst +++ b/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. diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 64f008c0aba1..d51360662a22 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [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" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 6bb60f6b2753..10ed470e018f 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,28 @@ 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) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/controllers/__init__.py b/source/isaaclab/isaaclab/controllers/__init__.py index ffc5a5fb9a77..6a5b884b78ac 100644 --- a/source/isaaclab/isaaclab/controllers/__init__.py +++ b/source/isaaclab/isaaclab/controllers/__init__.py @@ -15,3 +15,4 @@ from .differential_ik_cfg import DifferentialIKControllerCfg from .operational_space import OperationalSpaceController from .operational_space_cfg import OperationalSpaceControllerCfg +from .pink_ik import NullSpacePostureTask, PinkIKController, PinkIKControllerCfg diff --git a/source/isaaclab/isaaclab/controllers/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik.py deleted file mode 100644 index 8fff42247223..000000000000 --- a/source/isaaclab/isaaclab/controllers/pink_ik.py +++ /dev/null @@ -1,133 +0,0 @@ -# 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 implementation for IsaacLab. - -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. -""" - -import numpy as np -import torch - -from pink import solve_ik -from pink.configuration import Configuration -from pinocchio.robot_wrapper import RobotWrapper - -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 - """ - - def __init__(self, cfg: PinkIKControllerCfg, 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'). - """ - # Initialize the robot model from URDF and mesh files - self.robot_wrapper = RobotWrapper.BuildFromURDF(cfg.urdf_path, cfg.mesh_path, root_joint=None) - self.pink_configuration = Configuration( - self.robot_wrapper.model, self.robot_wrapper.data, self.robot_wrapper.q0 - ) - - # Set the default targets for each task from the configuration - for task in cfg.variable_input_tasks: - 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 - - # 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.cfg = cfg - self.device = device - - """ - Operations. - """ - - def reorder_array(self, input_array: list[float], reordering_array: list[int]) -> list[float]: - """Reorder the input array based on the provided ordering. - - Args: - input_array: The array to reorder. - reordering_array: The indices to use for reordering. - - Returns: - Reordered array. - """ - 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 - - def compute( - self, - curr_joint_pos: np.ndarray, - dt: float, - ) -> torch.Tensor: - """Compute the target joint positions based on current state and tasks. - - Args: - curr_joint_pos: The current joint positions. - dt: The time step for computing joint position changes. - - Returns: - The target joint positions as a tensor. - """ - # 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)) - - # Update Pink's robot configuration with the current joint positions - self.pink_configuration.update(joint_positions_pink) - - # 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" - ) - Delta_q = velocity * dt - except (AssertionError, Exception): - # 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." - ) - return torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32) - - # Discard the first 6 values (for root and universal joints) - pink_joint_angle_changes = Delta_q - - # 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), - device=self.device, - dtype=torch.float, - ) - - # 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_joint_pos, device=self.device, dtype=torch.float32) - ) - - return target_joint_pos diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py b/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py new file mode 100644 index 000000000000..005645f97985 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/__init__.py @@ -0,0 +1,13 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Pink IK controller package for IsaacLab. + +This package provides integration between Pink inverse kinematics solver and IsaacLab. +""" + +from .null_space_posture_task import NullSpacePostureTask +from .pink_ik import PinkIKController +from .pink_ik_cfg import PinkIKControllerCfg diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py new file mode 100644 index 000000000000..212071c904e8 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/null_space_posture_task.py @@ -0,0 +1,242 @@ +# 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 diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py new file mode 100644 index 000000000000..f37ebe163e19 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -0,0 +1,193 @@ +# 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 implementation for IsaacLab. + +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 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 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, robot_cfg: ArticulationCfg, device: str): + """Initialize the Pink IK Controller. + + Args: + 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) + self.pink_configuration = Configuration( + 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 + 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 = 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 + + def update_null_space_joint_targets(self, curr_joint_pos: np.ndarray): + """Update the null space joint targets. + + 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: + curr_joint_pos: The current joint positions of shape (num_joints,). + """ + for task in self.cfg.variable_input_tasks: + if isinstance(task, NullSpacePostureTask): + task.set_target(curr_joint_pos) + + def compute( + self, + curr_joint_pos: np.ndarray, + dt: float, + ) -> 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 of shape (num_joints,). + dt: The time step for computing joint position changes in seconds. + + Returns: + 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, 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) + + # 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", + safety_break=self.cfg.fail_on_joint_limit_violation, + ) + Delta_q = velocity * dt + 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" + f" positions.\nError: {e}" + ) + return torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32) + + # Discard the first 6 values (for root and universal joints) + pink_joint_angle_changes = Delta_q + + # Reorder the joint angle changes back to Isaac Lab conventions + joint_vel_isaac_lab = torch.tensor( + pink_joint_angle_changes[self.pink_to_isaac_lab_ordering], + device=self.device, + dtype=torch.float, + ) + + # 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_joint_pos, device=self.device, dtype=torch.float32) + ) + + return target_joint_pos diff --git a/source/isaaclab/isaaclab/controllers/pink_ik_cfg.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py similarity index 87% rename from source/isaaclab/isaaclab/controllers/pink_ik_cfg.py rename to source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py index 52bea14f6ccb..5add83a59168 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik_cfg.py +++ b/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.""" diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py index 6b7c412de7dc..834d23d955a0 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py +++ b/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. + """ diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py index e0d008834a52..f1e9fd7a819c 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_task_space_actions.py @@ -10,6 +10,8 @@ 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 @@ def __init__(self, cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg, env: Ma 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 @@ def pose_dim(self) -> int: @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 @@ def process_actions(self, actions: torch.Tensor): """ # 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 @@ def process_actions(self, actions: torch.Tensor): # 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 diff --git a/source/isaaclab/isaaclab/utils/string.py b/source/isaaclab/isaaclab/utils/string.py index e0bf5dc45780..43a2fa0b3106 100644 --- a/source/isaaclab/isaaclab/utils/string.py +++ b/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): diff --git a/source/isaaclab/test/controllers/simplified_test_robot.urdf b/source/isaaclab/test/controllers/simplified_test_robot.urdf new file mode 100644 index 000000000000..b66ce68324bb --- /dev/null +++ b/source/isaaclab/test/controllers/simplified_test_robot.urdf @@ -0,0 +1,191 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json b/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json new file mode 100644 index 000000000000..b033b95b81f6 --- /dev/null +++ b/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json @@ -0,0 +1,86 @@ +{ + "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 + } + } +} diff --git a/source/isaaclab/test/controllers/test_null_space_posture_task.py b/source/isaaclab/test/controllers/test_null_space_posture_task.py new file mode 100644 index 000000000000..97fc7221748a --- /dev/null +++ b/source/isaaclab/test/controllers/test_null_space_posture_task.py @@ -0,0 +1,339 @@ +# 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}" diff --git a/source/isaaclab/test/controllers/test_pink_ik.py b/source/isaaclab/test/controllers/test_pink_ik.py index a1b7993b9bde..3485f367e373 100644 --- a/source/isaaclab/test/controllers/test_pink_ik.py +++ b/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 @@ 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 @@ 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"]) diff --git a/source/isaaclab/test/utils/test_string.py b/source/isaaclab/test/utils/test_string.py index 501f73805810..f697509586b2 100644 --- a/source/isaaclab/test/utils/test_string.py +++ b/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 diff --git a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py b/source/isaaclab_assets/isaaclab_assets/robots/fourier.py index 42a2aa63885d..b2d87b1ee8f3 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/fourier.py +++ b/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 @@ }, ) """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.""" diff --git a/source/isaaclab_mimic/config/extension.toml b/source/isaaclab_mimic/config/extension.toml index 88cffbe962d2..5fa8eb214513 100644 --- a/source/isaaclab_mimic/config/extension.toml +++ b/source/isaaclab_mimic/config/extension.toml @@ -1,7 +1,7 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "1.0.12" +version = "1.0.13" # Description category = "isaaclab" diff --git a/source/isaaclab_mimic/docs/CHANGELOG.rst b/source/isaaclab_mimic/docs/CHANGELOG.rst index c4260d355d12..a234c5cd3ab8 100644 --- a/source/isaaclab_mimic/docs/CHANGELOG.rst +++ b/source/isaaclab_mimic/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ 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) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py index 89dc84590215..c782576c3630 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py @@ -11,6 +11,7 @@ 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 @@ 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", diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py index 5a70a3746191..6bede8c0897c 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env.py @@ -39,7 +39,7 @@ def target_eef_pose_to_action( target_eef_pose_dict: dict, gripper_action_dict: dict, action_noise_dict: dict | None = None, - env_id: int = 0, + env_id: int = 0, # Unused, but required to conform to interface ) -> torch.Tensor: """ Takes a target pose and gripper action for the end effector controller and returns an action @@ -49,7 +49,7 @@ def target_eef_pose_to_action( Args: target_eef_pose_dict: Dictionary of 4x4 target eef pose for each end-effector. gripper_action_dict: Dictionary of gripper actions for each end-effector. - noise: Noise to add to the action. If None, no noise is added. + 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: diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py index 2e53e20c89d2..14c0ebd2a9d9 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_mimic_env_cfg.py @@ -17,7 +17,7 @@ def __post_init__(self): super().__post_init__() # Override the existing values - self.datagen_config.name = "demo_src_gr1t2_demo_task_D0" + self.datagen_config.name = "gr1t2_pick_place_D0" self.datagen_config.generation_guarantee = True self.datagen_config.generation_keep_failed = False self.datagen_config.generation_num_trials = 1000 diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py new file mode 100644 index 000000000000..d5b033bf7371 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/pickplace_gr1t2_waist_enabled_mimic_env_cfg.py @@ -0,0 +1,111 @@ +# 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 diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index 289d07ed5b79..0299870aca2e 100644 --- a/source/isaaclab_tasks/config/extension.toml +++ b/source/isaaclab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.10.46" +version = "0.10.47" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 98e23db6c816..c33f645b33d2 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,25 @@ 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) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py index db926c6a162c..ff72798e0f4c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/__init__.py +++ b/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 @@ }, 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, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py index 554203a8b7c2..ed1f0f06130c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_base_env_cfg.py +++ b/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() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py index c430a194483d..8b35bf2c3cb9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/exhaustpipe_gr1t2_pink_ik_env_cfg.py +++ b/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 @@ def __post_init__(self): "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 @@ def __post_init__(self): 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 @@ def __post_init__(self): 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, ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py index a59bd6dfab3e..ffa7929c9539 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_base_env_cfg.py +++ b/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() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py index fd39e47df7ae..d18b4866d155 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/nutpour_gr1t2_pink_ik_env_cfg.py +++ b/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 @@ def __post_init__(self): "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 @@ def __post_init__(self): 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 @@ def __post_init__(self): 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, ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index f19bc3629f60..4d0871fcb8a0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/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 . 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 @@ def __post_init__(self): 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, ), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py new file mode 100644 index 000000000000..636f347109f4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_waist_enabled_env_cfg.py @@ -0,0 +1,91 @@ +# 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, + ), + } + )