diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index ee6200de8694..e33f2718db7d 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -111,6 +111,7 @@ Guidelines for modifications: * Özhan Özen * Patrick Yin * Peter Du +* Philipp Reist * Pulkit Goyal * Qian Wan * Qinxi Yu @@ -120,6 +121,7 @@ Guidelines for modifications: * Ritvik Singh * Rosario Scalise * Ryley McCarroll +* Sergey Grizan * Shafeef Omar * Shaoshu Su * Shaurya Dewan diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 859287560a84..3b2f016017d9 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -140,7 +140,7 @@ Pre-recorded demonstrations ^^^^^^^^^^^^^^^^^^^^^^^^^^^ We provide a pre-recorded ``dataset.hdf5`` containing 10 human demonstrations for ``Isaac-Stack-Cube-Franka-IK-Rel-v0`` -`here `__. +here: `[Franka Dataset] `__. This dataset may be downloaded and used in the remaining tutorial steps if you do not wish to collect your own demonstrations. .. note:: @@ -451,7 +451,7 @@ Generate the dataset ^^^^^^^^^^^^^^^^^^^^ If you skipped the prior collection and annotation step, download the pre-recorded annotated dataset ``dataset_annotated_gr1.hdf5`` from -`here `__. +here: `[Annotated GR1 Dataset] `_. Place the file under ``IsaacLab/datasets`` and run the following command to generate a new dataset with 1000 demonstrations. .. code:: bash @@ -508,13 +508,132 @@ Visualize the results of the trained policy by running the following command, us The trained policy performing the pick and place task in Isaac Lab. -Demo 2: Visuomotor Policy for a Humanoid Robot +Demo 2: Data Generation and Policy Training for Humanoid Robot Locomanipulation with Unitree G1 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +In this demo, we showcase the integration of locomotion and manipulation capabilities within a single humanoid robot system. +This locomanipulation environment enables data collection for complex tasks that combine navigation and object manipulation. +The demonstration follows a multi-step process: first, it generates pick and place tasks similar to Demo 1, then introduces +a navigation component that uses specialized scripts to generate scenes where the humanoid robot must move from point A to point B. +The robot picks up an object at the initial location (point A) and places it at the target destination (point B). + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/locomanipulation-g-1_steering_wheel_pick_place.gif + :width: 100% + :align: center + :alt: G1 humanoid robot with locomanipulation performing a pick and place task + :figclass: align-center + +Generate the manipulation dataset +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The same data generation and policy training steps from Demo 1.0 can be applied to the G1 humanoid robot with locomanipulation capabilities. +This demonstration shows how to train a G1 robot to perform pick and place tasks with full-body locomotion and manipulation. + +The process follows the same workflow as Demo 1.0, but uses the ``Isaac-PickPlace-Locomanipulation-G1-Abs-v0`` task environment. + +Follow the same data collection, annotation, and generation process as demonstrated in Demo 1.0, but adapted for the G1 locomanipulation task. + +.. hint:: + + If desired, data collection and annotation can be done using the same commands as the prior examples for validation of the dataset. + + The G1 robot with locomanipulation capabilities combines full-body locomotion with manipulation to perform pick and place tasks. + + **Note that the following commands are only for your reference and dataset validation purposes - they are not required for this demo.** + + To collect demonstrations: + + .. code:: bash + + ./isaaclab.sh -p scripts/tools/record_demos.py \ + --device cpu \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-v0 \ + --teleop_device handtracking \ + --dataset_file ./datasets/dataset_g1_locomanip.hdf5 \ + --num_demos 5 --enable_pinocchio + + You can replay the collected demonstrations by running: + + .. code:: bash + + ./isaaclab.sh -p scripts/tools/replay_demos.py \ + --device cpu \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-v0 \ + --dataset_file ./datasets/dataset_g1_locomanip.hdf5 --enable_pinocchio + + To annotate the demonstrations: + + .. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/annotate_demos.py \ + --device cpu \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-Mimic-v0 \ + --input_file ./datasets/dataset_g1_locomanip.hdf5 \ + --output_file ./datasets/dataset_annotated_g1_locomanip.hdf5 --enable_pinocchio + + +If you skipped the prior collection and annotation step, download the pre-recorded annotated dataset ``dataset_annotated_g1_locomanip.hdf5`` from +here: `[Annotated G1 Dataset] `_. +Place the file under ``IsaacLab/datasets`` and run the following command to generate a new dataset with 1000 demonstrations. + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/isaaclab_mimic/generate_dataset.py \ + --device cpu --headless --num_envs 20 --generation_num_trials 1000 --enable_pinocchio \ + --input_file ./datasets/dataset_annotated_g1_locomanip.hdf5 --output_file ./datasets/generated_dataset_g1_locomanip.hdf5 + + +Train a manipulation-only policy +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +At this point you can train a policy that only performs manipulation tasks using the generated dataset: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/train.py \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-v0 --algo bc \ + --normalize_training_actions \ + --dataset ./datasets/generated_dataset_g1_locomanip.hdf5 + +Visualize the results +^^^^^^^^^^^^^^^^^^^^^ + +Visualize the trained policy performance: + +.. code:: bash + + ./isaaclab.sh -p scripts/imitation_learning/robomimic/play.py \ + --device cpu \ + --enable_pinocchio \ + --task Isaac-PickPlace-Locomanipulation-G1-Abs-v0 \ + --num_rollouts 50 \ + --horizon 400 \ + --norm_factor_min \ + --norm_factor_max \ + --checkpoint /PATH/TO/desired_model_checkpoint.pth + +.. note:: + Change the ``NORM_FACTOR`` in the above command with the values generated in the training step. + +.. figure:: https://download.isaacsim.omniverse.nvidia.com/isaaclab/images/locomanipulation-g-1_steering_wheel_pick_place.gif + :width: 100% + :align: center + :alt: G1 humanoid robot performing a pick and place task + :figclass: align-center + + The trained policy performing the pick and place task in Isaac Lab. + +Generate the dataset with manipulation and point-to-point navigation +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + + +Demo 3: Visuomotor Policy for a Humanoid Robot ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Download the Dataset ^^^^^^^^^^^^^^^^^^^^ -Download the pre-generated dataset from `here `__ and place it under ``IsaacLab/datasets/generated_dataset_gr1_nut_pouring.hdf5``. +Download the pre-generated dataset from here: `[Generated GR1 Dataset] `__ and place it under ``IsaacLab/datasets/generated_dataset_gr1_nut_pouring.hdf5``. The dataset contains 1000 demonstrations of a humanoid robot performing a pouring/placing task that was generated using Isaac Lab Mimic for the ``Isaac-NutPour-GR1T2-Pink-IK-Abs-Mimic-v0`` task. diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index 021ee5ff80ff..e7615c1f6c8d 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -66,6 +66,7 @@ from isaaclab_tasks.utils import parse_env_cfg if args_cli.enable_pinocchio: + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 diff --git a/scripts/imitation_learning/robomimic/play.py b/scripts/imitation_learning/robomimic/play.py index 4b1476f6bea1..4cc327941d0d 100644 --- a/scripts/imitation_learning/robomimic/play.py +++ b/scripts/imitation_learning/robomimic/play.py @@ -70,6 +70,7 @@ if args_cli.enable_pinocchio: import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 from isaaclab_tasks.utils import parse_env_cfg diff --git a/scripts/imitation_learning/robomimic/train.py b/scripts/imitation_learning/robomimic/train.py index 945c1f40f980..718a18bcbca1 100644 --- a/scripts/imitation_learning/robomimic/train.py +++ b/scripts/imitation_learning/robomimic/train.py @@ -84,6 +84,7 @@ # Isaac Lab imports (needed so that environment is registered) import isaaclab_tasks # noqa: F401 +import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 diff --git a/scripts/tools/record_demos.py b/scripts/tools/record_demos.py index ec01ffaaf8db..5c1f776573cf 100644 --- a/scripts/tools/record_demos.py +++ b/scripts/tools/record_demos.py @@ -98,6 +98,7 @@ if args_cli.enable_pinocchio: import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 from collections.abc import Callable diff --git a/scripts/tools/replay_demos.py b/scripts/tools/replay_demos.py index 951220959b6f..c23e3a10d87c 100644 --- a/scripts/tools/replay_demos.py +++ b/scripts/tools/replay_demos.py @@ -66,6 +66,7 @@ if args_cli.enable_pinocchio: import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 import isaaclab_tasks # noqa: F401 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index a53b7e970cbc..7d8767e48bf1 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.15" +version = "0.45.16" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 91d5e1ab1ed4..b9714ffb3bcd 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.45.16 (2025-08-27) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added teleoperation environments for Unitree G1. This includes an environment with lower body fixed and upper body + controlled by IK, and an environment with the lower body controlled by a policy and the upper body controlled by IK. + + 0.45.15 (2025-09-05) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py index 145a69dfc85f..99b2f76abfa2 100644 --- a/source/isaaclab/isaaclab/assets/articulation/articulation_data.py +++ b/source/isaaclab/isaaclab/assets/articulation/articulation_data.py @@ -151,8 +151,9 @@ def update(self, dt: float): default_inertia: torch.Tensor = None """Default inertia for all the bodies in the articulation. Shape is (num_instances, num_bodies, 9). - The inertia is the inertia tensor relative to the center of mass frame. The values are stored in - the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + The inertia tensor should be given with respect to the center of mass, expressed in the articulation links' actor frame. + The values are stored in the order :math:`[I_{xx}, I_{yx}, I_{zx}, I_{xy}, I_{yy}, I_{zy}, I_{xz}, I_{yz}, I_{zz}]`. + However, due to the symmetry of inertia tensors, row- and column-major orders are equivalent. This quantity is parsed from the USD schema at the time of initialization. """ diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py index 3aac87d324f0..ee83900376f6 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py @@ -112,8 +112,11 @@ def update(self, dt: float): default_inertia: torch.Tensor = None """Default inertia tensor read from the simulation. Shape is (num_instances, 9). - The inertia is the inertia tensor relative to the center of mass frame. The values are stored in - the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + The inertia tensor should be given with respect to the center of mass, expressed in the rigid body's actor frame. + The values are stored in the order :math:`[I_{xx}, I_{yx}, I_{zx}, I_{xy}, I_{yy}, I_{zy}, I_{xz}, I_{yz}, I_{zz}]`. + However, due to the symmetry of inertia tensors, row- and column-major orders are equivalent. + + This quantity is parsed from the USD schema at the time of initialization. """ ## diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py index 897679f75aa5..328010bb14f6 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py @@ -118,8 +118,11 @@ def update(self, dt: float): default_inertia: torch.Tensor = None """Default object inertia tensor read from the simulation. Shape is (num_instances, num_objects, 9). - The inertia is the inertia tensor relative to the center of mass frame. The values are stored in - the order :math:`[I_{xx}, I_{xy}, I_{xz}, I_{yx}, I_{yy}, I_{yz}, I_{zx}, I_{zy}, I_{zz}]`. + The inertia tensor should be given with respect to the center of mass, expressed in the rigid body's actor frame. + The values are stored in the order :math:`[I_{xx}, I_{yx}, I_{zx}, I_{xy}, I_{yy}, I_{zy}, I_{xz}, I_{yz}, I_{zz}]`. + However, due to the symmetry of inertia tensors, row- and column-major orders are equivalent. + + This quantity is parsed from the USD schema at the time of initialization. """ ## diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/local_frame_task.py b/source/isaaclab/isaaclab/controllers/pink_ik/local_frame_task.py new file mode 100644 index 000000000000..e46174bcaa50 --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/local_frame_task.py @@ -0,0 +1,116 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +from collections.abc import Sequence + +import pinocchio as pin +from pink.tasks.frame_task import FrameTask + +from .pink_kinematics_configuration import PinkKinematicsConfiguration + + +class LocalFrameTask(FrameTask): + """ + A task that computes error in a local (custom) frame. + Inherits from FrameTask but overrides compute_error. + """ + + def __init__( + self, + frame: str, + base_link_frame_name: str, + position_cost: float | Sequence[float], + orientation_cost: float | Sequence[float], + lm_damping: float = 0.0, + gain: float = 1.0, + ): + """ + Initialize the LocalFrameTask with configuration. + + This task computes pose errors in a local (custom) frame rather than the world frame, + allowing for more flexible control strategies where the reference frame can be + specified independently. + + Args: + frame: Name of the frame to control (end-effector or target frame). + base_link_frame_name: Name of the base link frame used as reference frame + for computing transforms and errors. + position_cost: Cost weight(s) for position error. Can be a single float + for uniform weighting or a sequence of 3 floats for per-axis weighting. + orientation_cost: Cost weight(s) for orientation error. Can be a single float + for uniform weighting or a sequence of 3 floats for per-axis weighting. + lm_damping: Levenberg-Marquardt damping factor for numerical stability. + Defaults to 0.0 (no damping). + gain: Task gain factor that scales the overall task contribution. + Defaults to 1.0. + """ + super().__init__(frame, position_cost, orientation_cost, lm_damping, gain) + self.base_link_frame_name = base_link_frame_name + self.transform_target_to_base = None + + def set_target(self, transform_target_to_base: pin.SE3) -> None: + """Set task target pose in the world frame. + + Args: + transform_target_to_world: Transform from the task target frame to + the world frame. + """ + self.transform_target_to_base = transform_target_to_base.copy() + + def set_target_from_configuration(self, configuration: PinkKinematicsConfiguration) -> None: + """Set task target pose from a robot configuration. + + Args: + configuration: Robot configuration. + """ + if not isinstance(configuration, PinkKinematicsConfiguration): + raise ValueError("configuration must be a PinkKinematicsConfiguration") + self.set_target(configuration.get_transform(self.frame, self.base_link_frame_name)) + + def compute_error(self, configuration: PinkKinematicsConfiguration) -> np.ndarray: + """ + Compute the error between current and target pose in a local frame. + """ + if not isinstance(configuration, PinkKinematicsConfiguration): + raise ValueError("configuration must be a PinkKinematicsConfiguration") + if self.transform_target_to_base is None: + raise ValueError(f"no target set for frame '{self.frame}'") + + transform_frame_to_base = configuration.get_transform(self.frame, self.base_link_frame_name) + transform_target_to_frame = transform_frame_to_base.actInv(self.transform_target_to_base) + + error_in_frame: np.ndarray = pin.log(transform_target_to_frame).vector + return error_in_frame + + def compute_jacobian(self, configuration: PinkKinematicsConfiguration) -> np.ndarray: + r"""Compute the frame task Jacobian. + + The task Jacobian :math:`J(q) \in \mathbb{R}^{6 \times n_v}` is the + derivative of the task error :math:`e(q) \in \mathbb{R}^6` with respect + to the configuration :math:`q`. The formula for the frame task is: + + .. math:: + + J(q) = -\text{Jlog}_6(T_{tb}) {}_b J_{0b}(q) + + The derivation of the formula for this Jacobian is detailed in + [Caron2023]_. See also + :func:`pink.tasks.task.Task.compute_jacobian` for more context on task + Jacobians. + + Args: + configuration: Robot configuration :math:`q`. + + Returns: + Jacobian matrix :math:`J`, expressed locally in the frame. + """ + if self.transform_target_to_base is None: + raise Exception(f"no target set for frame '{self.frame}'") + transform_frame_to_base = configuration.get_transform(self.frame, self.base_link_frame_name) + transform_frame_to_target = self.transform_target_to_base.actInv(transform_frame_to_base) + jacobian_in_frame = configuration.get_frame_jacobian(self.frame) + J = -pin.Jlog6(transform_frame_to_target) @ jacobian_in_frame + return J diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py index 6bb4228e4e87..344244d141b9 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik.py @@ -19,14 +19,12 @@ 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 +from .pink_kinematics_configuration import PinkKinematicsConfiguration if TYPE_CHECKING: from .pink_ik_cfg import PinkIKControllerCfg @@ -47,7 +45,9 @@ class PinkIKController: Pink IK Solver: https://github.com/stephane-caron/pink """ - def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str): + def __init__( + self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str, controlled_joint_indices: list[int] + ): """Initialize the Pink IK Controller. Args: @@ -56,14 +56,28 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: 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'). + controlled_joint_indices: A list of joint indices in the USD asset controlled by the Pink IK controller. Raises: - KeyError: When Pink joint names cannot be matched to robot configuration joint positions. + ValueError: When joint_names or all_joint_names are not provided in the configuration. """ - # 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 + if cfg.joint_names is None: + raise ValueError("joint_names must be provided in the configuration") + if cfg.all_joint_names is None: + raise ValueError("all_joint_names must be provided in the configuration") + + self.cfg = cfg + self.device = device + self.controlled_joint_indices = controlled_joint_indices + + # Validate consistency between controlled_joint_indices and configuration + self._validate_consistency(cfg, controlled_joint_indices) + + # Initialize the Kinematics model used by pink IK to control robot + self.pink_configuration = PinkKinematicsConfiguration( + urdf_path=cfg.urdf_path, + mesh_path=cfg.mesh_path, + controlled_joint_names=cfg.joint_names, ) # Find the initial joint positions by matching Pink's joint names to robot_cfg.init_state.joint_pos, @@ -73,16 +87,11 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: 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( + indices, _, 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) + self.init_joint_positions = np.zeros(len(pink_joint_names)) + self.init_joint_positions[indices] = np.array(values) # Set the default targets for each task from the configuration for task in cfg.variable_input_tasks: @@ -94,27 +103,75 @@ def __init__(self, cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: 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" + # Create joint ordering mappings + self._setup_joint_ordering_mappings() - # 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) + def _validate_consistency(self, cfg: PinkIKControllerCfg, controlled_joint_indices: list[int]) -> None: + """Validate consistency between controlled_joint_indices and controller configuration. + + Args: + cfg: The Pink IK controller configuration. + controlled_joint_indices: List of joint indices in Isaac Lab joint space. + + Raises: + ValueError: If any consistency checks fail. + """ + # Check: Length consistency + if cfg.joint_names is None: + raise ValueError("cfg.joint_names cannot be None") + if len(controlled_joint_indices) != len(cfg.joint_names): + raise ValueError( + f"Length mismatch: controlled_joint_indices has {len(controlled_joint_indices)} elements " + f"but cfg.joint_names has {len(cfg.joint_names)} elements" + ) + + # Check: Joint name consistency - verify that the indices point to the expected joint names + actual_joint_names = [cfg.all_joint_names[idx] for idx in controlled_joint_indices] + if actual_joint_names != cfg.joint_names: + mismatches = [] + for i, (actual, expected) in enumerate(zip(actual_joint_names, cfg.joint_names)): + if actual != expected: + mismatches.append( + f"Index {i}: index {controlled_joint_indices[i]} points to '{actual}' but expected '{expected}'" + ) + if mismatches: + raise ValueError( + "Joint name mismatch between controlled_joint_indices and cfg.joint_names:\n" + + "\n".join(mismatches) + ) - # Create reordering arrays for joint indices + def _setup_joint_ordering_mappings(self): + """Setup joint ordering mappings between Isaac Lab and Pink conventions.""" + pink_joint_names = self.pink_configuration.all_joint_names_pinocchio_order + isaac_lab_joint_names = self.cfg.all_joint_names + + if pink_joint_names is None: + raise ValueError("pink_joint_names should not be None") + if isaac_lab_joint_names is None: + raise ValueError("isaac_lab_joint_names should not be None") + + # Create reordering arrays for all joints self.isaac_lab_to_pink_ordering = np.array( - [self.isaac_lab_joint_names.index(pink_joint) for pink_joint in self.pink_joint_names] + [isaac_lab_joint_names.index(pink_joint) for pink_joint in 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] + [pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_joint_names] ) + # Create reordering arrays for controlled joints only + pink_controlled_joint_names = self.pink_configuration.controlled_joint_names_pinocchio_order + isaac_lab_controlled_joint_names = self.cfg.joint_names - self.cfg = cfg - self.device = device + if pink_controlled_joint_names is None: + raise ValueError("pink_controlled_joint_names should not be None") + if isaac_lab_controlled_joint_names is None: + raise ValueError("isaac_lab_controlled_joint_names should not be None") + + self.isaac_lab_to_pink_controlled_ordering = np.array( + [isaac_lab_controlled_joint_names.index(pink_joint) for pink_joint in pink_controlled_joint_names] + ) + self.pink_to_isaac_lab_controlled_ordering = np.array( + [pink_controlled_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_controlled_joint_names] + ) def update_null_space_joint_targets(self, curr_joint_pos: np.ndarray): """Update the null space joint targets. @@ -149,13 +206,16 @@ def compute( 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. """ + # Get the current controlled joint positions + curr_controlled_joint_pos = [curr_joint_pos[i] for i in self.controlled_joint_indices] + # 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 + # Solve IK using Pink's solver try: velocity = solve_ik( self.pink_configuration, @@ -164,7 +224,7 @@ def compute( solver="osqp", safety_break=self.cfg.fail_on_joint_limit_violation, ) - Delta_q = velocity * dt + joint_angle_changes = 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 @@ -178,21 +238,18 @@ def compute( from isaaclab.ui.xr_widgets import XRVisualization XRVisualization.push_event("ik_error", {"error": 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 + return torch.tensor(curr_controlled_joint_pos, device=self.device, dtype=torch.float32) # 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], + joint_angle_changes[self.pink_to_isaac_lab_controlled_ordering], device=self.device, - dtype=torch.float, + dtype=torch.float32, ) # 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) + joint_vel_isaac_lab, torch.tensor(curr_controlled_joint_pos, device=self.device, dtype=torch.float32) ) return target_joint_pos diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py index d5f36a91523a..ed7e40b0c480 100644 --- a/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_ik_cfg.py @@ -46,6 +46,10 @@ class PinkIKControllerCfg: """ joint_names: list[str] | None = None + """A list of joint names in the USD asset controlled by the Pink IK controller. This is required because the joint naming conventions differ between USD and URDF files. + This value is currently designed to be automatically populated by the action term in a manager based environment.""" + + all_joint_names: list[str] | None = None """A list of joint names in the USD asset. This is required because the joint naming conventions differ between USD and URDF files. This value is currently designed to be automatically populated by the action term in a manager based environment.""" diff --git a/source/isaaclab/isaaclab/controllers/pink_ik/pink_kinematics_configuration.py b/source/isaaclab/isaaclab/controllers/pink_ik/pink_kinematics_configuration.py new file mode 100644 index 000000000000..5225a962268a --- /dev/null +++ b/source/isaaclab/isaaclab/controllers/pink_ik/pink_kinematics_configuration.py @@ -0,0 +1,179 @@ +# 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.exceptions import FrameNotFound +from pinocchio.robot_wrapper import RobotWrapper + + +class PinkKinematicsConfiguration(Configuration): + """ + A configuration class that maintains both a "controlled" (reduced) model and a "full" model. + + This class extends the standard Pink Configuration to allow for selective joint control: + - The "controlled" model/data/q represent the subset of joints being actively controlled (e.g., a kinematic chain or arm). + - The "full" model/data/q represent the complete robot, including all joints. + + This is useful for scenarios where only a subset of joints are being optimized or controlled, but full-model kinematics + (e.g., for collision checking, full-body Jacobians, or visualization) are still required. + + The class ensures that both models are kept up to date, and provides methods to update both the controlled and full + configurations as needed. + """ + + def __init__( + self, + controlled_joint_names: list[str], + urdf_path: str, + mesh_path: str = None, + copy_data: bool = True, + forward_kinematics: bool = True, + ): + """ + Initialize PinkKinematicsConfiguration. + + Args: + urdf_path (str): Path to the robot URDF file. + mesh_path (str): Path to the mesh files for the robot. + controlled_joint_names (list[str]): List of joint names to be actively controlled. + copy_data (bool, optional): If True, work on an internal copy of the input data. Defaults to True. + forward_kinematics (bool, optional): If True, compute forward kinematics from the configuration vector. Defaults to True. + + This constructor initializes the PinkKinematicsConfiguration, which maintains both a "controlled" (reduced) model and a "full" model. + The controlled model/data/q represent the subset of joints being actively controlled, while the full model/data/q represent the complete robot. + This is useful for scenarios where only a subset of joints are being optimized or controlled, but full-model kinematics are still required. + """ + self._controlled_joint_names = controlled_joint_names + + # Build robot model with all joints + if mesh_path: + self.robot_wrapper = RobotWrapper.BuildFromURDF(urdf_path, mesh_path) + else: + self.robot_wrapper = RobotWrapper.BuildFromURDF(urdf_path) + self.full_model = self.robot_wrapper.model + self.full_data = self.robot_wrapper.data + self.full_q = self.robot_wrapper.q0 + + # import pdb; pdb.set_trace() + self._all_joint_names = self.full_model.names.tolist()[1:] + # controlled_joint_indices: indices in all_joint_names for joints that are in controlled_joint_names, preserving all_joint_names order + self._controlled_joint_indices = [ + idx for idx, joint_name in enumerate(self._all_joint_names) if joint_name in self._controlled_joint_names + ] + + # Build the reduced model with only the controlled joints + joints_to_lock = [] + for joint_name in self._all_joint_names: + if joint_name not in self._controlled_joint_names: + joints_to_lock.append(self.full_model.getJointId(joint_name)) + + if len(joints_to_lock) == 0: + # No joints to lock, controlled model is the same as full model + self.controlled_model = self.full_model + self.controlled_data = self.full_data + self.controlled_q = self.full_q + else: + self.controlled_model = pin.buildReducedModel(self.full_model, joints_to_lock, self.full_q) + self.controlled_data = self.controlled_model.createData() + self.controlled_q = self.full_q[self._controlled_joint_indices] + + # Pink will should only have the controlled model + super().__init__(self.controlled_model, self.controlled_data, self.controlled_q, copy_data, forward_kinematics) + + def update(self, q: np.ndarray | None = None) -> None: + """Update configuration to a new vector. + + Calling this function runs forward kinematics and computes + collision-pair distances, if applicable. + + Args: + q: New configuration vector. + """ + if q is not None and len(q) != len(self._all_joint_names): + raise ValueError("q must have the same length as the number of joints in the model") + if q is not None: + super().update(q[self._controlled_joint_indices]) + + q_readonly = q.copy() + q_readonly.setflags(write=False) + self.full_q = q_readonly + pin.computeJointJacobians(self.full_model, self.full_data, q) + pin.updateFramePlacements(self.full_model, self.full_data) + else: + super().update() + pin.computeJointJacobians(self.full_model, self.full_data, self.full_q) + pin.updateFramePlacements(self.full_model, self.full_data) + + def get_frame_jacobian(self, frame: str) -> np.ndarray: + r"""Compute the Jacobian matrix of a frame velocity. + + Denoting our frame by :math:`B` and the world frame by :math:`W`, the + Jacobian matrix :math:`{}_B J_{WB}` is related to the body velocity + :math:`{}_B v_{WB}` by: + + .. math:: + + {}_B v_{WB} = {}_B J_{WB} \dot{q} + + Args: + frame: Name of the frame, typically a link name from the URDF. + + Returns: + Jacobian :math:`{}_B J_{WB}` of the frame. + + When the robot model includes a floating base + (pin.JointModelFreeFlyer), the configuration vector :math:`q` consists + of: + + - ``q[0:3]``: position in [m] of the floating base in the inertial + frame, formatted as :math:`[p_x, p_y, p_z]`. + - ``q[3:7]``: unit quaternion for the orientation of the floating base + in the inertial frame, formatted as :math:`[q_x, q_y, q_z, q_w]`. + - ``q[7:]``: joint angles in [rad]. + """ + if not self.full_model.existFrame(frame): + raise FrameNotFound(frame, self.full_model.frames) + frame_id = self.full_model.getFrameId(frame) + J: np.ndarray = pin.getFrameJacobian(self.full_model, self.full_data, frame_id, pin.ReferenceFrame.LOCAL) + return J[:, self._controlled_joint_indices] + + def get_transform_frame_to_world(self, frame: str) -> pin.SE3: + """Get the pose of a frame in the current configuration. + We override this method from the super class to solve the issue that in the default + Pink implementation, the frame placements do not take into account the non-controlled joints + being not at initial pose (which is a bad assumption when they are controlled by other controllers like a lower body controller). + + Args: + frame: Name of a frame, typically a link name from the URDF. + + Returns: + Current transform from the given frame to the world frame. + + Raises: + FrameNotFound: if the frame name is not found in the robot model. + """ + frame_id = self.full_model.getFrameId(frame) + try: + return self.full_data.oMf[frame_id].copy() + except IndexError as index_error: + raise FrameNotFound(frame, self.full_model.frames) from index_error + + def check_limits(self, tol: float = 1e-6, safety_break: bool = True) -> None: + """Check if limits are violated only if safety_break is enabled""" + if safety_break: + super().check_limits(tol, safety_break) + + @property + def controlled_joint_names_pinocchio_order(self) -> list[str]: + """Get the names of the controlled joints in the order of the pinocchio model.""" + return [self._all_joint_names[i] for i in self._controlled_joint_indices] + + @property + def all_joint_names_pinocchio_order(self) -> list[str]: + """Get the names of all joints in the order of the pinocchio model.""" + return self._all_joint_names diff --git a/source/isaaclab/isaaclab/controllers/utils.py b/source/isaaclab/isaaclab/controllers/utils.py index 70d627ac201a..b674b267acbb 100644 --- a/source/isaaclab/isaaclab/controllers/utils.py +++ b/source/isaaclab/isaaclab/controllers/utils.py @@ -9,6 +9,7 @@ """ import os +import re from isaacsim.core.utils.extensions import enable_extension @@ -98,3 +99,38 @@ def change_revolute_to_fixed(urdf_path: str, fixed_joints: list[str], verbose: b with open(urdf_path, "w") as file: file.write(content) + + +def change_revolute_to_fixed_regex(urdf_path: str, fixed_joints: list[str], verbose: bool = False): + """Change revolute joints to fixed joints in a URDF file. + + This function modifies a URDF file by changing specified revolute joints to fixed joints. + This is useful when you want to disable certain joints in a robot model. + + Args: + urdf_path: Path to the URDF file to modify. + fixed_joints: List of regular expressions matching joint names to convert from revolute to fixed. + verbose: Whether to print information about the changes being made. + """ + + with open(urdf_path) as file: + content = file.read() + + # Find all revolute joints in the URDF + revolute_joints = re.findall(r'', content) + + for joint in revolute_joints: + # Check if this joint matches any of the fixed joint patterns + should_fix = any(re.match(pattern, joint) for pattern in fixed_joints) + + if should_fix: + old_str = f'' + new_str = f'' + if verbose: + omni.log.warn(f"Replacing {joint} with fixed joint") + omni.log.warn(old_str) + omni.log.warn(new_str) + content = content.replace(old_str, new_str) + + with open(urdf_path, "w") as file: + file.write(content) diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py index b3a7401b522f..c687d287d258 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py @@ -5,6 +5,11 @@ """Retargeters for mapping input device data to robot commands.""" from .humanoid.fourier.gr1t2_retargeter import GR1T2Retargeter, GR1T2RetargeterCfg +from .humanoid.unitree.g1_lower_body_standing import G1LowerBodyStandingRetargeter, G1LowerBodyStandingRetargeterCfg +from .humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeter, + G1TriHandUpperBodyRetargeterCfg, +) from .manipulator.gripper_retargeter import GripperRetargeter, GripperRetargeterCfg from .manipulator.se3_abs_retargeter import Se3AbsRetargeter, Se3AbsRetargeterCfg from .manipulator.se3_rel_retargeter import Se3RelRetargeter, Se3RelRetargeterCfg diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py new file mode 100644 index 000000000000..9cf6ba09c426 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py @@ -0,0 +1,28 @@ +# 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 torch +from dataclasses import dataclass + +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg + + +@dataclass +class G1LowerBodyStandingRetargeterCfg(RetargeterCfg): + """Configuration for the G1 lower body standing retargeter.""" + + hip_height: float = 0.72 + """Height of the G1 robot hip in meters. The value is a fixed height suitable for G1 to do tabletop manipulation.""" + + +class G1LowerBodyStandingRetargeter(RetargeterBase): + """Provides lower body standing commands for the G1 robot.""" + + def __init__(self, cfg: G1LowerBodyStandingRetargeterCfg): + """Initialize the retargeter.""" + self.cfg = cfg + + def retarget(self, data: dict) -> torch.Tensor: + return torch.tensor([0.0, 0.0, 0.0, self.cfg.hip_height], device=self.cfg.sim_device) diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_left_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_left_dexpilot.yml new file mode 100644 index 000000000000..282b5d8438bf --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_left_dexpilot.yml @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +retargeting: + finger_tip_link_names: + - thumb_tip + - index_tip + - middle_tip + low_pass_alpha: 0.2 + scaling_factor: 1.0 + target_joint_names: + - left_hand_thumb_0_joint + - left_hand_thumb_1_joint + - left_hand_thumb_2_joint + - left_hand_middle_0_joint + - left_hand_middle_1_joint + - left_hand_index_0_joint + - left_hand_index_1_joint + type: DexPilot + urdf_path: /tmp/G1_left_hand.urdf + wrist_link_name: base_link diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_right_dexpilot.yml b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_right_dexpilot.yml new file mode 100644 index 000000000000..2629f9354fa6 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/data/configs/dex-retargeting/g1_hand_right_dexpilot.yml @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +retargeting: + finger_tip_link_names: + - thumb_tip + - index_tip + - middle_tip + low_pass_alpha: 0.2 + scaling_factor: 1.0 + target_joint_names: + - right_hand_thumb_0_joint + - right_hand_thumb_1_joint + - right_hand_thumb_2_joint + - right_hand_middle_0_joint + - right_hand_middle_1_joint + - right_hand_index_0_joint + - right_hand_index_1_joint + type: DexPilot + urdf_path: /tmp/G1_right_hand.urdf + wrist_link_name: base_link diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_dex_retargeting_utils.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_dex_retargeting_utils.py new file mode 100644 index 000000000000..4d8f58886b2f --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_dex_retargeting_utils.py @@ -0,0 +1,247 @@ +# 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 os +import torch +import yaml +from scipy.spatial.transform import Rotation as R + +import omni.log +from dex_retargeting.retargeting_config import RetargetingConfig + +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +# The index to map the OpenXR hand joints to the hand joints used +# in Dex-retargeting. +_HAND_JOINTS_INDEX = [1, 2, 3, 4, 5, 7, 8, 9, 10, 12, 13, 14, 15, 17, 18, 19, 20, 22, 23, 24, 25] + +# The transformation matrices to convert hand pose to canonical view. +_OPERATOR2MANO_RIGHT = np.array([ + [0, 0, 1], + [1, 0, 0], + [0, 1, 0], +]) + +_OPERATOR2MANO_LEFT = np.array([ + [0, 0, 1], + [1, 0, 0], + [0, 1, 0], +]) + +# G1 robot hand joint names - 2 fingers and 1 thumb configuration +_LEFT_HAND_JOINT_NAMES = [ + "left_hand_thumb_0_joint", # Thumb base (yaw axis) + "left_hand_thumb_1_joint", # Thumb middle (pitch axis) + "left_hand_thumb_2_joint", # Thumb tip + "left_hand_index_0_joint", # Index finger proximal + "left_hand_index_1_joint", # Index finger distal + "left_hand_middle_0_joint", # Middle finger proximal + "left_hand_middle_1_joint", # Middle finger distal +] + +_RIGHT_HAND_JOINT_NAMES = [ + "right_hand_thumb_0_joint", # Thumb base (yaw axis) + "right_hand_thumb_1_joint", # Thumb middle (pitch axis) + "right_hand_thumb_2_joint", # Thumb tip + "right_hand_index_0_joint", # Index finger proximal + "right_hand_index_1_joint", # Index finger distal + "right_hand_middle_0_joint", # Middle finger proximal + "right_hand_middle_1_joint", # Middle finger distal +] + + +class G1TriHandDexRetargeting: + """A class for hand retargeting with G1. + + Handles retargeting of OpenXRhand tracking data to G1 robot hand joint angles. + """ + + def __init__( + self, + hand_joint_names: list[str], + right_hand_config_filename: str = "g1_hand_right_dexpilot.yml", + left_hand_config_filename: str = "g1_hand_left_dexpilot.yml", + left_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_dexpilot_asset/G1_left_hand.urdf", + right_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_dexpilot_asset/G1_right_hand.urdf", + ): + """Initialize the hand retargeting. + + Args: + hand_joint_names: Names of hand joints in the robot model + right_hand_config_filename: Config file for right hand retargeting + left_hand_config_filename: Config file for left hand retargeting + """ + data_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), "data/")) + config_dir = os.path.join(data_dir, "configs/dex-retargeting") + + # Download urdf files from aws + local_left_urdf_path = retrieve_file_path(left_hand_urdf_path, force_download=True) + local_right_urdf_path = retrieve_file_path(right_hand_urdf_path, force_download=True) + + left_config_path = os.path.join(config_dir, left_hand_config_filename) + right_config_path = os.path.join(config_dir, right_hand_config_filename) + + # Update the YAML files with the correct URDF paths + self._update_yaml_with_urdf_path(left_config_path, local_left_urdf_path) + self._update_yaml_with_urdf_path(right_config_path, local_right_urdf_path) + + self._dex_left_hand = RetargetingConfig.load_from_file(left_config_path).build() + self._dex_right_hand = RetargetingConfig.load_from_file(right_config_path).build() + + self.left_dof_names = self._dex_left_hand.optimizer.robot.dof_joint_names + self.right_dof_names = self._dex_right_hand.optimizer.robot.dof_joint_names + self.dof_names = self.left_dof_names + self.right_dof_names + self.isaac_lab_hand_joint_names = hand_joint_names + + omni.log.info("[G1DexRetargeter] init done.") + + def _update_yaml_with_urdf_path(self, yaml_path: str, urdf_path: str): + """Update YAML file with the correct URDF path. + + Args: + yaml_path: Path to the YAML configuration file + urdf_path: Path to the URDF file to use + """ + try: + # Read the YAML file + with open(yaml_path) as file: + config = yaml.safe_load(file) + + # Update the URDF path in the configuration + if "retargeting" in config: + config["retargeting"]["urdf_path"] = urdf_path + omni.log.info(f"Updated URDF path in {yaml_path} to {urdf_path}") + else: + omni.log.warn(f"Unable to find 'retargeting' section in {yaml_path}") + + # Write the updated configuration back to the file + with open(yaml_path, "w") as file: + yaml.dump(config, file) + + except Exception as e: + omni.log.error(f"Error updating YAML file {yaml_path}: {e}") + + def convert_hand_joints(self, hand_poses: dict[str, np.ndarray], operator2mano: np.ndarray) -> np.ndarray: + """Prepares the hand joints data for retargeting. + + Args: + hand_poses: Dictionary containing hand pose data with joint positions and rotations + operator2mano: Transformation matrix to convert from operator to MANO frame + + Returns: + Joint positions with shape (21, 3) + """ + joint_position = np.zeros((21, 3)) + hand_joints = list(hand_poses.values()) + for i, joint_index in enumerate(_HAND_JOINTS_INDEX): + joint = hand_joints[joint_index] + joint_position[i] = joint[:3] + + # Convert hand pose to the canonical frame. + joint_position = joint_position - joint_position[0:1, :] + xr_wrist_quat = hand_poses.get("wrist")[3:] + # OpenXR hand uses w,x,y,z order for quaternions but scipy uses x,y,z,w order + wrist_rot = R.from_quat([xr_wrist_quat[1], xr_wrist_quat[2], xr_wrist_quat[3], xr_wrist_quat[0]]).as_matrix() + + return joint_position @ wrist_rot @ operator2mano + + def compute_ref_value(self, joint_position: np.ndarray, indices: np.ndarray, retargeting_type: str) -> np.ndarray: + """Computes reference value for retargeting. + + Args: + joint_position: Joint positions array + indices: Target link indices + retargeting_type: Type of retargeting ("POSITION" or other) + + Returns: + Reference value in cartesian space + """ + if retargeting_type == "POSITION": + return joint_position[indices, :] + else: + origin_indices = indices[0, :] + task_indices = indices[1, :] + ref_value = joint_position[task_indices, :] - joint_position[origin_indices, :] + return ref_value + + def compute_one_hand( + self, hand_joints: dict[str, np.ndarray], retargeting: RetargetingConfig, operator2mano: np.ndarray + ) -> np.ndarray: + """Computes retargeted joint angles for one hand. + + Args: + hand_joints: Dictionary containing hand joint data + retargeting: Retargeting configuration object + operator2mano: Transformation matrix from operator to MANO frame + + Returns: + Retargeted joint angles + """ + joint_pos = self.convert_hand_joints(hand_joints, operator2mano) + ref_value = self.compute_ref_value( + joint_pos, + indices=retargeting.optimizer.target_link_human_indices, + retargeting_type=retargeting.optimizer.retargeting_type, + ) + # Enable gradient calculation and inference mode in case some other script has disabled it + # This is necessary for the retargeting to work since it uses gradient features that + # are not available in inference mode + with torch.enable_grad(): + with torch.inference_mode(False): + return retargeting.retarget(ref_value) + + def get_joint_names(self) -> list[str]: + """Returns list of all joint names.""" + return self.dof_names + + def get_left_joint_names(self) -> list[str]: + """Returns list of left hand joint names.""" + return self.left_dof_names + + def get_right_joint_names(self) -> list[str]: + """Returns list of right hand joint names.""" + return self.right_dof_names + + def get_hand_indices(self, robot) -> np.ndarray: + """Gets indices of hand joints in robot's DOF array. + + Args: + robot: Robot object containing DOF information + + Returns: + Array of joint indices + """ + return np.array([robot.dof_names.index(name) for name in self.dof_names], dtype=np.int64) + + def compute_left(self, left_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + """Computes retargeted joints for left hand. + + Args: + left_hand_poses: Dictionary of left hand joint poses + + Returns: + Retargeted joint angles for left hand + """ + if left_hand_poses is not None: + left_hand_q = self.compute_one_hand(left_hand_poses, self._dex_left_hand, _OPERATOR2MANO_LEFT) + else: + left_hand_q = np.zeros(len(_LEFT_HAND_JOINT_NAMES)) + return left_hand_q + + def compute_right(self, right_hand_poses: dict[str, np.ndarray]) -> np.ndarray: + """Computes retargeted joints for right hand. + + Args: + right_hand_poses: Dictionary of right hand joint poses + + Returns: + Retargeted joint angles for right hand + """ + if right_hand_poses is not None: + right_hand_q = self.compute_one_hand(right_hand_poses, self._dex_right_hand, _OPERATOR2MANO_RIGHT) + else: + right_hand_q = np.zeros(len(_RIGHT_HAND_JOINT_NAMES)) + return right_hand_q diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py new file mode 100644 index 000000000000..41f7f49fd9f8 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py @@ -0,0 +1,166 @@ +# 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 + +from __future__ import annotations + +import contextlib +import numpy as np +import torch +from dataclasses import dataclass + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices import OpenXRDevice +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + +# This import exception is suppressed because g1_dex_retargeting_utils depends on pinocchio which is not available on windows +with contextlib.suppress(Exception): + from .g1_dex_retargeting_utils import G1TriHandDexRetargeting + + +@dataclass +class G1TriHandUpperBodyRetargeterCfg(RetargeterCfg): + """Configuration for the G1UpperBody retargeter.""" + + enable_visualization: bool = False + num_open_xr_hand_joints: int = 100 + hand_joint_names: list[str] | None = None # List of robot hand joint names + + +class G1TriHandUpperBodyRetargeter(RetargeterBase): + """Retargets OpenXR data to G1 upper body commands. + + This retargeter maps hand tracking data from OpenXR to wrist and hand joint commands for the G1 robot. + It handles both left and right hands, converting poses of the hands in OpenXR format to appropriate wrist poses + and joint angles for the G1 robot's upper body. + """ + + def __init__( + self, + cfg: G1TriHandUpperBodyRetargeterCfg, + ): + """Initialize the G1 upper body retargeter. + + Args: + cfg: Configuration for the retargeter. + """ + + # Store device name for runtime retrieval + self._sim_device = cfg.sim_device + self._hand_joint_names = cfg.hand_joint_names + + # Initialize the hands controller + if cfg.hand_joint_names is not None: + self._hands_controller = G1TriHandDexRetargeting(cfg.hand_joint_names) + else: + raise ValueError("hand_joint_names must be provided in configuration") + + # Initialize visualization if enabled + self._enable_visualization = cfg.enable_visualization + self._num_open_xr_hand_joints = cfg.num_open_xr_hand_joints + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_hand_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.005, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + """Convert hand joint poses to robot end-effector commands. + + Args: + data: Dictionary mapping tracking targets to joint data dictionaries. + + Returns: + A tensor containing the retargeted commands: + - Left wrist pose (7) + - Right wrist pose (7) + - Hand joint angles (len(hand_joint_names)) + """ + + # Access the left and right hand data using the enum key + left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT] + right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + + left_wrist = left_hand_poses.get("wrist") + right_wrist = right_hand_poses.get("wrist") + + # Handle case where wrist data is not available + if left_wrist is None or right_wrist is None: + # Set to default pose if no data available. + # pos=(0,0,0), quat=(1,0,0,0) (w,x,y,z) + default_pose = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + if left_wrist is None: + left_wrist = default_pose + if right_wrist is None: + right_wrist = default_pose + + # Visualization if enabled + if self._enable_visualization: + joints_position = np.zeros((self._num_open_xr_hand_joints, 3)) + joints_position[::2] = np.array([pose[:3] for pose in left_hand_poses.values()]) + joints_position[1::2] = np.array([pose[:3] for pose in right_hand_poses.values()]) + self._markers.visualize(translations=torch.tensor(joints_position, device=self._sim_device)) + + # Compute retargeted hand joints + left_hands_pos = self._hands_controller.compute_left(left_hand_poses) + indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_left_joint_names()] + left_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) + left_retargeted_hand_joints[indexes] = left_hands_pos + left_hand_joints = left_retargeted_hand_joints + + right_hands_pos = self._hands_controller.compute_right(right_hand_poses) + indexes = [self._hand_joint_names.index(name) for name in self._hands_controller.get_right_joint_names()] + right_retargeted_hand_joints = np.zeros(len(self._hands_controller.get_joint_names())) + right_retargeted_hand_joints[indexes] = right_hands_pos + right_hand_joints = right_retargeted_hand_joints + retargeted_hand_joints = left_hand_joints + right_hand_joints + + # Convert numpy arrays to tensors and store in command buffer + left_wrist_tensor = torch.tensor( + self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + ) + right_wrist_tensor = torch.tensor( + self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + ) + hand_joints_tensor = torch.tensor(retargeted_hand_joints, dtype=torch.float32, device=self._sim_device) + + # Combine all tensors into a single tensor + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + """Handle absolute pose retargeting. + + Args: + wrist: Wrist pose data from OpenXR. + is_left: True for the left hand, False for the right hand. + + Returns: + Retargeted wrist pose in USD control frame. + """ + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + if is_left: + # Corresponds to a rotation of (0, 90, 90) in euler angles (x,y,z) + combined_quat = torch.tensor([0.7071, 0, 0.7071, 0], dtype=torch.float32) + else: + # Corresponds to a rotation of (0, -90, -90) in euler angles (x,y,z) + combined_quat = torch.tensor([0, -0.7071, 0, 0.7071], dtype=torch.float32) + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + transform_pose = PoseUtils.make_pose(torch.zeros(3), PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py index f2a7eed32c6d..6f63a1a8d72c 100644 --- a/source/isaaclab/isaaclab/devices/teleop_device_factory.py +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -15,6 +15,10 @@ from isaaclab.devices.gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg from isaaclab.devices.keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg from isaaclab.devices.openxr.retargeters import ( + G1LowerBodyStandingRetargeter, + G1LowerBodyStandingRetargeterCfg, + G1TriHandUpperBodyRetargeter, + G1TriHandUpperBodyRetargeterCfg, GR1T2Retargeter, GR1T2RetargeterCfg, GripperRetargeter, @@ -50,6 +54,8 @@ Se3RelRetargeterCfg: Se3RelRetargeter, GripperRetargeterCfg: GripperRetargeter, GR1T2RetargeterCfg: GR1T2Retargeter, + G1TriHandUpperBodyRetargeterCfg: G1TriHandUpperBodyRetargeter, + G1LowerBodyStandingRetargeterCfg: G1LowerBodyStandingRetargeter, } 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 834d23d955a0..db478e7186e0 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/pink_actions_cfg.py @@ -26,15 +26,15 @@ class PinkInverseKinematicsActionCfg(ActionTermCfg): pink_controlled_joint_names: list[str] = MISSING """List of joint names or regular expression patterns that specify the joints controlled by pink IK.""" - ik_urdf_fixed_joint_names: list[str] = MISSING - """List of joint names that specify the joints to be locked in URDF.""" - hand_joint_names: list[str] = MISSING """List of joint names or regular expression patterns that specify the joints controlled by hand retargeting.""" controller: PinkIKControllerCfg = MISSING """Configuration for the Pink IK controller that will be used to solve the inverse kinematics.""" + enable_gravity_compensation: bool = True + """Whether to compensate for gravity in the Pink IK controller.""" + target_eef_link_names: dict[str, str] = MISSING """Dictionary mapping task names to controlled link names for the Pink IK controller. 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 f1e9fd7a819c..79490c07e426 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 @@ -5,7 +5,6 @@ from __future__ import annotations -import copy import torch from collections.abc import Sequence from typing import TYPE_CHECKING @@ -15,6 +14,7 @@ import isaaclab.utils.math as math_utils from isaaclab.assets.articulation import Articulation from isaaclab.controllers.pink_ik import PinkIKController +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask from isaaclab.managers.action_manager import ActionTerm if TYPE_CHECKING: @@ -27,8 +27,8 @@ class PinkInverseKinematicsAction(ActionTerm): r"""Pink Inverse Kinematics action term. - This action term processes the action tensor and sets these setpoints in the pink IK framework - The action tensor is ordered in the order of the tasks defined in PinkIKControllerCfg + This action term processes the action tensor and sets these setpoints in the pink IK framework. + The action tensor is ordered in the order of the tasks defined in PinkIKControllerCfg. """ cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg @@ -46,53 +46,78 @@ def __init__(self, cfg: pink_actions_cfg.PinkInverseKinematicsActionCfg, env: Ma """ super().__init__(cfg, env) - # Resolve joint IDs and names based on the configuration - self._pink_controlled_joint_ids, self._pink_controlled_joint_names = self._asset.find_joints( + self._env = env + self._sim_dt = env.sim.get_physics_dt() + + # Initialize joint information + self._initialize_joint_info() + + # Initialize IK controllers + self._initialize_ik_controllers() + + # Initialize action tensors + self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) + self._processed_actions = torch.zeros_like(self._raw_actions) + + # PhysX Articulation Floating joint indices offset from IsaacLab Articulation joint indices + self._physx_floating_joint_indices_offset = 6 + + # Pre-allocate tensors for runtime use + self._initialize_helper_tensors() + + def _initialize_joint_info(self) -> None: + """Initialize joint IDs and names based on configuration.""" + # Resolve pink controlled joints + self._isaaclab_controlled_joint_ids, self._isaaclab_controlled_joint_names = self._asset.find_joints( self.cfg.pink_controlled_joint_names ) - self.cfg.controller.joint_names = self._pink_controlled_joint_names + self.cfg.controller.joint_names = self._isaaclab_controlled_joint_names + self._isaaclab_all_joint_ids = list(range(len(self._asset.data.joint_names))) + self.cfg.controller.all_joint_names = self._asset.data.joint_names + + # Resolve hand joints self._hand_joint_ids, self._hand_joint_names = self._asset.find_joints(self.cfg.hand_joint_names) - self._joint_ids = self._pink_controlled_joint_ids + self._hand_joint_ids - self._joint_names = self._pink_controlled_joint_names + self._hand_joint_names - # Initialize the Pink IK controller - assert env.num_envs > 0, "Number of environments specified are less than 1." + # Combine all joint information + self._controlled_joint_ids = self._isaaclab_controlled_joint_ids + self._hand_joint_ids + self._controlled_joint_names = self._isaaclab_controlled_joint_names + self._hand_joint_names + + def _initialize_ik_controllers(self) -> None: + """Initialize Pink IK controllers for all environments.""" + assert self._env.num_envs > 0, "Number of environments specified are less than 1." + self._ik_controllers = [] - for _ in range(env.num_envs): + for _ in range(self._env.num_envs): self._ik_controllers.append( - PinkIKController(cfg=self.cfg.controller.copy(), robot_cfg=env.scene.cfg.robot, device=self.device) + PinkIKController( + cfg=self.cfg.controller.copy(), + robot_cfg=self._env.scene.cfg.robot, + device=self.device, + controlled_joint_indices=self._isaaclab_controlled_joint_ids, + ) ) - # Create tensors to store raw and processed actions - self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device) - self._processed_actions = torch.zeros_like(self.raw_actions) - - # Get the simulation time step - self._sim_dt = env.sim.get_physics_dt() - - self.total_time = 0 # Variable to accumulate the total time - self.num_runs = 0 # Counter for the number of runs - - # Save the base_link_frame pose in the world frame as a transformation matrix in - # order to transform the desired pose of the controlled_frame to be with respect to the base_link_frame - # Shape of env.scene[self.cfg.articulation_name].data.body_link_state_w is (num_instances, num_bodies, 13) - base_link_frame_in_world_origin = env.scene[self.cfg.controller.articulation_name].data.body_link_state_w[ - :, - env.scene[self.cfg.controller.articulation_name].data.body_names.index(self.cfg.controller.base_link_name), - :7, - ] + def _initialize_helper_tensors(self) -> None: + """Pre-allocate tensors and cache values for performance optimization.""" + # Cache frequently used tensor versions of joint IDs to avoid repeated creation + self._controlled_joint_ids_tensor = torch.tensor(self._controlled_joint_ids, device=self.device) - # Get robot base link frame in env origin frame - base_link_frame_in_env_origin = copy.deepcopy(base_link_frame_in_world_origin) - base_link_frame_in_env_origin[:, :3] -= self._env.scene.env_origins + # Cache base link index to avoid string lookup every time + articulation_data = self._env.scene[self.cfg.controller.articulation_name].data + self._base_link_idx = articulation_data.body_names.index(self.cfg.controller.base_link_name) - self.base_link_frame_in_env_origin = math_utils.make_pose( - base_link_frame_in_env_origin[:, :3], math_utils.matrix_from_quat(base_link_frame_in_env_origin[:, 3:7]) + # Pre-allocate working tensors + # Count only FrameTask instances in variable_input_tasks (not all tasks) + num_frame_tasks = sum( + 1 for task in self._ik_controllers[0].cfg.variable_input_tasks if isinstance(task, FrameTask) ) + self._num_frame_tasks = num_frame_tasks + self._controlled_frame_poses = torch.zeros(num_frame_tasks, self.num_envs, 4, 4, device=self.device) - # """ - # Properties. - # """ + # Pre-allocate tensor for base frame computations + self._base_link_frame_buffer = torch.zeros(self.num_envs, 4, 4, device=self.device) + + # ==================== Properties ==================== @property def hand_joint_dim(self) -> int: @@ -153,7 +178,7 @@ def IO_descriptor(self) -> GenericActionIODescriptor: self._IO_descriptor.shape = (self.action_dim,) self._IO_descriptor.dtype = str(self.raw_actions.dtype) self._IO_descriptor.action_type = "PinkInverseKinematicsAction" - self._IO_descriptor.pink_controller_joint_names = self._pink_controlled_joint_names + self._IO_descriptor.pink_controller_joint_names = self._isaaclab_controlled_joint_names self._IO_descriptor.hand_joint_names = self._hand_joint_names self._IO_descriptor.extras["controller_cfg"] = self.cfg.controller.__dict__ return self._IO_descriptor @@ -162,75 +187,175 @@ def IO_descriptor(self) -> GenericActionIODescriptor: # Operations. # """ - def process_actions(self, actions: torch.Tensor): + def process_actions(self, actions: torch.Tensor) -> None: """Process the input actions and set targets for each task. Args: actions: The input actions tensor. """ - # Store the raw actions + # Store raw actions self._raw_actions[:] = actions - # Make a copy of actions before modifying so that raw actions are not modified - actions_clone = actions.clone() - - # Extract hand joint positions (last 22 values) - self._target_hand_joint_positions = actions_clone[:, -self.hand_joint_dim :] - - # The action tensor provides the desired pose of the controlled_frame with respect to the env origin frame - # But the pink IK controller expects the desired pose of the controlled_frame with respect to the base_link_frame - # So we need to transform the desired pose of the controlled_frame to be with respect to the base_link_frame - - # Get the controlled_frame pose wrt to the env origin frame - all_controlled_frames_in_env_origin = [] - # The contrllers for all envs are the same, hence just using the first one to get the number of variable_input_tasks - for task_index in range(len(self._ik_controllers[0].cfg.variable_input_tasks)): - controlled_frame_in_env_origin_pos = actions_clone[ - :, task_index * self.pose_dim : task_index * self.pose_dim + self.position_dim - ] - controlled_frame_in_env_origin_quat = actions_clone[ - :, task_index * self.pose_dim + self.position_dim : (task_index + 1) * self.pose_dim - ] - controlled_frame_in_env_origin = math_utils.make_pose( - controlled_frame_in_env_origin_pos, math_utils.matrix_from_quat(controlled_frame_in_env_origin_quat) - ) - all_controlled_frames_in_env_origin.append(controlled_frame_in_env_origin) - # Stack all the controlled_frame poses in the env origin frame. Shape is (num_tasks, num_envs , 4, 4) - all_controlled_frames_in_env_origin = torch.stack(all_controlled_frames_in_env_origin) + # Extract hand joint positions directly (no cloning needed) + self._target_hand_joint_positions = actions[:, -self.hand_joint_dim :] - # Transform the controlled_frame to be with respect to the base_link_frame using batched matrix multiplication - controlled_frame_in_base_link_frame = math_utils.pose_in_A_to_pose_in_B( - all_controlled_frames_in_env_origin, math_utils.pose_inv(self.base_link_frame_in_env_origin) + # Get base link frame transformation + self.base_link_frame_in_world_rf = self._get_base_link_frame_transform() + + # Process controlled frame poses (pass original actions, no clone needed) + controlled_frame_poses = self._extract_controlled_frame_poses(actions) + transformed_poses = self._transform_poses_to_base_link_frame(controlled_frame_poses) + + # Set targets for all tasks + self._set_task_targets(transformed_poses) + + def _get_base_link_frame_transform(self) -> torch.Tensor: + """Get the base link frame transformation matrix. + + Returns: + Base link frame transformation matrix. + """ + # Get base link frame pose in world origin using cached index + articulation_data = self._env.scene[self.cfg.controller.articulation_name].data + base_link_frame_in_world_origin = articulation_data.body_link_state_w[:, self._base_link_idx, :7] + + # Transform to environment origin frame (reuse buffer to avoid allocation) + torch.sub( + base_link_frame_in_world_origin[:, :3], + self._env.scene.env_origins, + out=self._base_link_frame_buffer[:, :3, 3], ) - controlled_frame_in_base_link_frame_pos, controlled_frame_in_base_link_frame_mat = math_utils.unmake_pose( - controlled_frame_in_base_link_frame + # Copy orientation (avoid clone) + base_link_frame_quat = base_link_frame_in_world_origin[:, 3:7] + + # Create transformation matrix + return math_utils.make_pose( + self._base_link_frame_buffer[:, :3, 3], math_utils.matrix_from_quat(base_link_frame_quat) ) - # Loop through each task and set the target + def _extract_controlled_frame_poses(self, actions: torch.Tensor) -> torch.Tensor: + """Extract controlled frame poses from action tensor. + + Args: + actions: The action tensor. + + Returns: + Stacked controlled frame poses tensor. + """ + # Use pre-allocated tensor instead of list operations + for task_index in range(self._num_frame_tasks): + # Extract position and orientation for this task + pos_start = task_index * self.pose_dim + pos_end = pos_start + self.position_dim + quat_start = pos_end + quat_end = (task_index + 1) * self.pose_dim + + position = actions[:, pos_start:pos_end] + quaternion = actions[:, quat_start:quat_end] + + # Create pose matrix directly into pre-allocated tensor + self._controlled_frame_poses[task_index] = math_utils.make_pose( + position, math_utils.matrix_from_quat(quaternion) + ) + + return self._controlled_frame_poses + + def _transform_poses_to_base_link_frame(self, poses: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Transform poses from world frame to base link frame. + + Args: + poses: Poses in world frame. + + Returns: + Tuple of (positions, rotation_matrices) in base link frame. + """ + # Transform poses to base link frame + base_link_inv = math_utils.pose_inv(self.base_link_frame_in_world_rf) + transformed_poses = math_utils.pose_in_A_to_pose_in_B(poses, base_link_inv) + + # Extract position and rotation + positions, rotation_matrices = math_utils.unmake_pose(transformed_poses) + + return positions, rotation_matrices + + def _set_task_targets(self, transformed_poses: tuple[torch.Tensor, torch.Tensor]) -> None: + """Set targets for all tasks across all environments. + + Args: + transformed_poses: Tuple of (positions, rotation_matrices) in base link frame. + """ + positions, rotation_matrices = transformed_poses + for env_index, ik_controller in enumerate(self._ik_controllers): - for task_index, task in enumerate(ik_controller.cfg.variable_input_tasks): - if isinstance(task, FrameTask): + for frame_task_index, task in enumerate(ik_controller.cfg.variable_input_tasks): + if isinstance(task, LocalFrameTask): + target = task.transform_target_to_base + elif 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) + else: + continue + + # Set position and rotation targets using frame_task_index + target.translation = positions[frame_task_index, env_index, :].cpu().numpy() + target.rotation = rotation_matrices[frame_task_index, env_index, :].cpu().numpy() - def apply_actions(self): - # start_time = time.time() # Capture the time before the step + task.set_target(target) + + # ==================== Action Application ==================== + + def apply_actions(self) -> None: """Apply the computed joint positions based on the inverse kinematics solution.""" - all_envs_joint_pos_des = [] + # Compute IK solutions for all environments + ik_joint_positions = self._compute_ik_solutions() + + # Combine IK and hand joint positions + all_joint_positions = torch.cat((ik_joint_positions, self._target_hand_joint_positions), dim=1) + self._processed_actions = all_joint_positions + + # Apply gravity compensation to arm joints + if self.cfg.enable_gravity_compensation: + self._apply_gravity_compensation() + + # Apply joint position targets + self._asset.set_joint_position_target(self._processed_actions, self._controlled_joint_ids) + + def _apply_gravity_compensation(self) -> None: + """Apply gravity compensation to arm joints if not disabled in props.""" + if not self._asset.cfg.spawn.rigid_props.disable_gravity: + # Get gravity compensation forces using cached tensor + if self._asset.is_fixed_base: + gravity = torch.zeros_like( + self._asset.root_physx_view.get_gravity_compensation_forces()[:, self._controlled_joint_ids_tensor] + ) + else: + # If floating base, then need to skip the first 6 joints (base) + gravity = self._asset.root_physx_view.get_gravity_compensation_forces()[ + :, self._controlled_joint_ids_tensor + self._physx_floating_joint_indices_offset + ] + + # Apply gravity compensation to arm joints + self._asset.set_joint_effort_target(gravity, self._controlled_joint_ids) + + def _compute_ik_solutions(self) -> torch.Tensor: + """Compute IK solutions for all environments. + + Returns: + IK joint positions tensor for all environments. + """ + ik_solutions = [] + for env_index, ik_controller in enumerate(self._ik_controllers): - curr_joint_pos = self._asset.data.joint_pos[:, self._pink_controlled_joint_ids].cpu().numpy()[env_index] - joint_pos_des = ik_controller.compute(curr_joint_pos, self._sim_dt) - all_envs_joint_pos_des.append(joint_pos_des) - all_envs_joint_pos_des = torch.stack(all_envs_joint_pos_des) + # Get current joint positions for this environment + current_joint_pos = self._asset.data.joint_pos.cpu().numpy()[env_index] + + # Compute IK solution + joint_pos_des = ik_controller.compute(current_joint_pos, self._sim_dt) + ik_solutions.append(joint_pos_des) - # Combine IK joint positions with hand joint positions - all_envs_joint_pos_des = torch.cat((all_envs_joint_pos_des, self._target_hand_joint_positions), dim=1) - self._processed_actions = all_envs_joint_pos_des + return torch.stack(ik_solutions) - self._asset.set_joint_position_target(self._processed_actions, self._joint_ids) + # ==================== Reset ==================== def reset(self, env_ids: Sequence[int] | None = None) -> None: """Reset the action term for specified environments. diff --git a/source/isaaclab/isaaclab/utils/io/__init__.py b/source/isaaclab/isaaclab/utils/io/__init__.py index 1808eb1df7bb..87a14819a613 100644 --- a/source/isaaclab/isaaclab/utils/io/__init__.py +++ b/source/isaaclab/isaaclab/utils/io/__init__.py @@ -8,4 +8,5 @@ """ from .pkl import dump_pickle, load_pickle +from .torchscript import load_torchscript_model from .yaml import dump_yaml, load_yaml diff --git a/source/isaaclab/isaaclab/utils/io/torchscript.py b/source/isaaclab/isaaclab/utils/io/torchscript.py new file mode 100644 index 000000000000..df5fe454bf32 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/io/torchscript.py @@ -0,0 +1,39 @@ +# 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 + + +"""TorchScript I/O utilities.""" + +import os +import torch + + +def load_torchscript_model(model_path: str, device: str = "cpu") -> torch.nn.Module: + """Load a TorchScript model from the specified path. + + This function only loads TorchScript models (.pt or .pth files created with torch.jit.save). + It will not work with raw PyTorch checkpoints (.pth files created with torch.save). + + Args: + model_path (str): Path to the TorchScript model file (.pt or .pth) + device (str, optional): Device to load the model on. Defaults to 'cpu'. + + Returns: + torch.nn.Module: The loaded TorchScript model in evaluation mode + + Raises: + FileNotFoundError: If the model file does not exist + """ + if not os.path.exists(model_path): + raise FileNotFoundError(f"TorchScript model file not found: {model_path}") + + try: + model = torch.jit.load(model_path, map_location=device) + model.eval() + print(f"Successfully loaded TorchScript model from {model_path}") + return model + except Exception as e: + print(f"Error loading TorchScript model: {e}") + return None diff --git a/source/isaaclab/test/app/test_non_headless_launch.py b/source/isaaclab/test/app/test_non_headless_launch.py new file mode 100644 index 000000000000..52c35a109167 --- /dev/null +++ b/source/isaaclab/test/app/test_non_headless_launch.py @@ -0,0 +1,65 @@ +# 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 + +""" +This script checks if the app can be launched with non-headless app and start the simulation. +""" + +"""Launch Isaac Sim Simulator first.""" + + +import pytest + +from isaaclab.app import AppLauncher + +# launch omniverse app +app_launcher = AppLauncher(experience="isaaclab.python.kit", headless=True) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.utils import configclass + + +@configclass +class SensorsSceneCfg(InteractiveSceneCfg): + """Design the scene with sensors on the robot.""" + + # ground plane + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + +def run_simulator( + sim: sim_utils.SimulationContext, +): + """Run the simulator.""" + + count = 0 + + # Simulate physics + while simulation_app.is_running() and count < 100: + # perform step + sim.step() + count += 1 + + +@pytest.mark.isaacsim_ci +def test_non_headless_launch(): + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.005) + sim = sim_utils.SimulationContext(sim_cfg) + # design scene + scene_cfg = SensorsSceneCfg(num_envs=1, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + print(scene) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim) diff --git a/source/isaaclab/test/controllers/test_controller_utils.py b/source/isaaclab/test/controllers/test_controller_utils.py new file mode 100644 index 000000000000..f1f249dacf9c --- /dev/null +++ b/source/isaaclab/test/controllers/test_controller_utils.py @@ -0,0 +1,662 @@ +# 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 + +"""Test cases for Isaac Lab controller utilities.""" + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import os + +# Import the function to test +import tempfile +import torch + +import pytest + +from isaaclab.controllers.utils import change_revolute_to_fixed, change_revolute_to_fixed_regex +from isaaclab.utils.assets import retrieve_file_path +from isaaclab.utils.io.torchscript import load_torchscript_model + + +@pytest.fixture +def mock_urdf_content(): + """Create mock URDF content for testing.""" + return """ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +""" + + +@pytest.fixture +def test_urdf_file(mock_urdf_content): + """Create a temporary URDF file for testing.""" + # Create a temporary directory for test files + test_dir = tempfile.mkdtemp() + + # Create the test URDF file + test_urdf_path = os.path.join(test_dir, "test_robot.urdf") + with open(test_urdf_path, "w") as f: + f.write(mock_urdf_content) + + yield test_urdf_path + + # Clean up the temporary directory and all its contents + import shutil + + shutil.rmtree(test_dir) + + +# ============================================================================= +# Test cases for change_revolute_to_fixed function +# ============================================================================= + + +def test_single_joint_conversion(test_urdf_file, mock_urdf_content): + """Test converting a single revolute joint to fixed.""" + # Test converting shoulder_to_elbow joint + fixed_joints = ["shoulder_to_elbow"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the joint was converted + assert '' in modified_content + assert '' not in modified_content + + # Check that other revolute joints remain unchanged + assert '' in modified_content + assert '' in modified_content + + +def test_multiple_joints_conversion(test_urdf_file, mock_urdf_content): + """Test converting multiple revolute joints to fixed.""" + # Test converting multiple joints + fixed_joints = ["base_to_shoulder", "elbow_to_wrist"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that both joints were converted + assert '' in modified_content + assert '' in modified_content + assert '' not in modified_content + assert '' not in modified_content + + # Check that the middle joint remains unchanged + assert '' in modified_content + + +def test_non_existent_joint(test_urdf_file, mock_urdf_content): + """Test behavior when trying to convert a non-existent joint.""" + # Try to convert a joint that doesn't exist + fixed_joints = ["non_existent_joint"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged + assert modified_content == mock_urdf_content + + +def test_mixed_existent_and_non_existent_joints(test_urdf_file, mock_urdf_content): + """Test converting a mix of existent and non-existent joints.""" + # Try to convert both existent and non-existent joints + fixed_joints = ["base_to_shoulder", "non_existent_joint", "elbow_to_wrist"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that existent joints were converted + assert '' in modified_content + assert '' in modified_content + + # Check that non-existent joint didn't cause issues + assert '' not in modified_content + + +def test_already_fixed_joint(test_urdf_file, mock_urdf_content): + """Test behavior when trying to convert an already fixed joint.""" + # Try to convert a joint that is already fixed + fixed_joints = ["wrist_to_gripper"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged (no conversion happened) + assert modified_content == mock_urdf_content + + +def test_empty_joints_list(test_urdf_file, mock_urdf_content): + """Test behavior when passing an empty list of joints.""" + # Try to convert with empty list + fixed_joints = [] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged + assert modified_content == mock_urdf_content + + +def test_file_not_found(test_urdf_file): + """Test behavior when URDF file doesn't exist.""" + non_existent_path = os.path.join(os.path.dirname(test_urdf_file), "non_existent.urdf") + fixed_joints = ["base_to_shoulder"] + + # Should raise FileNotFoundError + with pytest.raises(FileNotFoundError): + change_revolute_to_fixed(non_existent_path, fixed_joints) + + +def test_preserve_other_content(test_urdf_file): + """Test that other content in the URDF file is preserved.""" + fixed_joints = ["shoulder_to_elbow"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that other content is preserved + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + # Check that the fixed joint remains unchanged + assert '' in modified_content + + +def test_joint_attributes_preserved(test_urdf_file): + """Test that joint attributes other than type are preserved.""" + fixed_joints = ["base_to_shoulder"] + change_revolute_to_fixed(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the joint was converted but other attributes preserved + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + +# ============================================================================= +# Test cases for change_revolute_to_fixed_regex function +# ============================================================================= + + +def test_regex_single_joint_conversion(test_urdf_file, mock_urdf_content): + """Test converting a single revolute joint to fixed using regex pattern.""" + # Test converting shoulder_to_elbow joint using exact match + fixed_joints = ["shoulder_to_elbow"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the joint was converted + assert '' in modified_content + assert '' not in modified_content + + # Check that other revolute joints remain unchanged + assert '' in modified_content + assert '' in modified_content + + +def test_regex_pattern_matching(test_urdf_file, mock_urdf_content): + """Test converting joints using regex patterns.""" + # Test converting joints that contain "to" in their name + fixed_joints = [r".*to.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that all joints with "to" in the name were converted + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + # Check that the fixed joint remains unchanged + assert '' in modified_content + + +def test_regex_multiple_patterns(test_urdf_file, mock_urdf_content): + """Test converting joints using multiple regex patterns.""" + # Test converting joints that start with "base" or end with "wrist" + fixed_joints = [r"^base.*", r".*wrist$"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that matching joints were converted + assert '' in modified_content + assert '' in modified_content + + # Check that non-matching joints remain unchanged + assert '' in modified_content + + +def test_regex_case_sensitive_matching(test_urdf_file, mock_urdf_content): + """Test that regex matching is case sensitive.""" + # Test with uppercase pattern that won't match lowercase joint names + fixed_joints = [r".*TO.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that no joints were converted (case sensitive) + assert modified_content == mock_urdf_content + + +def test_regex_partial_word_matching(test_urdf_file, mock_urdf_content): + """Test converting joints using partial word matching.""" + # Test converting joints that contain "shoulder" in their name + fixed_joints = [r".*shoulder.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that shoulder-related joints were converted + assert '' in modified_content + assert '' in modified_content + + # Check that other joints remain unchanged + assert '' in modified_content + + +def test_regex_no_matches(test_urdf_file, mock_urdf_content): + """Test behavior when regex patterns don't match any joints.""" + # Test with pattern that won't match any joint names + fixed_joints = [r"^nonexistent.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged + assert modified_content == mock_urdf_content + + +def test_regex_empty_patterns_list(test_urdf_file, mock_urdf_content): + """Test behavior when passing an empty list of regex patterns.""" + # Try to convert with empty list + fixed_joints = [] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged + assert modified_content == mock_urdf_content + + +def test_regex_file_not_found(test_urdf_file): + """Test behavior when URDF file doesn't exist for regex function.""" + non_existent_path = os.path.join(os.path.dirname(test_urdf_file), "non_existent.urdf") + fixed_joints = [r".*to.*"] + + # Should raise FileNotFoundError + with pytest.raises(FileNotFoundError): + change_revolute_to_fixed_regex(non_existent_path, fixed_joints) + + +def test_regex_preserve_other_content(test_urdf_file): + """Test that other content in the URDF file is preserved with regex function.""" + fixed_joints = [r".*shoulder.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that other content is preserved + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + # Check that the fixed joint remains unchanged + assert '' in modified_content + + +def test_regex_joint_attributes_preserved(test_urdf_file): + """Test that joint attributes other than type are preserved with regex function.""" + fixed_joints = [r"^base.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the joint was converted but other attributes preserved + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + +def test_regex_complex_pattern(test_urdf_file, mock_urdf_content): + """Test converting joints using a complex regex pattern.""" + # Test converting joints that have "to" and end with a word starting with "w" + fixed_joints = [r".*to.*w.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that matching joints were converted + assert '' in modified_content + assert '' in modified_content + + # Check that non-matching joints remain unchanged + assert '' in modified_content + + +def test_regex_already_fixed_joint(test_urdf_file, mock_urdf_content): + """Test behavior when regex pattern matches an already fixed joint.""" + # Try to convert joints that contain "gripper" (which is already fixed) + fixed_joints = [r".*gripper.*"] + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that the file content remains unchanged (no conversion happened) + assert modified_content == mock_urdf_content + + +def test_regex_special_characters(test_urdf_file, mock_urdf_content): + """Test regex patterns with special characters.""" + # Test with pattern that includes special regex characters + fixed_joints = [r".*to.*"] # This should match joints with "to" + change_revolute_to_fixed_regex(test_urdf_file, fixed_joints) + + # Read the modified file + with open(test_urdf_file) as f: + modified_content = f.read() + + # Check that joints with "to" were converted + assert '' in modified_content + assert '' in modified_content + assert '' in modified_content + + # Check that the fixed joint remains unchanged + assert '' in modified_content + + +# ============================================================================= +# Test cases for load_torchscript_model function +# ============================================================================= + + +@pytest.fixture +def policy_model_path(): + """Path to the test TorchScript model.""" + _policy_path = "omniverse://isaac-dev.ov.nvidia.com/Projects/agile/policy_checkpoints/agile_locomotion.pt" + return retrieve_file_path(_policy_path) + + +def test_load_torchscript_model_success(policy_model_path): + """Test successful loading of a TorchScript model.""" + model = load_torchscript_model(policy_model_path) + + # Check that model was loaded successfully + assert model is not None + assert isinstance(model, torch.nn.Module) + + # Check that model is in evaluation mode + assert model.training is False + + +def test_load_torchscript_model_cpu_device(policy_model_path): + """Test loading TorchScript model on CPU device.""" + model = load_torchscript_model(policy_model_path, device="cpu") + + # Check that model was loaded successfully + assert model is not None + assert isinstance(model, torch.nn.Module) + + # Check that model is in evaluation mode + assert model.training is False + + +def test_load_torchscript_model_cuda_device(policy_model_path): + """Test loading TorchScript model on CUDA device if available.""" + if torch.cuda.is_available(): + model = load_torchscript_model(policy_model_path, device="cuda") + + # Check that model was loaded successfully + assert model is not None + assert isinstance(model, torch.nn.Module) + + # Check that model is in evaluation mode + assert model.training is False + else: + # Skip test if CUDA is not available + pytest.skip("CUDA not available") + + +def test_load_torchscript_model_file_not_found(): + """Test behavior when TorchScript model file doesn't exist.""" + non_existent_path = "non_existent_model.pt" + + # Should raise FileNotFoundError + with pytest.raises(FileNotFoundError): + load_torchscript_model(non_existent_path) + + +def test_load_torchscript_model_invalid_file(): + """Test behavior when trying to load an invalid TorchScript file.""" + # Create a temporary file with invalid content + import tempfile + + with tempfile.NamedTemporaryFile(suffix=".pt", delete=False) as temp_file: + temp_file.write(b"invalid torchscript content") + temp_file_path = temp_file.name + + try: + # Should handle the error gracefully and return None + model = load_torchscript_model(temp_file_path) + assert model is None + finally: + # Clean up the temporary file + os.unlink(temp_file_path) + + +def test_load_torchscript_model_empty_file(): + """Test behavior when trying to load an empty TorchScript file.""" + # Create a temporary empty file + import tempfile + + with tempfile.NamedTemporaryFile(suffix=".pt", delete=False) as temp_file: + temp_file_path = temp_file.name + + try: + # Should handle the error gracefully and return None + model = load_torchscript_model(temp_file_path) + assert model is None + finally: + # Clean up the temporary file + os.unlink(temp_file_path) + + +def test_load_torchscript_model_different_device_mapping(policy_model_path): + """Test loading model with different device mapping.""" + # Test with specific device mapping + model = load_torchscript_model(policy_model_path, device="cpu") + + # Check that model was loaded successfully + assert model is not None + assert isinstance(model, torch.nn.Module) + + +def test_load_torchscript_model_evaluation_mode(policy_model_path): + """Test that loaded model is in evaluation mode.""" + model = load_torchscript_model(policy_model_path) + + # Check that model is in evaluation mode + assert model.training is False + + # Verify we can set it to training mode and back + model.train() + assert model.training is True + model.eval() + assert model.training is False + + +def test_load_torchscript_model_inference_capability(policy_model_path): + """Test that loaded model can perform inference.""" + model = load_torchscript_model(policy_model_path) + + # Check that model was loaded successfully + assert model is not None + + # Try to create a dummy input tensor (actual input shape depends on the model) + # This is a basic test to ensure the model can handle tensor inputs + try: + # Create a dummy input tensor (adjust size based on expected input) + dummy_input = torch.randn(1, 75) # Adjust dimensions as needed + + # Try to run inference (this might fail if input shape is wrong, but shouldn't crash) + with torch.no_grad(): + try: + output = model(dummy_input) + # If successful, check that output is a tensor + assert isinstance(output, torch.Tensor) + except (RuntimeError, ValueError): + # Expected if input shape doesn't match model expectations + # This is acceptable for this test + pass + except Exception: + # If model doesn't accept this input format, that's okay for this test + # The main goal is to ensure the model loads without crashing + pass + + +def test_load_torchscript_model_error_handling(): + """Test error handling when loading fails.""" + # Create a temporary file that will cause a loading error + import tempfile + + with tempfile.NamedTemporaryFile(suffix=".pt", delete=False) as temp_file: + temp_file.write(b"definitely not a torchscript model") + temp_file_path = temp_file.name + + try: + # Should handle the error gracefully and return None + model = load_torchscript_model(temp_file_path) + assert model is None + finally: + # Clean up the temporary file + os.unlink(temp_file_path) diff --git a/source/isaaclab/test/controllers/test_ik_configs/README.md b/source/isaaclab/test/controllers/test_ik_configs/README.md new file mode 100644 index 000000000000..ccbdae06b52e --- /dev/null +++ b/source/isaaclab/test/controllers/test_ik_configs/README.md @@ -0,0 +1,119 @@ +# Test Configuration Generation Guide + +This document explains how to generate test configurations for the Pink IK controller tests used in `test_pink_ik.py`. + +## File Structure + +Test configurations are JSON files with the following structure: + +```json +{ + "tolerances": { + "position": ..., + "pd_position": ..., + "rotation": ..., + "check_errors": true + }, + "allowed_steps_to_settle": ..., + "tests": { + "test_name": { + "left_hand_pose": [...], + "right_hand_pose": [...], + "allowed_steps_per_motion": ..., + "repeat": ... + } + } +} +``` + +## Parameters + +### Tolerances +- **position**: Maximum position error in meters +- **pd_position**: Maximum PD controller error in meters +- **rotation**: Maximum rotation error in radians +- **check_errors**: Whether to verify errors (should be `true`) + +### Test Parameters +- **allowed_steps_to_settle**: Initial settling steps (typically 100) +- **allowed_steps_per_motion**: Steps per motion phase +- **repeat**: Number of test repetitions +- **requires_waist_bending**: Whether the test requires waist bending (boolean) + +## Coordinate System + +### Robot Reset Pose +From `g1_locomanipulation_robot_cfg.py`: +- **Base position**: (0, 0, 0.75) - 75cm above ground +- **Base orientation**: 90° rotation around X-axis (facing forward) +- **Joint positions**: Standing pose with slight knee bend + +### EEF Pose Format +Each pose: `[x, y, z, qw, qx, qy, qz]` +- **Position**: Cartesian coordinates relative to robot base frame +- **Orientation**: Quaternion relative to the world. Typically you want this to start in the same orientation as robot base. (e.g. if robot base is reset to (0.7071, 0.0, 0.0, 0.7071), hand pose should be the same) + +**Note**: The system automatically compensates for hand rotational offsets, so specify orientations relative to the robot's reset orientation. + +## Creating Configurations + +### Step 1: Choose Robot Type +- `pink_ik_g1_test_configs.json` for G1 robot +- `pink_ik_gr1_test_configs.json` for GR1 robot + +### Step 2: Define Tolerances +```json +"tolerances": { + "position": 0.003, + "pd_position": 0.001, + "rotation": 0.017, + "check_errors": true +} +``` + +### Step 3: Create Test Movements +Common test types: +- **stay_still**: Same pose repeated +- **horizontal_movement**: Side-to-side movement +- **vertical_movement**: Up-and-down movement +- **rotation_movements**: Hand orientation changes + +### Step 4: Specify Hand Poses +```json +"horizontal_movement": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.28, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.28, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 100, + "repeat": 2, + "requires_waist_bending": false +} +``` + +## Pose Guidelines + +### Orientation Examples +- **Default**: `[0.7071, 0.0, 0.0, 0.7071]` (90° around X-axis) +- **Z-rotation**: `[0.5, 0.0, 0.0, 0.866]` (60° around Z) +- **Y-rotation**: `[0.866, 0.0, 0.5, 0.0]` (60° around Y) + +## Testing Process + +1. Robot starts in reset pose and settles +2. Moves through each pose in sequence +3. Errors computed and verified against tolerances +4. Sequence repeats specified number of times + +### Waist Bending Logic +Tests marked with `"requires_waist_bending": true` will only run if waist joints are enabled in the environment configuration. The test system automatically detects waist capability by checking if waist joints (`waist_yaw_joint`, `waist_pitch_joint`, `waist_roll_joint`) are included in the `pink_controlled_joint_names` list. + +## Troubleshooting + +- **Can't reach target**: Check if within safe workspace +- **High errors**: Increase tolerances or adjust poses +- **Test failures**: Increase `allowed_steps_per_motion` diff --git a/source/isaaclab/test/controllers/test_ik_configs/pink_ik_g1_test_configs.json b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_g1_test_configs.json new file mode 100644 index 000000000000..f5d0d60717da --- /dev/null +++ b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_g1_test_configs.json @@ -0,0 +1,111 @@ +{ + "tolerances": { + "position": 0.003, + "pd_position": 0.002, + "rotation": 0.017, + "check_errors": true + }, + "allowed_steps_to_settle": 50, + "tests": { + "horizontal_movement": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.28, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.28, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 15, + "repeat": 2, + "requires_waist_bending": false + }, + "horizontal_small_movement": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.19, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.19, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 15, + "repeat": 2, + "requires_waist_bending": false + }, + "stay_still": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 20, + "repeat": 4, + "requires_waist_bending": false + }, + "vertical_movement": { + "left_hand_pose": [ + [-0.18, 0.15, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.15, 0.85, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.15, 0.9, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.15, 0.85, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.15, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.15, 0.85, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.15, 0.9, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.15, 0.85, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 30, + "repeat": 2, + "requires_waist_bending": false + }, + "forward_waist_bending_movement": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.2, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.18, 0.3, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.2, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.18, 0.3, 0.8, 0.7071, 0.0, 0.0, 0.7071] + ], + "allowed_steps_per_motion": 60, + "repeat": 2, + "requires_waist_bending": true + }, + "rotation_movements": { + "left_hand_pose": [ + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.2, 0.11, 0.8, 0.6946, 0.1325, 0.1325, 0.6946], + [-0.2, 0.11, 0.8, 0.6533, 0.2706, 0.2706, 0.6533], + [-0.2, 0.11, 0.8, 0.5848, 0.3975, 0.3975, 0.5848], + [-0.2, 0.11, 0.8, 0.5, 0.5, 0.5, 0.5], + [-0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [-0.2, 0.11, 0.8, 0.6946, -0.1325, -0.1325, 0.6946], + [-0.2, 0.11, 0.8, 0.6533, -0.2706, -0.2706, 0.6533], + [-0.2, 0.11, 0.8, 0.5848, -0.3975, -0.3975, 0.5848], + [-0.2, 0.11, 0.8, 0.5, -0.5, -0.5, 0.5] + ], + "right_hand_pose": [ + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.2, 0.11, 0.8, 0.6946, -0.1325, -0.1325, 0.6946], + [0.2, 0.11, 0.8, 0.6533, -0.2706, -0.2706, 0.6533], + [0.2, 0.11, 0.8, 0.5848, -0.3975, -0.3975, 0.5848], + [0.2, 0.11, 0.8, 0.5, -0.5, -0.5, 0.5], + [0.18, 0.1, 0.8, 0.7071, 0.0, 0.0, 0.7071], + [0.2, 0.11, 0.8, 0.6946, 0.1325, 0.1325, 0.6946], + [0.2, 0.11, 0.8, 0.6533, 0.2706, 0.2706, 0.6533], + [0.2, 0.11, 0.8, 0.5848, 0.3975, 0.3975, 0.5848], + [0.2, 0.11, 0.8, 0.5, 0.5, 0.5, 0.5] + ], + "allowed_steps_per_motion": 25, + "repeat": 2, + "requires_waist_bending": false + } + } +} diff --git a/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_gr1_test_configs.json similarity index 76% rename from source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json rename to source/isaaclab/test/controllers/test_ik_configs/pink_ik_gr1_test_configs.json index b033b95b81f6..be40d7cf7abc 100644 --- a/source/isaaclab/test/controllers/test_configs/pink_ik_gr1_test_configs.json +++ b/source/isaaclab/test/controllers/test_ik_configs/pink_ik_gr1_test_configs.json @@ -5,30 +5,33 @@ "rotation": 0.02, "check_errors": true }, + "allowed_steps_to_settle": 5, "tests": { - "stay_still": { + "vertical_movement": { "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] + [-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.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": 10, - "repeat": 2 + "allowed_steps_per_motion": 8, + "repeat": 2, + "requires_waist_bending": false }, - "vertical_movement": { + "stay_still": { "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] + [-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.32, 1.2, 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": 15, - "repeat": 2 + "allowed_steps_per_motion": 8, + "repeat": 4, + "requires_waist_bending": false }, "horizontal_movement": { "left_hand_pose": [ @@ -39,8 +42,9 @@ [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 + "allowed_steps_per_motion": 8, + "repeat": 2, + "requires_waist_bending": false }, "horizontal_small_movement": { "left_hand_pose": [ @@ -51,8 +55,9 @@ [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 + "allowed_steps_per_motion": 8, + "repeat": 2, + "requires_waist_bending": false }, "forward_waist_bending_movement": { "left_hand_pose": [ @@ -63,24 +68,26 @@ [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 + "allowed_steps_per_motion": 25, + "repeat": 3, + "requires_waist_bending": true }, "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] + [-0.23, 0.32, 1.1, 0.0, 0.0, -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.32, 1.1, 0.0, 0.0, -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 + "allowed_steps_per_motion": 10, + "repeat": 2, + "requires_waist_bending": false } } } diff --git a/source/isaaclab/test/controllers/test_local_frame_task.py b/source/isaaclab/test/controllers/test_local_frame_task.py new file mode 100644 index 000000000000..48c86eec0826 --- /dev/null +++ b/source/isaaclab/test/controllers/test_local_frame_task.py @@ -0,0 +1,481 @@ +# 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 + +"""Test cases for LocalFrameTask class.""" +# 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 + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import numpy as np +from pathlib import Path + +import pinocchio as pin +import pytest + +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask +from isaaclab.controllers.pink_ik.pink_kinematics_configuration import PinkKinematicsConfiguration + +# class TestLocalFrameTask: +# """Test suite for LocalFrameTask class.""" + + +@pytest.fixture +def urdf_path(): + """Path to test URDF file.""" + return Path(__file__).parent / "urdfs" / "test_urdf_two_link_robot.urdf" + + +@pytest.fixture +def controlled_joint_names(): + """List of controlled joint names for testing.""" + return ["joint_1", "joint_2"] + + +@pytest.fixture +def pink_config(urdf_path, controlled_joint_names): + """Create a PinkKinematicsConfiguration instance for testing.""" + return PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + controlled_joint_names=controlled_joint_names, + # copy_data=True, + # forward_kinematics=True, + ) + + +@pytest.fixture +def local_frame_task(): + """Create a LocalFrameTask instance for testing.""" + return LocalFrameTask( + frame="link_2", + base_link_frame_name="base_link", + position_cost=1.0, + orientation_cost=1.0, + lm_damping=0.0, + gain=1.0, + ) + + +def test_initialization(local_frame_task): + """Test proper initialization of LocalFrameTask.""" + # Check that the task is properly initialized + assert local_frame_task.frame == "link_2" + assert local_frame_task.base_link_frame_name == "base_link" + assert np.allclose(local_frame_task.cost[:3], [1.0, 1.0, 1.0]) + assert np.allclose(local_frame_task.cost[3:], [1.0, 1.0, 1.0]) + assert local_frame_task.lm_damping == 0.0 + assert local_frame_task.gain == 1.0 + + # Check that target is initially None + assert local_frame_task.transform_target_to_base is None + + +def test_initialization_with_sequence_costs(): + """Test initialization with sequence costs.""" + task = LocalFrameTask( + frame="link_1", + base_link_frame_name="base_link", + position_cost=[1.0, 1.0, 1.0], + orientation_cost=[1.0, 1.0, 1.0], + lm_damping=0.1, + gain=2.0, + ) + + assert task.frame == "link_1" + assert task.base_link_frame_name == "base_link" + assert np.allclose(task.cost[:3], [1.0, 1.0, 1.0]) + assert np.allclose(task.cost[3:], [1.0, 1.0, 1.0]) + assert task.lm_damping == 0.1 + assert task.gain == 2.0 + + +def test_inheritance_from_frame_task(local_frame_task): + """Test that LocalFrameTask properly inherits from FrameTask.""" + from pink.tasks.frame_task import FrameTask + + # Check inheritance + assert isinstance(local_frame_task, FrameTask) + + # Check that we can call parent class methods + assert hasattr(local_frame_task, "compute_error") + assert hasattr(local_frame_task, "compute_jacobian") + + +def test_set_target(local_frame_task): + """Test setting target with a transform.""" + # Create a test transform + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + target_transform.rotation = pin.exp3(np.array([0.1, 0.0, 0.0])) + + # Set the target + local_frame_task.set_target(target_transform) + + # Check that target was set correctly + assert local_frame_task.transform_target_to_base is not None + assert isinstance(local_frame_task.transform_target_to_base, pin.SE3) + + # Check that it's a copy (not the same object) + assert local_frame_task.transform_target_to_base is not target_transform + + # Check that values match + assert np.allclose(local_frame_task.transform_target_to_base.translation, target_transform.translation) + assert np.allclose(local_frame_task.transform_target_to_base.rotation, target_transform.rotation) + + +def test_set_target_from_configuration(local_frame_task, pink_config): + """Test setting target from a robot configuration.""" + # Set target from configuration + local_frame_task.set_target_from_configuration(pink_config) + + # Check that target was set + assert local_frame_task.transform_target_to_base is not None + assert isinstance(local_frame_task.transform_target_to_base, pin.SE3) + + +def test_set_target_from_configuration_wrong_type(local_frame_task): + """Test that set_target_from_configuration raises error with wrong type.""" + with pytest.raises(ValueError, match="configuration must be a PinkKinematicsConfiguration"): + local_frame_task.set_target_from_configuration("not_a_configuration") + + +def test_compute_error_with_target_set(local_frame_task, pink_config): + """Test computing error when target is set.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute error + error = local_frame_task.compute_error(pink_config) + + # Check that error is computed correctly + assert isinstance(error, np.ndarray) + assert error.shape == (6,) # 6D error (3 position + 3 orientation) + + # Error should not be all zeros (unless target exactly matches current pose) + # This is a reasonable assumption for a random target + + +def test_compute_error_without_target(local_frame_task, pink_config): + """Test that compute_error raises error when no target is set.""" + with pytest.raises(ValueError, match="no target set for frame 'link_2'"): + local_frame_task.compute_error(pink_config) + + +def test_compute_error_wrong_configuration_type(local_frame_task): + """Test that compute_error raises error with wrong configuration type.""" + # Set a target first + target_transform = pin.SE3.Identity() + local_frame_task.set_target(target_transform) + + with pytest.raises(ValueError, match="configuration must be a PinkKinematicsConfiguration"): + local_frame_task.compute_error("not_a_configuration") + + +def test_compute_jacobian_with_target_set(local_frame_task, pink_config): + """Test computing Jacobian when target is set.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute Jacobian + jacobian = local_frame_task.compute_jacobian(pink_config) + + # Check that Jacobian is computed correctly + assert isinstance(jacobian, np.ndarray) + assert jacobian.shape == (6, 2) # 6 rows (error), 2 columns (controlled joints) + + # Jacobian should not be all zeros + assert not np.allclose(jacobian, 0.0) + + +def test_compute_jacobian_without_target(local_frame_task, pink_config): + """Test that compute_jacobian raises error when no target is set.""" + with pytest.raises(Exception, match="no target set for frame 'link_2'"): + local_frame_task.compute_jacobian(pink_config) + + +def test_error_consistency_across_configurations(local_frame_task, pink_config): + """Test that error computation is consistent across different configurations.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute error at initial configuration + error_1 = local_frame_task.compute_error(pink_config) + + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.5 # Change first revolute joint + pink_config.update(new_q) + + # Compute error at new configuration + error_2 = local_frame_task.compute_error(pink_config) + + # Errors should be different (not all close) + assert not np.allclose(error_1, error_2) + + +def test_jacobian_consistency_across_configurations(local_frame_task, pink_config): + """Test that Jacobian computation is consistent across different configurations.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute Jacobian at initial configuration + jacobian_1 = local_frame_task.compute_jacobian(pink_config) + + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.3 # Change first revolute joint + pink_config.update(new_q) + + # Compute Jacobian at new configuration + jacobian_2 = local_frame_task.compute_jacobian(pink_config) + + # Jacobians should be different (not all close) + assert not np.allclose(jacobian_1, jacobian_2) + + +def test_error_zero_at_target_pose(local_frame_task, pink_config): + """Test that error is zero when current pose matches target pose.""" + # Get current transform of the frame + current_transform = pink_config.get_transform_frame_to_world("link_2") + + # Set target to current pose + local_frame_task.set_target(current_transform) + + # Compute error + error = local_frame_task.compute_error(pink_config) + + # Error should be very close to zero + assert np.allclose(error, 0.0, atol=1e-10) + + +def test_different_frames(pink_config): + """Test LocalFrameTask with different frame names.""" + # Test with link_1 frame + task_link1 = LocalFrameTask( + frame="link_1", + base_link_frame_name="base_link", + position_cost=1.0, + orientation_cost=1.0, + ) + + # Set target and compute error + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.0, 0.0]) + task_link1.set_target(target_transform) + + error_link1 = task_link1.compute_error(pink_config) + assert error_link1.shape == (6,) + + # Test with base_link frame + task_base = LocalFrameTask( + frame="base_link", + base_link_frame_name="base_link", + position_cost=1.0, + orientation_cost=1.0, + ) + + task_base.set_target(target_transform) + error_base = task_base.compute_error(pink_config) + assert error_base.shape == (6,) + + +def test_different_base_frames(pink_config): + """Test LocalFrameTask with different base frame names.""" + # Test with base_link as base frame + task_base_base = LocalFrameTask( + frame="link_2", + base_link_frame_name="base_link", + position_cost=1.0, + orientation_cost=1.0, + ) + + target_transform = pin.SE3.Identity() + task_base_base.set_target(target_transform) + error_base_base = task_base_base.compute_error(pink_config) + assert error_base_base.shape == (6,) + + # Test with link_1 as base frame + task_link1_base = LocalFrameTask( + frame="link_2", + base_link_frame_name="link_1", + position_cost=1.0, + orientation_cost=1.0, + ) + + task_link1_base.set_target(target_transform) + error_link1_base = task_link1_base.compute_error(pink_config) + assert error_link1_base.shape == (6,) + + +def test_sequence_cost_parameters(): + """Test LocalFrameTask with sequence cost parameters.""" + task = LocalFrameTask( + frame="link_2", + base_link_frame_name="base_link", + position_cost=[1.0, 2.0, 3.0], + orientation_cost=[0.5, 1.0, 1.5], + lm_damping=0.1, + gain=2.0, + ) + + assert np.allclose(task.cost[:3], [1.0, 2.0, 3.0]) # Position costs + assert np.allclose(task.cost[3:], [0.5, 1.0, 1.5]) # Orientation costs + assert task.lm_damping == 0.1 + assert task.gain == 2.0 + + +def test_error_magnitude_consistency(local_frame_task, pink_config): + """Test that error computation produces reasonable results.""" + # Set a small target offset + small_target = pin.SE3.Identity() + small_target.translation = np.array([0.01, 0.01, 0.01]) + local_frame_task.set_target(small_target) + + error_small = local_frame_task.compute_error(pink_config) + + # Set a large target offset + large_target = pin.SE3.Identity() + large_target.translation = np.array([0.5, 0.5, 0.5]) + local_frame_task.set_target(large_target) + + error_large = local_frame_task.compute_error(pink_config) + + # Both errors should be finite and reasonable + assert np.all(np.isfinite(error_small)) + assert np.all(np.isfinite(error_large)) + assert not np.allclose(error_small, error_large) # Different targets should produce different errors + + +def test_jacobian_structure(local_frame_task, pink_config): + """Test that Jacobian has the correct structure.""" + # Set a target + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.1, 0.2, 0.3]) + local_frame_task.set_target(target_transform) + + # Compute Jacobian + jacobian = local_frame_task.compute_jacobian(pink_config) + + # Check structure + assert jacobian.shape == (6, 2) # 6 error dimensions, 2 controlled joints + + # Check that Jacobian is not all zeros (basic functionality check) + assert not np.allclose(jacobian, 0.0) + + +def test_multiple_target_updates(local_frame_task, pink_config): + """Test that multiple target updates work correctly.""" + # Set first target + target1 = pin.SE3.Identity() + target1.translation = np.array([0.1, 0.0, 0.0]) + local_frame_task.set_target(target1) + + error1 = local_frame_task.compute_error(pink_config) + + # Set second target + target2 = pin.SE3.Identity() + target2.translation = np.array([0.0, 0.1, 0.0]) + local_frame_task.set_target(target2) + + error2 = local_frame_task.compute_error(pink_config) + + # Errors should be different + assert not np.allclose(error1, error2) + + +def test_inheritance_behavior(local_frame_task): + """Test that LocalFrameTask properly overrides parent class methods.""" + # Check that the class has the expected methods + assert hasattr(local_frame_task, "set_target") + assert hasattr(local_frame_task, "set_target_from_configuration") + assert hasattr(local_frame_task, "compute_error") + assert hasattr(local_frame_task, "compute_jacobian") + + # Check that these are the overridden methods, not the parent ones + assert local_frame_task.set_target.__qualname__ == "LocalFrameTask.set_target" + assert local_frame_task.compute_error.__qualname__ == "LocalFrameTask.compute_error" + assert local_frame_task.compute_jacobian.__qualname__ == "LocalFrameTask.compute_jacobian" + + +def test_target_copying_behavior(local_frame_task): + """Test that target transforms are properly copied.""" + # Create a target transform + original_target = pin.SE3.Identity() + original_target.translation = np.array([0.1, 0.2, 0.3]) + original_rotation = original_target.rotation.copy() + + # Set the target + local_frame_task.set_target(original_target) + + # Modify the original target + original_target.translation = np.array([0.5, 0.5, 0.5]) + original_target.rotation = pin.exp3(np.array([0.5, 0.0, 0.0])) + + # Check that the stored target is unchanged + assert np.allclose(local_frame_task.transform_target_to_base.translation, np.array([0.1, 0.2, 0.3])) + assert np.allclose(local_frame_task.transform_target_to_base.rotation, original_rotation) + + +def test_error_computation_with_orientation_difference(local_frame_task, pink_config): + """Test error computation when there's an orientation difference.""" + # Set a target with orientation difference + target_transform = pin.SE3.Identity() + target_transform.rotation = pin.exp3(np.array([0.2, 0.0, 0.0])) # Rotation around X-axis + local_frame_task.set_target(target_transform) + + # Compute error + error = local_frame_task.compute_error(pink_config) + + # Check that error is computed correctly + assert isinstance(error, np.ndarray) + assert error.shape == (6,) + + # Error should not be all zeros + assert not np.allclose(error, 0.0) + + +def test_jacobian_rank_consistency(local_frame_task, pink_config): + """Test that Jacobian maintains consistent shape across configurations.""" + # Set a target that we know can be reached by the test robot. + target_transform = pin.SE3.Identity() + target_transform.translation = np.array([0.0, 0.0, 0.45]) + # 90 degrees around x axis = pi/2 radians + target_transform.rotation = pin.exp3(np.array([np.pi / 2, 0.0, 0.0])) + local_frame_task.set_target(target_transform) + + # Compute Jacobian at multiple configurations + jacobians = [] + for i in range(5): + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.1 * i # Vary first joint + pink_config.update(new_q) + + # Compute Jacobian + jacobian = local_frame_task.compute_jacobian(pink_config) + jacobians.append(jacobian) + + # All Jacobians should have the same shape + for jacobian in jacobians: + assert jacobian.shape == (6, 2) + + # All Jacobians should have rank 2 (full rank for 2-DOF planar arm) + for jacobian in jacobians: + assert np.linalg.matrix_rank(jacobian) == 2 diff --git a/source/isaaclab/test/controllers/test_pink_ik.py b/source/isaaclab/test/controllers/test_pink_ik.py index 3485f367e373..46f610c42f51 100644 --- a/source/isaaclab/test/controllers/test_pink_ik.py +++ b/source/isaaclab/test/controllers/test_pink_ik.py @@ -22,43 +22,55 @@ import gymnasium as gym import json import numpy as np -import os +import re import torch +from pathlib import Path +import omni.usd import pytest from pink.configuration import Configuration +from pink.tasks import FrameTask from isaaclab.utils.math import axis_angle_from_quat, matrix_from_quat, quat_from_matrix, quat_inv import isaaclab_tasks # noqa: F401 +import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 from isaaclab_tasks.utils.parse_cfg import parse_env_cfg -@pytest.fixture(scope="module") -def test_cfg(): - """Load test configuration.""" - config_path = os.path.join(os.path.dirname(__file__), "test_configs", "pink_ik_gr1_test_configs.json") +def load_test_config(env_name): + """Load test configuration based on environment type.""" + # Determine which config file to load based on environment name + if "G1" in env_name: + config_file = "pink_ik_g1_test_configs.json" + elif "GR1" in env_name: + config_file = "pink_ik_gr1_test_configs.json" + else: + raise ValueError(f"Unknown environment type in {env_name}. Expected G1 or GR1.") + + config_path = Path(__file__).parent / "test_ik_configs" / config_file 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 is_waist_enabled(env_cfg): + """Check if waist joints are enabled in the environment configuration.""" + if not hasattr(env_cfg.actions, "upper_body_ik"): + return False + + pink_controlled_joints = env_cfg.actions.upper_body_ik.pink_controlled_joint_names + + # Also check for pattern-based joint names (e.g., "waist_.*_joint") + return any(re.match("waist", joint) for joint in pink_controlled_joints) -def create_test_env(num_envs): +def create_test_env(env_name, num_envs): """Create a test environment with the Pink IK controller.""" - env_name = "Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0" device = "cuda:0" + omni.usd.get_context().new_stage() + 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 @@ -71,85 +83,133 @@ def create_test_env(num_envs): raise -@pytest.fixture(scope="module") -def env_and_cfg(): +@pytest.fixture( + scope="module", + params=[ + "Isaac-PickPlace-GR1T2-Abs-v0", + "Isaac-PickPlace-GR1T2-WaistEnabled-Abs-v0", + "Isaac-PickPlace-FixedBaseUpperBodyIK-G1-Abs-v0", + "Isaac-PickPlace-Locomanipulation-G1-Abs-v0", + ], +) +def env_and_cfg(request): """Create environment and configuration for tests.""" - env, env_cfg = create_test_env(num_envs=1) + env_name = request.param + + # Load the appropriate test configuration based on environment type + test_cfg = load_test_config(env_name) + + env, env_cfg = create_test_env(env_name, num_envs=1) + + # Get only the FrameTasks from variable_input_tasks + variable_input_tasks = [ + task for task in env_cfg.actions.upper_body_ik.controller.variable_input_tasks if isinstance(task, FrameTask) + ] + assert len(variable_input_tasks) == 2, "Expected exactly two FrameTasks (left and right hand)." + frames = [task.frame for task in variable_input_tasks] + # Try to infer which is left and which is right + left_candidates = [f for f in frames if "left" in f.lower()] + right_candidates = [f for f in frames if "right" in f.lower()] + assert ( + len(left_candidates) == 1 and len(right_candidates) == 1 + ), f"Could not uniquely identify left/right frames from: {frames}" + left_eef_urdf_link_name = left_candidates[0] + right_eef_urdf_link_name = right_candidates[0] # 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 + # Create test parameters from test_cfg + test_params = { + "position": test_cfg["tolerances"]["position"], + "rotation": test_cfg["tolerances"]["rotation"], + "pd_position": test_cfg["tolerances"]["pd_position"], + "check_errors": test_cfg["tolerances"]["check_errors"], + "left_eef_urdf_link_name": left_eef_urdf_link_name, + "right_eef_urdf_link_name": right_eef_urdf_link_name, + } + + try: + yield env, env_cfg, test_cfg, test_params + finally: + env.close() @pytest.fixture def test_setup(env_and_cfg): """Set up test case - runs before each test.""" - env, env_cfg = env_and_cfg + env, env_cfg, test_cfg, test_params = env_and_cfg - num_joints_in_robot_hands = env_cfg.actions.pink_ik_cfg.controller.num_hand_joints + num_joints_in_robot_hands = env_cfg.actions.upper_body_ik.controller.num_hand_joints # Get Action Term and IK controller - action_term = env.action_manager.get_term(name="pink_ik_cfg") + action_term = env.action_manager.get_term(name="upper_body_ik") 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, + test_kinematics_model = Configuration( + pink_controllers[0].pink_configuration.model, + pink_controllers[0].pink_configuration.data, + pink_controllers[0].pink_configuration.q, ) - 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"] + left_target_link_name = env_cfg.actions.upper_body_ik.target_eef_link_names["left_wrist"] + right_target_link_name = env_cfg.actions.upper_body_ik.target_eef_link_names["right_wrist"] return { "env": env, "env_cfg": env_cfg, + "test_cfg": test_cfg, + "test_params": test_params, "num_joints_in_robot_hands": num_joints_in_robot_hands, "action_term": action_term, "pink_controllers": pink_controllers, "articulation": articulation, - "kinematics_model": kinematics_model, + "test_kinematics_model": test_kinematics_model, "left_target_link_name": left_target_link_name, "right_target_link_name": right_target_link_name, + "left_eef_urdf_link_name": test_params["left_eef_urdf_link_name"], + "right_eef_urdf_link_name": test_params["right_eef_urdf_link_name"], } -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) - - -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) - - -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) - - -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) - - -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) - +@pytest.mark.parametrize( + "test_name", + [ + "horizontal_movement", + "horizontal_small_movement", + "stay_still", + "forward_waist_bending_movement", + "vertical_movement", + "rotation_movements", + ], +) +def test_movement_types(test_setup, test_name): + """Test different movement types using parametrization.""" + test_cfg = test_setup["test_cfg"] + env_cfg = test_setup["env_cfg"] + + if test_name not in test_cfg["tests"]: + print(f"Skipping {test_name} test for {env_cfg.__class__.__name__} environment (test not defined)...") + pytest.skip(f"Test {test_name} not defined for {env_cfg.__class__.__name__}") + return + + test_config = test_cfg["tests"][test_name] + + # Check if test requires waist bending and if waist is enabled + requires_waist_bending = test_config.get("requires_waist_bending", False) + waist_enabled = is_waist_enabled(env_cfg) + + if requires_waist_bending and not waist_enabled: + print( + f"Skipping {test_name} test because it requires waist bending but waist is not enabled in" + f" {env_cfg.__class__.__name__}..." + ) + pytest.skip(f"Test {test_name} requires waist bending but waist is not enabled") + return -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) + print(f"Running {test_name} test...") + run_movement_test(test_setup, test_config, test_cfg) def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): @@ -167,8 +227,14 @@ def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode(): obs, _ = env.reset() + # Make the first phase longer than subsequent ones + initial_steps = test_cfg["allowed_steps_to_settle"] + phase = "initial" + steps_in_phase = 0 + while simulation_app.is_running() and not simulation_app.is_exiting(): num_runs += 1 + steps_in_phase += 1 # Call auxiliary function if provided if aux_function is not None: @@ -178,20 +244,40 @@ def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): 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) + # Append base command for Locomanipulation environments with fixed height + if test_setup["env_cfg"].__class__.__name__ == "LocomanipulationG1EnvCfg": + # Use a named variable for base height for clarity and maintainability + BASE_HEIGHT = 0.72 + base_command = torch.zeros(4, device=env.device, dtype=actions.dtype) + base_command[3] = BASE_HEIGHT + actions = torch.cat([actions, base_command]) actions = actions.repeat(env.num_envs, 1) # Step environment obs, _, _, _, _ = env.step(actions) + # Determine the step interval for error checking + if phase == "initial": + check_interval = initial_steps + else: + check_interval = test_config["allowed_steps_per_motion"] + # Check convergence and verify errors - if num_runs % test_config["allowed_steps_per_motion"] == 0: + if steps_in_phase % check_interval == 0: print("Computing errors...") errors = compute_errors( - test_setup, env, left_hand_poses[curr_pose_idx], right_hand_poses[curr_pose_idx] + test_setup, + env, + left_hand_poses[curr_pose_idx], + right_hand_poses[curr_pose_idx], + test_setup["left_eef_urdf_link_name"], + test_setup["right_eef_urdf_link_name"], ) print_debug_info(errors, test_counter) - if test_cfg["tolerances"]["check_errors"]: - verify_errors(errors, test_setup, test_cfg["tolerances"]) + test_params = test_setup["test_params"] + if test_params["check_errors"]: + verify_errors(errors, test_setup, test_params) + num_runs += 1 curr_pose_idx = (curr_pose_idx + 1) % len(left_hand_poses) if curr_pose_idx == 0: @@ -199,6 +285,10 @@ def run_movement_test(test_setup, test_config, test_cfg, aux_function=None): if test_counter > test_config["repeat"]: print("Test completed successfully") break + # After the first phase, switch to normal interval + if phase == "initial": + phase = "normal" + steps_in_phase = 0 def get_link_pose(env, link_name): @@ -225,15 +315,16 @@ def calculate_rotation_error(current_rot, target_rot): ) -def compute_errors(test_setup, env, left_target_pose, right_target_pose): +def compute_errors( + test_setup, env, left_target_pose, right_target_pose, left_eef_urdf_link_name, right_eef_urdf_link_name +): """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"] + test_kinematics_model = test_setup["test_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) @@ -244,10 +335,6 @@ def compute_errors(test_setup, env, left_target_pose, right_target_pose): 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 @@ -257,32 +344,24 @@ def compute_errors(test_setup, env, left_target_pose, right_target_pose): # Calculate PD controller errors ik_controller = pink_controllers[0] - pink_controlled_joint_ids = action_term._pink_controlled_joint_ids + isaaclab_controlled_joint_ids = action_term._isaaclab_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] + # Get current and target positions for controlled joints only + curr_joints = articulation.data.joint_pos[:, isaaclab_controlled_joint_ids].cpu().numpy()[0] + target_joints = action_term.processed_actions[:, : len(isaaclab_controlled_joint_ids)].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] + # Reorder joints for Pink IK (using controlled joint ordering) + curr_joints = np.array(curr_joints)[ik_controller.isaac_lab_to_pink_controlled_ordering] + target_joints = np.array(target_joints)[ik_controller.isaac_lab_to_pink_controlled_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 + test_kinematics_model.update(curr_joints) + left_curr_pos = test_kinematics_model.get_transform_frame_to_world(frame=left_eef_urdf_link_name).translation + right_curr_pos = test_kinematics_model.get_transform_frame_to_world(frame=right_eef_urdf_link_name).translation + + test_kinematics_model.update(target_joints) + left_target_pos = test_kinematics_model.get_transform_frame_to_world(frame=left_eef_urdf_link_name).translation + right_target_pos = test_kinematics_model.get_transform_frame_to_world(frame=right_eef_urdf_link_name).translation # Calculate PD errors left_pd_error = ( diff --git a/source/isaaclab/test/controllers/test_pink_ik_components.py b/source/isaaclab/test/controllers/test_pink_ik_components.py new file mode 100644 index 000000000000..6a691c353b2d --- /dev/null +++ b/source/isaaclab/test/controllers/test_pink_ik_components.py @@ -0,0 +1,307 @@ +# 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 + +"""Test cases for PinkKinematicsConfiguration class.""" +# 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 + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +import numpy as np +from pathlib import Path + +import pinocchio as pin +import pytest +from pink.exceptions import FrameNotFound + +from isaaclab.controllers.pink_ik.pink_kinematics_configuration import PinkKinematicsConfiguration + + +class TestPinkKinematicsConfiguration: + """Test suite for PinkKinematicsConfiguration class.""" + + @pytest.fixture + def urdf_path(self): + """Path to test URDF file.""" + return Path(__file__).parent / "urdfs/test_urdf_two_link_robot.urdf" + + @pytest.fixture + def mesh_path(self): + """Path to mesh directory (empty for simple test).""" + return "" + + @pytest.fixture + def controlled_joint_names(self): + """List of controlled joint names for testing.""" + return ["joint_1", "joint_2"] + + @pytest.fixture + def pink_config(self, urdf_path, mesh_path, controlled_joint_names): + """Create a PinkKinematicsConfiguration instance for testing.""" + return PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=controlled_joint_names, + copy_data=True, + forward_kinematics=True, + ) + + def test_initialization(self, pink_config, controlled_joint_names): + """Test proper initialization of PinkKinematicsConfiguration.""" + # Check that controlled joint names are stored correctly + assert pink_config._controlled_joint_names == controlled_joint_names + + # Check that both full and controlled models are created + assert pink_config.full_model is not None + assert pink_config.controlled_model is not None + assert pink_config.full_data is not None + assert pink_config.controlled_data is not None + + # Check that configuration vectors are initialized + assert pink_config.full_q is not None + assert pink_config.controlled_q is not None + + # Check that the controlled model has the same number or fewer joints than the full model + assert pink_config.controlled_model.nq == pink_config.full_model.nq + + def test_joint_names_properties(self, pink_config): + """Test joint name properties.""" + # Test controlled joint names in pinocchio order + controlled_names = pink_config.controlled_joint_names_pinocchio_order + assert isinstance(controlled_names, list) + assert len(controlled_names) == len(pink_config._controlled_joint_names) + assert "joint_1" in controlled_names + assert "joint_2" in controlled_names + + # Test all joint names in pinocchio order + all_names = pink_config.all_joint_names_pinocchio_order + assert isinstance(all_names, list) + assert len(all_names) == len(controlled_names) + assert "joint_1" in all_names + assert "joint_2" in all_names + + def test_update_with_valid_configuration(self, pink_config): + """Test updating configuration with valid joint values.""" + # Get initial configuration + initial_q = pink_config.full_q.copy() + + # Create a new configuration with different joint values + new_q = initial_q.copy() + new_q[1] = 0.5 # Change first revolute joint value (index 1, since 0 is fixed joint) + + # Update configuration + pink_config.update(new_q) + + # Check that the configuration was updated + print(pink_config.full_q) + assert not np.allclose(pink_config.full_q, initial_q) + assert np.allclose(pink_config.full_q, new_q) + + def test_update_with_none(self, pink_config): + """Test updating configuration with None (should use current configuration).""" + # Get initial configuration + initial_q = pink_config.full_q.copy() + + # Update with None + pink_config.update(None) + + # Configuration should remain the same + assert np.allclose(pink_config.full_q, initial_q) + + def test_update_with_wrong_dimensions(self, pink_config): + """Test that update raises ValueError with wrong configuration dimensions.""" + # Create configuration with wrong number of joints + wrong_q = np.array([0.1, 0.2, 0.3]) # Wrong number of joints + + with pytest.raises(ValueError, match="q must have the same length as the number of joints"): + pink_config.update(wrong_q) + + def test_get_frame_jacobian_existing_frame(self, pink_config): + """Test getting Jacobian for an existing frame.""" + # Get Jacobian for link_1 frame + jacobian = pink_config.get_frame_jacobian("link_1") + + # Check that Jacobian has correct shape + # Should be 6 rows (linear + angular velocity) and columns equal to controlled joints + expected_rows = 6 + expected_cols = len(pink_config._controlled_joint_names) + assert jacobian.shape == (expected_rows, expected_cols) + + # Check that Jacobian is not all zeros (should have some non-zero values) + assert not np.allclose(jacobian, 0.0) + + def test_get_frame_jacobian_nonexistent_frame(self, pink_config): + """Test that get_frame_jacobian raises FrameNotFound for non-existent frame.""" + with pytest.raises(FrameNotFound): + pink_config.get_frame_jacobian("nonexistent_frame") + + def test_get_transform_frame_to_world_existing_frame(self, pink_config): + """Test getting transform for an existing frame.""" + # Get transform for link_1 frame + transform = pink_config.get_transform_frame_to_world("link_1") + + # Check that transform is a pinocchio SE3 object + assert isinstance(transform, pin.SE3) + + # Check that transform has reasonable values (not identity for non-zero joint angles) + assert not np.allclose(transform.homogeneous, np.eye(4)) + + def test_get_transform_frame_to_world_nonexistent_frame(self, pink_config): + """Test that get_transform_frame_to_world raises FrameNotFound for non-existent frame.""" + with pytest.raises(FrameNotFound): + pink_config.get_transform_frame_to_world("nonexistent_frame") + + def test_multiple_controlled_joints(self, urdf_path, mesh_path): + """Test configuration with multiple controlled joints.""" + # Create configuration with all available joints as controlled + controlled_joint_names = ["joint_1", "joint_2"] # Both revolute joints + + pink_config = PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=controlled_joint_names, + ) + + # Check that controlled model has correct number of joints + assert pink_config.controlled_model.nq == len(controlled_joint_names) + + def test_no_controlled_joints(self, urdf_path, mesh_path): + """Test configuration with no controlled joints.""" + controlled_joint_names = [] + + pink_config = PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=controlled_joint_names, + ) + + # Check that controlled model has 0 joints + assert pink_config.controlled_model.nq == 0 + assert len(pink_config.controlled_q) == 0 + + def test_jacobian_consistency(self, pink_config): + """Test that Jacobian computation is consistent across updates.""" + # Get Jacobian at initial configuration + jacobian_1 = pink_config.get_frame_jacobian("link_2") + + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.3 # Change first revolute joint (index 1, since 0 is fixed joint) + pink_config.update(new_q) + + # Get Jacobian at new configuration + jacobian_2 = pink_config.get_frame_jacobian("link_2") + + # Jacobians should be different (not all close) + assert not np.allclose(jacobian_1, jacobian_2) + + def test_transform_consistency(self, pink_config): + """Test that transform computation is consistent across updates.""" + # Get transform at initial configuration + transform_1 = pink_config.get_transform_frame_to_world("link_2") + + # Update configuration + new_q = pink_config.full_q.copy() + new_q[1] = 0.5 # Change first revolute joint (index 1, since 0 is fixed joint) + pink_config.update(new_q) + + # Get transform at new configuration + transform_2 = pink_config.get_transform_frame_to_world("link_2") + + # Transforms should be different + assert not np.allclose(transform_1.homogeneous, transform_2.homogeneous) + + def test_inheritance_from_configuration(self, pink_config): + """Test that PinkKinematicsConfiguration properly inherits from Pink Configuration.""" + from pink.configuration import Configuration + + # Check inheritance + assert isinstance(pink_config, Configuration) + + # Check that we can call parent class methods + assert hasattr(pink_config, "update") + assert hasattr(pink_config, "get_transform_frame_to_world") + + def test_controlled_joint_indices_calculation(self, pink_config): + """Test that controlled joint indices are calculated correctly.""" + # Check that controlled joint indices are valid + assert len(pink_config._controlled_joint_indices) == len(pink_config._controlled_joint_names) + + # Check that all indices are within bounds + for idx in pink_config._controlled_joint_indices: + assert 0 <= idx < len(pink_config._all_joint_names) + + # Check that indices correspond to controlled joint names + for i, idx in enumerate(pink_config._controlled_joint_indices): + joint_name = pink_config._all_joint_names[idx] + assert joint_name in pink_config._controlled_joint_names + + def test_full_model_integrity(self, pink_config): + """Test that the full model maintains integrity.""" + # Check that full model has all joints + assert pink_config.full_model.nq > 0 + assert len(pink_config.full_model.names) > 1 # More than just "universe" + + def test_controlled_model_integrity(self, pink_config): + """Test that the controlled model maintains integrity.""" + # Check that controlled model has correct number of joints + assert pink_config.controlled_model.nq == len(pink_config._controlled_joint_names) + + def test_configuration_vector_consistency(self, pink_config): + """Test that configuration vectors are consistent between full and controlled models.""" + # Check that controlled_q is a subset of full_q + controlled_indices = pink_config._controlled_joint_indices + for i, idx in enumerate(controlled_indices): + assert np.isclose(pink_config.controlled_q[i], pink_config.full_q[idx]) + + def test_error_handling_invalid_urdf(self, mesh_path, controlled_joint_names): + """Test error handling with invalid URDF path.""" + with pytest.raises(Exception): # Should raise some exception for invalid URDF + PinkKinematicsConfiguration( + urdf_path="nonexistent.urdf", + mesh_path=mesh_path, + controlled_joint_names=controlled_joint_names, + ) + + def test_error_handling_invalid_joint_names(self, urdf_path, mesh_path): + """Test error handling with invalid joint names.""" + invalid_joint_names = ["nonexistent_joint"] + + # This should not raise an error, but the controlled model should have 0 joints + pink_config = PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=invalid_joint_names, + ) + + assert pink_config.controlled_model.nq == 0 + assert len(pink_config.controlled_q) == 0 + + def test_undercontrolled_kinematics_model(self, urdf_path, mesh_path): + """Test that the fixed joint to world is properly handled.""" + + test_model = PinkKinematicsConfiguration( + urdf_path=str(urdf_path), + mesh_path=mesh_path, + controlled_joint_names=["joint_1"], + copy_data=True, + forward_kinematics=True, + ) + # Check that the controlled model only includes the revolute joints + assert "joint_1" in test_model.controlled_joint_names_pinocchio_order + assert "joint_2" not in test_model.controlled_joint_names_pinocchio_order + assert len(test_model.controlled_joint_names_pinocchio_order) == 1 # Only the two revolute joints + + # Check that the full configuration has more elements than controlled + assert len(test_model.full_q) > len(test_model.controlled_q) + assert len(test_model.full_q) == len(test_model.all_joint_names_pinocchio_order) + assert len(test_model.controlled_q) == len(test_model.controlled_joint_names_pinocchio_order) diff --git a/source/isaaclab/test/controllers/urdfs/test_urdf_two_link_robot.urdf b/source/isaaclab/test/controllers/urdfs/test_urdf_two_link_robot.urdf new file mode 100644 index 000000000000..cb1a305c50da --- /dev/null +++ b/source/isaaclab/test/controllers/urdfs/test_urdf_two_link_robot.urdf @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/source/isaaclab/test/performance/test_kit_startup_performance.py b/source/isaaclab/test/performance/test_kit_startup_performance.py index 056b2e6293b1..dfa716cd0b23 100644 --- a/source/isaaclab/test/performance/test_kit_startup_performance.py +++ b/source/isaaclab/test/performance/test_kit_startup_performance.py @@ -10,12 +10,9 @@ import time -import pytest - from isaaclab.app import AppLauncher -@pytest.mark.isaacsim_ci def test_kit_start_up_time(): """Test kit start-up time.""" start_time = time.time() diff --git a/source/isaaclab/test/performance/test_robot_load_performance.py b/source/isaaclab/test/performance/test_robot_load_performance.py index 4acf8ad63314..bca8c36d9d5d 100644 --- a/source/isaaclab/test/performance/test_robot_load_performance.py +++ b/source/isaaclab/test/performance/test_robot_load_performance.py @@ -33,7 +33,6 @@ ({"name": "Anymal_D", "robot_cfg": ANYMAL_D_CFG, "expected_load_time": 40.0}, "cpu"), ], ) -@pytest.mark.isaacsim_ci def test_robot_load_performance(test_config, device): """Test robot load time.""" with build_simulation_context(device=device) as sim: diff --git a/source/isaaclab_assets/config/extension.toml b/source/isaaclab_assets/config/extension.toml index ccde51a7166d..dac5494087e0 100644 --- a/source/isaaclab_assets/config/extension.toml +++ b/source/isaaclab_assets/config/extension.toml @@ -1,6 +1,6 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "0.2.2" +version = "0.2.3" # Description title = "Isaac Lab Assets" diff --git a/source/isaaclab_assets/docs/CHANGELOG.rst b/source/isaaclab_assets/docs/CHANGELOG.rst index 85f70e7e8c33..b6582e77e8a2 100644 --- a/source/isaaclab_assets/docs/CHANGELOG.rst +++ b/source/isaaclab_assets/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ Changelog --------- +0.2.3 (2025-08-11) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Configuration for G1 robot used for locomanipulation tasks. + 0.2.2 (2025-03-10) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py index ab963aafff56..a8674c069a91 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py @@ -14,6 +14,7 @@ * :obj:`H1_MINIMAL_CFG`: H1 humanoid robot with minimal collision bodies * :obj:`G1_CFG`: G1 humanoid robot * :obj:`G1_MINIMAL_CFG`: G1 humanoid robot with minimal collision bodies +* :obj:`G1_29DOF_CFG`: G1 humanoid robot configured for locomanipulation tasks Reference: https://github.com/unitreerobotics/unitree_ros """ @@ -21,7 +22,7 @@ import isaaclab.sim as sim_utils from isaaclab.actuators import ActuatorNetMLPCfg, DCMotorCfg, ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg -from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR ## # Configuration - Actuators. @@ -381,3 +382,174 @@ This configuration removes most collision meshes to speed up simulation. """ + + +G1_29DOF_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Unitree/G1/g1.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + fix_root_link=False, # Configurable - can be set to True for fixed base + solver_position_iteration_count=8, + solver_velocity_iteration_count=4, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.75), + rot=(0.7071, 0, 0, 0.7071), + joint_pos={ + ".*_hip_pitch_joint": -0.10, + ".*_knee_joint": 0.30, + ".*_ankle_pitch_joint": -0.20, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": DCMotorCfg( + joint_names_expr=[ + ".*_hip_yaw_joint", + ".*_hip_roll_joint", + ".*_hip_pitch_joint", + ".*_knee_joint", + ], + effort_limit={ + ".*_hip_yaw_joint": 88.0, + ".*_hip_roll_joint": 88.0, + ".*_hip_pitch_joint": 88.0, + ".*_knee_joint": 139.0, + }, + velocity_limit={ + ".*_hip_yaw_joint": 32.0, + ".*_hip_roll_joint": 32.0, + ".*_hip_pitch_joint": 32.0, + ".*_knee_joint": 20.0, + }, + stiffness={ + ".*_hip_yaw_joint": 100.0, + ".*_hip_roll_joint": 100.0, + ".*_hip_pitch_joint": 100.0, + ".*_knee_joint": 200.0, + }, + damping={ + ".*_hip_yaw_joint": 2.5, + ".*_hip_roll_joint": 2.5, + ".*_hip_pitch_joint": 2.5, + ".*_knee_joint": 5.0, + }, + armature={ + ".*_hip_.*": 0.03, + ".*_knee_joint": 0.03, + }, + saturation_effort=180.0, + ), + "feet": DCMotorCfg( + joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], + stiffness={ + ".*_ankle_pitch_joint": 20.0, + ".*_ankle_roll_joint": 20.0, + }, + damping={ + ".*_ankle_pitch_joint": 0.2, + ".*_ankle_roll_joint": 0.1, + }, + effort_limit={ + ".*_ankle_pitch_joint": 50.0, + ".*_ankle_roll_joint": 50.0, + }, + velocity_limit={ + ".*_ankle_pitch_joint": 37.0, + ".*_ankle_roll_joint": 37.0, + }, + armature=0.03, + saturation_effort=80.0, + ), + "waist": ImplicitActuatorCfg( + joint_names_expr=[ + "waist_.*_joint", + ], + effort_limit={ + "waist_yaw_joint": 88.0, + "waist_roll_joint": 50.0, + "waist_pitch_joint": 50.0, + }, + velocity_limit={ + "waist_yaw_joint": 32.0, + "waist_roll_joint": 37.0, + "waist_pitch_joint": 37.0, + }, + stiffness={ + "waist_yaw_joint": 5000.0, + "waist_roll_joint": 5000.0, + "waist_pitch_joint": 5000.0, + }, + damping={ + "waist_yaw_joint": 5.0, + "waist_roll_joint": 5.0, + "waist_pitch_joint": 5.0, + }, + armature=0.001, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_.*_joint", + ], + effort_limit=300, + velocity_limit=100, + stiffness=3000.0, + damping=10.0, + armature={ + ".*_shoulder_.*": 0.001, + ".*_elbow_.*": 0.001, + ".*_wrist_.*_joint": 0.001, + }, + ), + "hands": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_index_.*", + ".*_middle_.*", + ".*_thumb_.*", + ], + effort_limit=300, + velocity_limit=100, + stiffness=4000, + damping=50, + armature=0.001, + ), + }, + prim_path="/World/envs/env_.*/Robot", +) +"""Configuration for the Unitree G1 Humanoid robot for locomanipulation tasks. + +This configuration sets up the G1 humanoid robot for locomanipulation tasks, +allowing both locomotion and manipulation capabilities. The robot can be configured +for either fixed base or mobile scenarios by modifying the fix_root_link parameter. + +Key features: +- Configurable base (fixed or mobile) via fix_root_link parameter +- Optimized actuator parameters for locomanipulation tasks +- Enhanced hand and arm configurations for manipulation + +Usage examples: + # For fixed base scenarios (upper body manipulation only) + fixed_base_cfg = G1_29DOF_CFG.copy() + fixed_base_cfg.spawn.articulation_props.fix_root_link = True + + # For mobile scenarios (locomotion + manipulation) + mobile_cfg = G1_29DOF_CFG.copy() + mobile_cfg.spawn.articulation_props.fix_root_link = False +""" 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 c782576c3630..7b6e491b6c6a 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/__init__.py @@ -8,6 +8,8 @@ import gymnasium as gym from .exhaustpipe_gr1t2_mimic_env_cfg import ExhaustPipeGR1T2MimicEnvCfg +from .locomanipulation_g1_mimic_env import LocomanipulationG1MimicEnv +from .locomanipulation_g1_mimic_env_cfg import LocomanipulationG1MimicEnvCfg from .nutpour_gr1t2_mimic_env_cfg import NutPourGR1T2MimicEnvCfg from .pickplace_gr1t2_mimic_env import PickPlaceGR1T2MimicEnv from .pickplace_gr1t2_mimic_env_cfg import PickPlaceGR1T2MimicEnvCfg @@ -44,3 +46,10 @@ kwargs={"env_cfg_entry_point": exhaustpipe_gr1t2_mimic_env_cfg.ExhaustPipeGR1T2MimicEnvCfg}, disable_env_checker=True, ) + +gym.register( + id="Isaac-Locomanipulation-G1-Abs-Mimic-v0", + entry_point="isaaclab_mimic.envs.pinocchio_envs:LocomanipulationG1MimicEnv", + kwargs={"env_cfg_entry_point": locomanipulation_g1_mimic_env_cfg.LocomanipulationG1MimicEnvCfg}, + disable_env_checker=True, +) diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env.py new file mode 100644 index 000000000000..ad612c61b0a6 --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env.py @@ -0,0 +1,129 @@ +# 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 + +import torch +from collections.abc import Sequence + +import isaaclab.utils.math as PoseUtils +from isaaclab.envs import ManagerBasedRLMimicEnv + + +class LocomanipulationG1MimicEnv(ManagerBasedRLMimicEnv): + + def get_robot_eef_pose(self, eef_name: str, env_ids: Sequence[int] | None = None) -> torch.Tensor: + """ + Get current robot end effector pose. Should be the same frame as used by the robot end-effector controller. + + Args: + eef_name: Name of the end effector. + env_ids: Environment indices to get the pose for. If None, all envs are considered. + + Returns: + A torch.Tensor eef pose matrix. Shape is (len(env_ids), 4, 4) + """ + if env_ids is None: + env_ids = slice(None) + + eef_pos_name = f"{eef_name}_eef_pos" + eef_quat_name = f"{eef_name}_eef_quat" + + target_wrist_position = self.obs_buf["policy"][eef_pos_name][env_ids] + target_rot_mat = PoseUtils.matrix_from_quat(self.obs_buf["policy"][eef_quat_name][env_ids]) + + return PoseUtils.make_pose(target_wrist_position, target_rot_mat) + + def target_eef_pose_to_action( + self, + target_eef_pose_dict: dict, + gripper_action_dict: dict, + action_noise_dict: dict | None = None, + env_id: int = 0, + ) -> torch.Tensor: + """ + Takes a target pose and gripper action for the end effector controller and returns an action + (usually a normalized delta pose action) to try and achieve that target pose. + Noise is added to the target pose action if specified. + + 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. + 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: + An action torch.Tensor that's compatible with env.step(). + """ + + # target position and rotation + target_left_eef_pos, left_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["left"]) + target_right_eef_pos, right_target_rot = PoseUtils.unmake_pose(target_eef_pose_dict["right"]) + + target_left_eef_rot_quat = PoseUtils.quat_from_matrix(left_target_rot) + target_right_eef_rot_quat = PoseUtils.quat_from_matrix(right_target_rot) + + # gripper actions + left_gripper_action = gripper_action_dict["left"] + right_gripper_action = gripper_action_dict["right"] + + if action_noise_dict is not None: + pos_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_pos) + pos_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_pos) + quat_noise_left = action_noise_dict["left"] * torch.randn_like(target_left_eef_rot_quat) + quat_noise_right = action_noise_dict["right"] * torch.randn_like(target_right_eef_rot_quat) + + target_left_eef_pos += pos_noise_left + target_right_eef_pos += pos_noise_right + target_left_eef_rot_quat += quat_noise_left + target_right_eef_rot_quat += quat_noise_right + + return torch.cat( + ( + target_left_eef_pos, + target_left_eef_rot_quat, + target_right_eef_pos, + target_right_eef_rot_quat, + left_gripper_action, + right_gripper_action, + ), + dim=0, + ) + + def action_to_target_eef_pose(self, action: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Converts action (compatible with env.step) to a target pose for the end effector controller. + Inverse of @target_eef_pose_to_action. Usually used to infer a sequence of target controller poses + from a demonstration trajectory using the recorded actions. + + Args: + action: Environment action. Shape is (num_envs, action_dim). + + Returns: + A dictionary of eef pose torch.Tensor that @action corresponds to. + """ + target_poses = {} + + target_left_wrist_position = action[:, 0:3] + target_left_rot_mat = PoseUtils.matrix_from_quat(action[:, 3:7]) + target_pose_left = PoseUtils.make_pose(target_left_wrist_position, target_left_rot_mat) + target_poses["left"] = target_pose_left + + target_right_wrist_position = action[:, 7:10] + target_right_rot_mat = PoseUtils.matrix_from_quat(action[:, 10:14]) + target_pose_right = PoseUtils.make_pose(target_right_wrist_position, target_right_rot_mat) + target_poses["right"] = target_pose_right + + return target_poses + + def actions_to_gripper_actions(self, actions: torch.Tensor) -> dict[str, torch.Tensor]: + """ + Extracts the gripper actuation part from a sequence of env actions (compatible with env.step). + + Args: + actions: environment actions. The shape is (num_envs, num steps in a demo, action_dim). + + Returns: + A dictionary of torch.Tensor gripper actions. Key to each dict is an eef_name. + """ + return {"left": actions[:, 14:21], "right": actions[:, 21:]} diff --git a/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env_cfg.py b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env_cfg.py new file mode 100644 index 000000000000..39bc02653b1f --- /dev/null +++ b/source/isaaclab_mimic/isaaclab_mimic/envs/pinocchio_envs/locomanipulation_g1_mimic_env_cfg.py @@ -0,0 +1,112 @@ +# 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.locomanipulation.pick_place.locomanipulation_g1_env_cfg import ( + LocomanipulationG1EnvCfg, +) + + +@configclass +class LocomanipulationG1MimicEnvCfg(LocomanipulationG1EnvCfg, MimicEnvCfg): + + def __post_init__(self): + # Call parent post-init + super().__post_init__() + + # Override datagen config values for demonstration generation + self.datagen_config.name = "demo_src_g1_locomanip_demo_task_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 + + # Subtask configs for right arm + 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", + # Randomization range for starting index of the first subtask + first_subtask_start_offset_range=(0, 0), + # Time offsets for data generation when splitting a trajectory + subtask_term_offset_range=(0, 0), + # Selection strategy for the source subtask segment during data generation + 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.002, + # 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.002, + # 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 for left arm + 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.002, + # 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/isaaclab_tasks/manager_based/loco_manipulation/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/__init__.py similarity index 68% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/__init__.py index cb907a3f0c8b..739fdf113e69 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/__init__.py @@ -3,6 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Locomotion environments for legged robots.""" + +"""This sub-module contains the functions that are specific to the locomanipulation environments.""" from .tracking import * # noqa diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py new file mode 100644 index 000000000000..a3b30988b7fc --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/__init__.py @@ -0,0 +1,31 @@ +# 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 + + +"""This sub-module contains the functions that are specific to the locomanipulation environments.""" + +import gymnasium as gym +import os + +from . import agents, fixed_base_upper_body_ik_g1_env_cfg, locomanipulation_g1_env_cfg + +gym.register( + id="Isaac-PickPlace-Locomanipulation-G1-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": locomanipulation_g1_env_cfg.LocomanipulationG1EnvCfg, + "robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"), + }, + disable_env_checker=True, +) + +gym.register( + id="Isaac-PickPlace-FixedBaseUpperBodyIK-G1-Abs-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": fixed_base_upper_body_ik_g1_env_cfg.FixedBaseUpperBodyIKG1EnvCfg, + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json new file mode 100644 index 000000000000..c1dce5f832c8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/agents/robomimic/bc_rnn_low_dim.json @@ -0,0 +1,117 @@ +{ + "algo_name": "bc", + "experiment": { + "name": "bc_rnn_low_dim_g1", + "validate": false, + "logging": { + "terminal_output_to_txt": true, + "log_tb": true + }, + "save": { + "enabled": true, + "every_n_seconds": null, + "every_n_epochs": 100, + "epochs": [], + "on_best_validation": false, + "on_best_rollout_return": false, + "on_best_rollout_success_rate": true + }, + "epoch_every_n_steps": 100, + "env": null, + "additional_envs": null, + "render": false, + "render_video": false, + "rollout": { + "enabled": false + } + }, + "train": { + "data": null, + "num_data_workers": 4, + "hdf5_cache_mode": "all", + "hdf5_use_swmr": true, + "hdf5_normalize_obs": false, + "hdf5_filter_key": null, + "hdf5_validation_filter_key": null, + "seq_length": 10, + "dataset_keys": [ + "actions" + ], + "goal_mode": null, + "cuda": true, + "batch_size": 100, + "num_epochs": 2000, + "seed": 101 + }, + "algo": { + "optim_params": { + "policy": { + "optimizer_type": "adam", + "learning_rate": { + "initial": 0.001, + "decay_factor": 0.1, + "epoch_schedule": [], + "scheduler_type": "multistep" + }, + "regularization": { + "L2": 0.0 + } + } + }, + "loss": { + "l2_weight": 1.0, + "l1_weight": 0.0, + "cos_weight": 0.0 + }, + "actor_layer_dims": [], + "gmm": { + "enabled": false, + "num_modes": 5, + "min_std": 0.0001, + "std_activation": "softplus", + "low_noise_eval": true + }, + "rnn": { + "enabled": true, + "horizon": 10, + "hidden_dim": 400, + "rnn_type": "LSTM", + "num_layers": 2, + "open_loop": false, + "kwargs": { + "bidirectional": false + } + }, + "transformer": { + "enabled": false, + "context_length": 10, + "embed_dim": 512, + "num_layers": 6, + "num_heads": 8, + "emb_dropout": 0.1, + "attn_dropout": 0.1, + "block_output_dropout": 0.1, + "sinusoidal_embedding": false, + "activation": "gelu", + "supervise_all_steps": false, + "nn_parameter_for_timesteps": true + } + }, + "observation": { + "modalities": { + "obs": { + "low_dim": [ + "left_eef_pos", + "left_eef_quat", + "right_eef_pos", + "right_eef_quat", + "hand_joint_state", + "object" + ], + "rgb": [], + "depth": [], + "scan": [] + } + } + } +} diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/action_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/action_cfg.py new file mode 100644 index 000000000000..4d8db0b0c150 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/action_cfg.py @@ -0,0 +1,34 @@ +# 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 + +from dataclasses import MISSING + +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg +from isaaclab.utils import configclass + +from ..mdp.actions import AgileBasedLowerBodyAction + + +@configclass +class AgileBasedLowerBodyActionCfg(ActionTermCfg): + """Configuration for the lower body action term that is based on Agile lower body RL policy.""" + + class_type: type[ActionTerm] = AgileBasedLowerBodyAction + """The class type for the lower body action term.""" + + joint_names: list[str] = MISSING + """The names of the joints to control.""" + + obs_group_name: str = MISSING + """The name of the observation group to use.""" + + policy_path: str = MISSING + """The path to the policy model.""" + + policy_output_offset: float = 0.0 + """Offsets the output of the policy.""" + + policy_output_scale: float = 1.0 + """Scales the output of the policy.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/agile_locomotion_observation_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/agile_locomotion_observation_cfg.py new file mode 100644 index 000000000000..e4e22987442a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/agile_locomotion_observation_cfg.py @@ -0,0 +1,84 @@ +# 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 + +from isaaclab.envs import mdp +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import configclass + + +@configclass +class AgileTeacherPolicyObservationsCfg(ObsGroup): + """Observation specifications for the Agile lower body policy. + + Note: This configuration defines only part of the observation input to the Agile lower body policy. + The lower body command portion is appended to the observation tensor in the action term, as that + is where the environment has access to those commands. + """ + + base_lin_vel = ObsTerm( + func=mdp.base_lin_vel, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + base_ang_vel = ObsTerm( + func=mdp.base_ang_vel, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + + projected_gravity = ObsTerm( + func=mdp.projected_gravity, + scale=1.0, + ) + + joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_shoulder_.*_joint", + ".*_elbow_joint", + ".*_wrist_.*_joint", + ".*_hip_.*_joint", + ".*_knee_joint", + ".*_ankle_.*_joint", + "waist_.*_joint", + ], + ), + }, + ) + + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + scale=0.1, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_shoulder_.*_joint", + ".*_elbow_joint", + ".*_wrist_.*_joint", + ".*_hip_.*_joint", + ".*_knee_joint", + ".*_ankle_.*_joint", + "waist_.*_joint", + ], + ), + }, + ) + + actions = ObsTerm( + func=mdp.last_action, + scale=1.0, + params={ + "action_name": "lower_body_joint_pos", + }, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/pink_controller_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/pink_controller_cfg.py new file mode 100644 index 000000000000..1c80674e3831 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/configs/pink_controller_cfg.py @@ -0,0 +1,126 @@ +# 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 + +"""Configuration for pink controller. + +This module provides configurations for humanoid robot pink IK controllers, +including both fixed base and mobile configurations for upper body manipulation. +""" + +from isaaclab.controllers.pink_ik.local_frame_task import LocalFrameTask +from isaaclab.controllers.pink_ik.null_space_posture_task import NullSpacePostureTask +from isaaclab.controllers.pink_ik.pink_ik_cfg import PinkIKControllerCfg +from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg + +## +# Pink IK Controller Configuration for G1 +## + +G1_UPPER_BODY_IK_CONTROLLER_CFG = PinkIKControllerCfg( + articulation_name="robot", + base_link_name="pelvis", + num_hand_joints=14, + show_ik_warnings=True, + fail_on_joint_limit_violation=False, + variable_input_tasks=[ + LocalFrameTask( + "g1_29dof_with_hand_rev_1_0_left_wrist_yaw_link", + base_link_frame_name="g1_29dof_with_hand_rev_1_0_pelvis", + position_cost=8.0, # [cost] / [m] + orientation_cost=2.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + LocalFrameTask( + "g1_29dof_with_hand_rev_1_0_right_wrist_yaw_link", + base_link_frame_name="g1_29dof_with_hand_rev_1_0_pelvis", + position_cost=8.0, # [cost] / [m] + orientation_cost=2.0, # [cost] / [rad] + lm_damping=10, # dampening for solver for step jumps + gain=0.5, + ), + NullSpacePostureTask( + cost=0.5, + lm_damping=1, + controlled_frames=[ + "g1_29dof_with_hand_rev_1_0_left_wrist_yaw_link", + "g1_29dof_with_hand_rev_1_0_right_wrist_yaw_link", + ], + controlled_joints=[ + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "waist_yaw_joint", + "waist_pitch_joint", + "waist_roll_joint", + ], + gain=0.3, + ), + ], + fixed_input_tasks=[], +) +"""Base configuration for the G1 pink IK controller. + +This configuration sets up the pink IK controller for the G1 humanoid robot with +left and right wrist control tasks. The controller is designed for upper body +manipulation tasks. +""" + + +## +# Pink IK Action Configuration for G1 +## + +G1_UPPER_BODY_IK_ACTION_CFG = PinkInverseKinematicsActionCfg( + pink_controlled_joint_names=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_pitch_joint", + ".*_wrist_roll_joint", + ".*_wrist_yaw_joint", + "waist_.*_joint", + ], + hand_joint_names=[ + "left_hand_index_0_joint", # Index finger proximal + "left_hand_middle_0_joint", # Middle finger proximal + "left_hand_thumb_0_joint", # Thumb base (yaw axis) + "right_hand_index_0_joint", # Index finger proximal + "right_hand_middle_0_joint", # Middle finger proximal + "right_hand_thumb_0_joint", # Thumb base (yaw axis) + "left_hand_index_1_joint", # Index finger distal + "left_hand_middle_1_joint", # Middle finger distal + "left_hand_thumb_1_joint", # Thumb middle (pitch axis) + "right_hand_index_1_joint", # Index finger distal + "right_hand_middle_1_joint", # Middle finger distal + "right_hand_thumb_1_joint", # Thumb middle (pitch axis) + "left_hand_thumb_2_joint", # Thumb tip + "right_hand_thumb_2_joint", # Thumb tip + ], + target_eef_link_names={ + "left_wrist": "left_wrist_yaw_link", + "right_wrist": "right_wrist_yaw_link", + }, + # the robot in the sim scene we are controlling + asset_name="robot", + # Configuration for the IK controller + # The frames names are the ones present in the URDF file + # The urdf has to be generated from the USD that is being used in the scene + controller=G1_UPPER_BODY_IK_CONTROLLER_CFG, +) +"""Base configuration for the G1 pink IK action. + +This configuration sets up the pink IK action for the G1 humanoid robot, +defining which joints are controlled by the IK solver and which are fixed. +The configuration includes: +- Upper body joints controlled by IK (shoulders, elbows, wrists) +- Fixed joints (pelvis, legs, hands) +- Hand joint names for additional control +- Reference to the pink IK controller configuration +""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py new file mode 100644 index 000000000000..1ba7b9ab5a67 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/fixed_base_upper_body_ik_g1_env_cfg.py @@ -0,0 +1,215 @@ +# 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 + + +from isaaclab_assets.robots.unitree import G1_29DOF_CFG + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeterCfg, +) +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +from isaaclab_tasks.manager_based.locomanipulation.pick_place import mdp as locomanip_mdp +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp + +from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.pink_controller_cfg import ( # isort: skip + G1_UPPER_BODY_IK_ACTION_CFG, +) + + +## +# Scene definition +## +@configclass +class FixedBaseUpperBodyIKG1SceneCfg(InteractiveSceneCfg): + """Scene configuration for fixed base upper body IK environment with G1 robot. + + This configuration sets up the G1 humanoid robot with fixed pelvis and legs, + allowing only arm manipulation while the base remains stationary. The robot is + controlled using upper body IK. + """ + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, -0.3], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.45, 0.6996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Unitree G1 Humanoid robot - fixed base configuration + robot: ArticulationCfg = G1_29DOF_CFG + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + def __post_init__(self): + """Post initialization.""" + # Set the robot to fixed base + self.robot.spawn.articulation_props.fix_root_link = True + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = G1_UPPER_BODY_IK_ACTION_CFG + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP. + This class is required by the environment configuration but not used in this implementation + """ + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=manip_mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=manip_mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "left_wrist_yaw_link"}) + left_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "left_wrist_yaw_link"}) + right_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "right_wrist_yaw_link"}) + right_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "right_wrist_yaw_link"}) + + hand_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": [".*_hand.*"]}) + head_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": []}) + + object = ObsTerm( + func=manip_mdp.object_obs, + params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=locomanip_mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=base_mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=manip_mdp.task_done_pick_place, params={"task_link_name": "right_wrist_yaw_link"}) + + +## +# MDP settings +## + + +@configclass +class FixedBaseUpperBodyIKG1EnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the G1 fixed base upper body IK environment. + + This environment is designed for manipulation tasks where the G1 humanoid robot + has a fixed pelvis and legs, allowing only arm and hand movements for manipulation. The robot is + controlled using upper body IK. + """ + + # Scene settings + scene: FixedBaseUpperBodyIKG1SceneCfg = FixedBaseUpperBodyIKG1SceneCfg( + num_envs=1, env_spacing=2.5, replicate_physics=True + ) + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + + # 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.2), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 200 # 200Hz + self.sim.render_interval = 2 + + # Set the URDF and mesh paths for the IK controller + urdf_omniverse_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_kinematics_asset/g1_29dof_with_hand_only_kinematics.urdf" + + # Retrieve local paths for the URDF and mesh files. Will be cached for call after the first time. + self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + G1TriHandUpperBodyRetargeterCfg( + enable_visualization=True, + # OpenXR hand tracking has 26 joints per hand + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py new file mode 100644 index 000000000000..e807f100c419 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py @@ -0,0 +1,229 @@ +# 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 + + +from isaaclab_assets.robots.unitree import G1_29DOF_CFG + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.g1_lower_body_standing import G1LowerBodyStandingRetargeterCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( + G1TriHandUpperBodyRetargeterCfg, +) +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR, retrieve_file_path + +from isaaclab_tasks.manager_based.locomanipulation.pick_place import mdp as locomanip_mdp +from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.action_cfg import AgileBasedLowerBodyActionCfg +from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.agile_locomotion_observation_cfg import ( + AgileTeacherPolicyObservationsCfg, +) +from isaaclab_tasks.manager_based.manipulation.pick_place import mdp as manip_mdp + +from isaaclab_tasks.manager_based.locomanipulation.pick_place.configs.pink_controller_cfg import ( # isort: skip + G1_UPPER_BODY_IK_ACTION_CFG, +) + + +## +# Scene definition +## +@configclass +class LocomanipulationG1SceneCfg(InteractiveSceneCfg): + """Scene configuration for locomanipulation environment with G1 robot. + + This configuration sets up the G1 humanoid robot for locomanipulation tasks, + allowing both locomotion and manipulation capabilities. The robot can move its + base and use its arms for manipulation tasks. + """ + + # Table + packing_table = AssetBaseCfg( + prim_path="/World/envs/env_.*/PackingTable", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, -0.3], rot=[1.0, 0.0, 0.0, 0.0]), + spawn=UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/PackingTable/packing_table.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + ), + ) + + object = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.45, 0.6996], rot=[1, 0, 0, 0]), + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Mimic/pick_place_task/pick_place_assets/steering_wheel.usd", + scale=(0.75, 0.75, 0.75), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + ), + ) + + # Humanoid robot w/ arms higher + robot: ArticulationCfg = G1_29DOF_CFG + + # Ground plane + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + spawn=GroundPlaneCfg(), + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=3000.0), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + upper_body_ik = G1_UPPER_BODY_IK_ACTION_CFG + + lower_body_joint_pos = AgileBasedLowerBodyActionCfg( + asset_name="robot", + joint_names=[ + ".*_hip_.*_joint", + ".*_knee_joint", + ".*_ankle_.*_joint", + ], + policy_output_scale=0.25, + obs_group_name="lower_body_policy", # need to be the same name as the on in ObservationCfg + policy_path="omniverse://isaac-dev.ov.nvidia.com/Projects/agile/policy_checkpoints/agile_locomotion.pt", + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP. + This class is required by the environment configuration but not used in this implementation + """ + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group with state values.""" + + actions = ObsTerm(func=manip_mdp.last_action) + robot_joint_pos = ObsTerm( + func=base_mdp.joint_pos, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + robot_root_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot")}) + robot_root_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot")}) + object_pos = ObsTerm(func=base_mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("object")}) + object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) + robot_links_state = ObsTerm(func=manip_mdp.get_all_robot_link_state) + + left_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "left_wrist_yaw_link"}) + left_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "left_wrist_yaw_link"}) + right_eef_pos = ObsTerm(func=manip_mdp.get_eef_pos, params={"link_name": "right_wrist_yaw_link"}) + right_eef_quat = ObsTerm(func=manip_mdp.get_eef_quat, params={"link_name": "right_wrist_yaw_link"}) + + hand_joint_state = ObsTerm(func=manip_mdp.get_robot_joint_state, params={"joint_names": [".*_hand.*"]}) + + object = ObsTerm( + func=manip_mdp.object_obs, + params={"left_eef_link_name": "left_wrist_yaw_link", "right_eef_link_name": "right_wrist_yaw_link"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + lower_body_policy: AgileTeacherPolicyObservationsCfg = AgileTeacherPolicyObservationsCfg() + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=locomanip_mdp.time_out, time_out=True) + + object_dropping = DoneTerm( + func=base_mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} + ) + + success = DoneTerm(func=manip_mdp.task_done_pick_place, params={"task_link_name": "right_wrist_yaw_link"}) + + +## +# MDP settings +## + + +@configclass +class LocomanipulationG1EnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the G1 locomanipulation environment. + + This environment is designed for locomanipulation tasks where the G1 humanoid robot + can perform both locomotion and manipulation simultaneously. The robot can move its + base and use its arms for manipulation tasks, enabling complex mobile manipulation + behaviors. + """ + + # Scene settings + scene: LocomanipulationG1SceneCfg = LocomanipulationG1SceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True) + # MDP settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands = None + terminations: TerminationsCfg = TerminationsCfg() + + # Unused managers + rewards = None + curriculum = None + + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.0, 0.0, 0.3), + anchor_rot=(1.0, 0.0, 0.0, 0.0), + ) + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 200 # 200Hz + self.sim.render_interval = 2 + + # Set the URDF and mesh paths for the IK controller + urdf_omniverse_path = f"{ISAACLAB_NUCLEUS_DIR}/Controllers/LocomanipulationAssets/unitree_g1_kinematics_asset/g1_29dof_with_hand_only_kinematics.urdf" + + # Retrieve local paths for the URDF and mesh files. Will be cached for call after the first time. + self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + + self.teleop_devices = DevicesCfg( + devices={ + "handtracking": OpenXRDeviceCfg( + retargeters=[ + G1TriHandUpperBodyRetargeterCfg( + enable_visualization=True, + # OpenXR hand tracking has 26 joints per hand + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + G1LowerBodyStandingRetargeterCfg( + sim_device=self.sim.device, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/__init__.py new file mode 100644 index 000000000000..18ec38070d59 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/__init__.py @@ -0,0 +1,12 @@ +# 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 + + +"""This sub-module contains the functions that are specific to the locomanipulation environments.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .actions import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/actions.py new file mode 100644 index 000000000000..ad0384a5b821 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/actions.py @@ -0,0 +1,125 @@ +# 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 + +from __future__ import annotations + +import torch +from typing import TYPE_CHECKING + +from isaaclab.assets.articulation import Articulation +from isaaclab.managers.action_manager import ActionTerm +from isaaclab.utils.assets import retrieve_file_path +from isaaclab.utils.io.torchscript import load_torchscript_model + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .configs.action_cfg import AgileBasedLowerBodyActionCfg + + +class AgileBasedLowerBodyAction(ActionTerm): + """Action term that is based on Agile lower body RL policy.""" + + cfg: AgileBasedLowerBodyActionCfg + """The configuration of the action term.""" + + _asset: Articulation + """The articulation asset to which the action term is applied.""" + + def __init__(self, cfg: AgileBasedLowerBodyActionCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) + + # Save the observation config from cfg + self._observation_cfg = env.cfg.observations + self._obs_group_name = cfg.obs_group_name + + # Load policy here if needed + _temp_policy_path = retrieve_file_path(cfg.policy_path) + self._policy = load_torchscript_model(_temp_policy_path, device=env.device) + self._env = env + + # Find joint ids for the lower body joints + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + + # Get the scale and offset from the configuration + self._policy_output_scale = torch.tensor(cfg.policy_output_scale, device=env.device) + self._policy_output_offset = self._asset.data.default_joint_pos[:, self._joint_ids].clone() + + # Create tensors to store raw and processed actions + self._raw_actions = torch.zeros(self.num_envs, len(self._joint_ids), device=self.device) + self._processed_actions = torch.zeros(self.num_envs, len(self._joint_ids), device=self.device) + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + """Lower Body Action: [vx, vy, wz, hip_height]""" + return 4 + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + def _compose_policy_input(self, base_command: torch.Tensor, obs_tensor: torch.Tensor) -> torch.Tensor: + """Compose the policy input by concatenating repeated commands with observations. + + Args: + base_command: The base command tensor [vx, vy, wz, hip_height]. + obs_tensor: The observation tensor from the environment. + + Returns: + The composed policy input tensor with repeated commands concatenated to observations. + """ + # Get history length from observation configuration + history_length = getattr(self._observation_cfg, self._obs_group_name).history_length + # Default to 1 if history_length is None (no history, just current observation) + if history_length is None: + history_length = 1 + + # Repeat commands based on history length and concatenate with observations + repeated_commands = base_command.unsqueeze(1).repeat(1, history_length, 1).reshape(base_command.shape[0], -1) + policy_input = torch.cat([repeated_commands, obs_tensor], dim=-1) + + return policy_input + + def process_actions(self, actions: torch.Tensor): + """Process the input actions using the locomotion policy. + + Args: + actions: The lower body commands. + """ + + # Extract base command from the action tensor + # Assuming the base command [vx, vy, wz, hip_height] + base_command = actions + + obs_tensor = self._env.obs_buf["lower_body_policy"] + + # Compose policy input using helper function + policy_input = self._compose_policy_input(base_command, obs_tensor) + + joint_actions = self._policy.forward(policy_input) + + self._raw_actions[:] = joint_actions + + # Apply scaling and offset to the raw actions from the policy + self._processed_actions = joint_actions * self._policy_output_scale + self._policy_output_offset + + # Clip actions if configured + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + + def apply_actions(self): + """Apply the actions to the environment.""" + # Store the raw actions + self._asset.set_joint_position_target(self._processed_actions, joint_ids=self._joint_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/observations.py new file mode 100644 index 000000000000..ab027ce0bf13 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/observations.py @@ -0,0 +1,32 @@ +# 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 torch + +from isaaclab.envs import ManagerBasedRLEnv +from isaaclab.managers import SceneEntityCfg + + +def upper_body_last_action( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), +) -> torch.Tensor: + """Extract the last action of the upper body.""" + asset = env.scene[asset_cfg.name] + joint_pos_target = asset.data.joint_pos_target + + # Use joint_names from asset_cfg to find indices + joint_names = asset_cfg.joint_names if hasattr(asset_cfg, "joint_names") else None + if joint_names is None: + raise ValueError("asset_cfg must have 'joint_names' attribute for upper_body_last_action.") + + # Find joint indices matching the provided joint_names (supports regex) + joint_indices, _ = asset.find_joints(joint_names) + joint_indices = torch.tensor(joint_indices, dtype=torch.long) + + # Get upper body joint positions for all environments + upper_body_joint_pos_target = joint_pos_target[:, joint_indices] + + return upper_body_joint_pos_target diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/__init__.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/agents/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/__init__.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/loco_manip_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/loco_manip_env_cfg.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/manager_based/loco_manipulation/tracking/config/digit/loco_manip_env_cfg.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/loco_manip_env_cfg.py 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 ed1f0f06130c..488db92d0e7a 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 @@ -184,13 +184,16 @@ class PolicyCfg(ObsGroup): params={"asset_cfg": SceneEntityCfg("robot")}, ) - left_eef_pos = ObsTerm(func=mdp.get_left_eef_pos) - left_eef_quat = ObsTerm(func=mdp.get_left_eef_quat) - right_eef_pos = ObsTerm(func=mdp.get_right_eef_pos) - right_eef_quat = ObsTerm(func=mdp.get_right_eef_quat) - - hand_joint_state = ObsTerm(func=mdp.get_hand_state) - head_joint_state = ObsTerm(func=mdp.get_head_state) + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) robot_pov_cam = ObsTerm( func=mdp.image, @@ -216,7 +219,7 @@ class TerminationsCfg: params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("blue_exhaust_pipe")}, ) - success = DoneTerm(func=mdp.task_done_exhaust_pipe) + success = DoneTerm(func=mdp.task_done_exhaust_pipe, params={"relevant_link_name": "right_hand_roll_link"}) @configclass 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 0a3cb26b4d3e..01feeab1cc2e 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 @@ -42,49 +42,6 @@ def __post_init__(self): "right_wrist_roll_joint", "right_wrist_pitch_joint", ], - # Joints to be locked in URDF - ik_urdf_fixed_joint_names=[ - "left_hip_roll_joint", - "right_hip_roll_joint", - "left_hip_yaw_joint", - "right_hip_yaw_joint", - "left_hip_pitch_joint", - "right_hip_pitch_joint", - "left_knee_pitch_joint", - "right_knee_pitch_joint", - "left_ankle_pitch_joint", - "right_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_ankle_roll_joint", - "L_index_proximal_joint", - "L_middle_proximal_joint", - "L_pinky_proximal_joint", - "L_ring_proximal_joint", - "L_thumb_proximal_yaw_joint", - "R_index_proximal_joint", - "R_middle_proximal_joint", - "R_pinky_proximal_joint", - "R_ring_proximal_joint", - "R_thumb_proximal_yaw_joint", - "L_index_intermediate_joint", - "L_middle_intermediate_joint", - "L_pinky_intermediate_joint", - "L_ring_intermediate_joint", - "L_thumb_proximal_pitch_joint", - "R_index_intermediate_joint", - "R_middle_intermediate_joint", - "R_pinky_intermediate_joint", - "R_ring_intermediate_joint", - "R_thumb_proximal_pitch_joint", - "L_thumb_distal_joint", - "R_thumb_distal_joint", - "head_roll_joint", - "head_pitch_joint", - "head_yaw_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], hand_joint_names=[ "L_index_proximal_joint", "L_middle_proximal_joint", @@ -164,14 +121,7 @@ def __post_init__(self): ], ), ], - 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=[], xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), ), ) @@ -179,9 +129,6 @@ def __post_init__(self): 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.gr1_action.ik_urdf_fixed_joint_names - ) # Set the URDF and mesh paths for the IK controller self.actions.gr1_action.controller.urdf_path = temp_urdf_output_path diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py index efc8d9f7b1e1..b4dfcb6829f1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py @@ -14,6 +14,8 @@ def object_obs( env: ManagerBasedRLEnv, + left_eef_link_name: str, + right_eef_link_name: str, ) -> torch.Tensor: """ Object observations (in world frame): @@ -24,8 +26,8 @@ def object_obs( """ body_pos_w = env.scene["robot"].data.body_pos_w - left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") - right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") + left_eef_idx = env.scene["robot"].data.body_names.index(left_eef_link_name) + right_eef_idx = env.scene["robot"].data.body_names.index(right_eef_link_name) left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins right_eef_pos = body_pos_w[:, right_eef_idx] - env.scene.env_origins @@ -46,63 +48,32 @@ def object_obs( ) -def get_left_eef_pos( - env: ManagerBasedRLEnv, -) -> torch.Tensor: +def get_eef_pos(env: ManagerBasedRLEnv, link_name: str) -> torch.Tensor: body_pos_w = env.scene["robot"].data.body_pos_w - left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") + left_eef_idx = env.scene["robot"].data.body_names.index(link_name) left_eef_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins return left_eef_pos -def get_left_eef_quat( - env: ManagerBasedRLEnv, -) -> torch.Tensor: +def get_eef_quat(env: ManagerBasedRLEnv, link_name: str) -> torch.Tensor: body_quat_w = env.scene["robot"].data.body_quat_w - left_eef_idx = env.scene["robot"].data.body_names.index("left_hand_roll_link") + left_eef_idx = env.scene["robot"].data.body_names.index(link_name) left_eef_quat = body_quat_w[:, left_eef_idx] return left_eef_quat -def get_right_eef_pos( - env: ManagerBasedRLEnv, -) -> torch.Tensor: - body_pos_w = env.scene["robot"].data.body_pos_w - right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") - right_eef_pos = body_pos_w[:, right_eef_idx] - env.scene.env_origins - - return right_eef_pos - - -def get_right_eef_quat( - env: ManagerBasedRLEnv, -) -> torch.Tensor: - body_quat_w = env.scene["robot"].data.body_quat_w - right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") - right_eef_quat = body_quat_w[:, right_eef_idx] - - return right_eef_quat - - -def get_hand_state( - env: ManagerBasedRLEnv, -) -> torch.Tensor: - hand_joint_states = env.scene["robot"].data.joint_pos[:, -22:] # Hand joints are last 22 entries of joint state - - return hand_joint_states - - -def get_head_state( +def get_robot_joint_state( env: ManagerBasedRLEnv, + joint_names: list[str], ) -> torch.Tensor: - robot_joint_names = env.scene["robot"].data.joint_names - head_joint_names = ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"] - indexes = torch.tensor([robot_joint_names.index(name) for name in head_joint_names], dtype=torch.long) - head_joint_states = env.scene["robot"].data.joint_pos[:, indexes] + # hand_joint_names is a list of regex, use find_joints + indexes, _ = env.scene["robot"].find_joints(joint_names) + indexes = torch.tensor(indexes, dtype=torch.long) + robot_joint_states = env.scene["robot"].data.joint_pos[:, indexes] - return head_joint_states + return robot_joint_states def get_all_robot_link_state( diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py index ee6dbd685268..477552bbdbae 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/terminations.py @@ -23,6 +23,7 @@ def task_done_pick_place( env: ManagerBasedRLEnv, + task_link_name: str = "", object_cfg: SceneEntityCfg = SceneEntityCfg("object"), right_wrist_max_x: float = 0.26, min_x: float = 0.40, @@ -54,6 +55,9 @@ def task_done_pick_place( Returns: Boolean tensor indicating which environments have completed the task. """ + if task_link_name == "": + raise ValueError("task_link_name must be provided to task_done_pick_place") + # Get object entity from the scene object: RigidObject = env.scene[object_cfg.name] @@ -65,7 +69,7 @@ def task_done_pick_place( # Get right wrist position relative to environment origin robot_body_pos_w = env.scene["robot"].data.body_pos_w - right_eef_idx = env.scene["robot"].data.body_names.index("right_hand_roll_link") + right_eef_idx = env.scene["robot"].data.body_names.index(task_link_name) right_wrist_x = robot_body_pos_w[:, right_eef_idx, 0] - env.scene.env_origins[:, 0] # Check all success conditions and combine with logical AND 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 ffa7929c9539..8467b67f39b6 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 @@ -205,13 +205,16 @@ class PolicyCfg(ObsGroup): params={"asset_cfg": SceneEntityCfg("robot")}, ) - left_eef_pos = ObsTerm(func=mdp.get_left_eef_pos) - left_eef_quat = ObsTerm(func=mdp.get_left_eef_quat) - right_eef_pos = ObsTerm(func=mdp.get_right_eef_pos) - right_eef_quat = ObsTerm(func=mdp.get_right_eef_quat) - - hand_joint_state = ObsTerm(func=mdp.get_hand_state) - head_joint_state = ObsTerm(func=mdp.get_head_state) + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) + + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) robot_pov_cam = ObsTerm( func=mdp.image, @@ -243,7 +246,7 @@ class TerminationsCfg: func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("factory_nut")} ) - success = DoneTerm(func=mdp.task_done_nut_pour) + success = DoneTerm(func=mdp.task_done_nut_pour, params={"relevant_link_name": "right_hand_roll_link"}) @configclass 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 b7e1ff3ddecf..6dcdd9a1e8fc 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 @@ -40,49 +40,6 @@ def __post_init__(self): "right_wrist_roll_joint", "right_wrist_pitch_joint", ], - # Joints to be locked in URDF - ik_urdf_fixed_joint_names=[ - "left_hip_roll_joint", - "right_hip_roll_joint", - "left_hip_yaw_joint", - "right_hip_yaw_joint", - "left_hip_pitch_joint", - "right_hip_pitch_joint", - "left_knee_pitch_joint", - "right_knee_pitch_joint", - "left_ankle_pitch_joint", - "right_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_ankle_roll_joint", - "L_index_proximal_joint", - "L_middle_proximal_joint", - "L_pinky_proximal_joint", - "L_ring_proximal_joint", - "L_thumb_proximal_yaw_joint", - "R_index_proximal_joint", - "R_middle_proximal_joint", - "R_pinky_proximal_joint", - "R_ring_proximal_joint", - "R_thumb_proximal_yaw_joint", - "L_index_intermediate_joint", - "L_middle_intermediate_joint", - "L_pinky_intermediate_joint", - "L_ring_intermediate_joint", - "L_thumb_proximal_pitch_joint", - "R_index_intermediate_joint", - "R_middle_intermediate_joint", - "R_pinky_intermediate_joint", - "R_ring_intermediate_joint", - "R_thumb_proximal_pitch_joint", - "L_thumb_distal_joint", - "R_thumb_distal_joint", - "head_roll_joint", - "head_pitch_joint", - "head_yaw_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], hand_joint_names=[ "L_index_proximal_joint", "L_middle_proximal_joint", @@ -162,14 +119,7 @@ def __post_init__(self): ], ), ], - 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=[], xr_enabled=bool(carb.settings.get_settings().get("/app/xr/enabled")), ), ) @@ -177,9 +127,6 @@ def __post_init__(self): 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.gr1_action.ik_urdf_fixed_joint_names - ) # Set the URDF and mesh paths for the IK controller self.actions.gr1_action.controller.urdf_path = temp_urdf_output_path 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 9343db5ffc58..b2c1c2d83a3e 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 @@ -116,7 +116,7 @@ class ObjectTableSceneCfg(InteractiveSceneCfg): class ActionsCfg: """Action specifications for the MDP.""" - pink_ik_cfg = PinkInverseKinematicsActionCfg( + upper_body_ik = PinkInverseKinematicsActionCfg( pink_controlled_joint_names=[ "left_shoulder_pitch_joint", "left_shoulder_roll_joint", @@ -133,49 +133,6 @@ class ActionsCfg: "right_wrist_roll_joint", "right_wrist_pitch_joint", ], - # Joints to be locked in URDF - ik_urdf_fixed_joint_names=[ - "left_hip_roll_joint", - "right_hip_roll_joint", - "left_hip_yaw_joint", - "right_hip_yaw_joint", - "left_hip_pitch_joint", - "right_hip_pitch_joint", - "left_knee_pitch_joint", - "right_knee_pitch_joint", - "left_ankle_pitch_joint", - "right_ankle_pitch_joint", - "left_ankle_roll_joint", - "right_ankle_roll_joint", - "L_index_proximal_joint", - "L_middle_proximal_joint", - "L_pinky_proximal_joint", - "L_ring_proximal_joint", - "L_thumb_proximal_yaw_joint", - "R_index_proximal_joint", - "R_middle_proximal_joint", - "R_pinky_proximal_joint", - "R_ring_proximal_joint", - "R_thumb_proximal_yaw_joint", - "L_index_intermediate_joint", - "L_middle_intermediate_joint", - "L_pinky_intermediate_joint", - "L_ring_intermediate_joint", - "L_thumb_proximal_pitch_joint", - "R_index_intermediate_joint", - "R_middle_intermediate_joint", - "R_pinky_intermediate_joint", - "R_ring_intermediate_joint", - "R_thumb_proximal_pitch_joint", - "L_thumb_distal_joint", - "R_thumb_distal_joint", - "head_roll_joint", - "head_pitch_joint", - "head_yaw_joint", - "waist_yaw_joint", - "waist_pitch_joint", - "waist_roll_joint", - ], hand_joint_names=[ "L_index_proximal_joint", "L_middle_proximal_joint", @@ -280,15 +237,21 @@ class PolicyCfg(ObsGroup): object_rot = ObsTerm(func=base_mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("object")}) robot_links_state = ObsTerm(func=mdp.get_all_robot_link_state) - left_eef_pos = ObsTerm(func=mdp.get_left_eef_pos) - left_eef_quat = ObsTerm(func=mdp.get_left_eef_quat) - right_eef_pos = ObsTerm(func=mdp.get_right_eef_pos) - right_eef_quat = ObsTerm(func=mdp.get_right_eef_quat) + left_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "left_hand_roll_link"}) + left_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "left_hand_roll_link"}) + right_eef_pos = ObsTerm(func=mdp.get_eef_pos, params={"link_name": "right_hand_roll_link"}) + right_eef_quat = ObsTerm(func=mdp.get_eef_quat, params={"link_name": "right_hand_roll_link"}) - hand_joint_state = ObsTerm(func=mdp.get_hand_state) - head_joint_state = ObsTerm(func=mdp.get_head_state) + hand_joint_state = ObsTerm(func=mdp.get_robot_joint_state, params={"joint_names": ["R_.*", "L_.*"]}) + head_joint_state = ObsTerm( + func=mdp.get_robot_joint_state, + params={"joint_names": ["head_pitch_joint", "head_roll_joint", "head_yaw_joint"]}, + ) - object = ObsTerm(func=mdp.object_obs) + object = ObsTerm( + func=mdp.object_obs, + params={"left_eef_link_name": "left_hand_roll_link", "right_eef_link_name": "right_hand_roll_link"}, + ) def __post_init__(self): self.enable_corruption = False @@ -308,7 +271,7 @@ class TerminationsCfg: func=mdp.root_height_below_minimum, params={"minimum_height": 0.5, "asset_cfg": SceneEntityCfg("object")} ) - success = DoneTerm(func=mdp.task_done_pick_place) + success = DoneTerm(func=mdp.task_done_pick_place, params={"task_link_name": "right_hand_roll_link"}) @configclass @@ -416,13 +379,10 @@ def __post_init__(self): 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.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.actions.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path self.teleop_devices = DevicesCfg( devices={ @@ -433,7 +393,7 @@ def __post_init__(self): # 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, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, ), ], sim_device=self.sim.device, @@ -445,7 +405,7 @@ def __post_init__(self): enable_visualization=True, num_open_xr_hand_joints=2 * 26, sim_device=self.sim.device, - hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, ), ], sim_device=self.sim.device, 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 index 636f347109f4..30b17e89493a 100644 --- 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 @@ -57,20 +57,16 @@ def __post_init__(self): # 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) + self.actions.upper_body_ik.pink_controlled_joint_names.append(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.actions.upper_body_ik.controller.urdf_path = temp_urdf_output_path + self.actions.upper_body_ik.controller.mesh_path = temp_urdf_meshes_output_path self.teleop_devices = DevicesCfg( devices={ @@ -81,7 +77,7 @@ def __post_init__(self): # 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, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, ), ], sim_device=self.sim.device, diff --git a/tools/test_settings.py b/tools/test_settings.py index 936d38c49584..d15658757f2b 100644 --- a/tools/test_settings.py +++ b/tools/test_settings.py @@ -26,6 +26,7 @@ "test_factory_environments.py": 1000, # This test runs through Factory environments for 100 steps each "test_multi_agent_environments.py": 800, # This test runs through multi-agent environments for 100 steps each "test_generate_dataset.py": 500, # This test runs annotation for 10 demos and generation until one succeeds + "test_pink_ik.py": 1000, # This test runs through all the pink IK environments through various motions "test_environments_training.py": 6000, "test_simulation_render_config.py": 500, "test_operational_space.py": 500,