Commit 4d3eddc8 authored by Ashwin Varghese Kuruttukulam's avatar Ashwin Varghese Kuruttukulam Committed by Kelly Guo

Adds GR1 scene with Pink IK + Groot Mimic data generation and training (#294)

This PR adds GR00T Mimic support for GR1, including addition of:

1. GR1T2 Scene
2. Pink IK controller
3. Integration for GR1T2 scene with Groot Mimic. Record, Annotate and
Data Generation
4. Training scripts for GR1T2 scene using Groot Mimic Data
5. Coding style fixes for math.py

1. https://github.com/isaac-sim/IsaacLab-Internal/pull/267
2. https://github.com/isaac-sim/IsaacLab-Internal/pull/280
3. https://github.com/isaac-sim/IsaacLab-Internal/pull/287
Note: These branches have been merged into this PR branch for testing.

- New feature (non-breaking change which adds functionality)
- This change requires a documentation update

Please attach before and after screenshots of the change if applicable.

<!--
Example:

| Before | After |
| ------ | ----- |
| _gif/png before_ | _gif/png after_ |

To upload images to a PR -- simply drag and drop an image while in edit
mode and it should upload the image directly. You can then paste that
source into the above before/after sections.
-->

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] documentation PR is here:
https://github.com/isaac-sim/IsaacLab-Internal/pull/299
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

Note: There are these 3 TODOs related to moving USD files to the correct
location. But I'm planning to move those to the right location and then
make the changes in a follow up MR.

---------
Signed-off-by: 's avatarAshwin Varghese Kuruttukulam <123109010+ashwinvkNV@users.noreply.github.com>
Signed-off-by: 's avatarKelly Guo <kellyguo123@hotmail.com>
Co-authored-by: 's avatarRafael Wiltz <rwiltz@nvidia.com>
Co-authored-by: 's avatarJiakai Zhang <jaczhang@nvidia.com>
Co-authored-by: 's avatarCY Chen <cyc@nvidia.com>
Co-authored-by: 's avatarPeter Du <peterd@nvidia.com>
Co-authored-by: 's avatarKelly Guo <kellyg@nvidia.com>
parent f03c8335
......@@ -64,6 +64,7 @@ Guidelines for modifications:
* Jan Kerner
* Jean Tampon
* Jia Lin Yuan
* Jiakai Zhang
* Jinghuan Shang
* Jingzhou Liu
* Jinqi Wei
......
......@@ -178,6 +178,10 @@ autodoc_mock_imports = [
"tensordict",
"trimesh",
"toml",
"qpsolvers",
"pink",
"pinocchio",
"nvidia.srl",
]
# List of zero or more Sphinx-specific warning categories to be squelched (i.e.,
......
channels:
- conda-forge
- defaults
dependencies:
- python=3.10
- importlib_metadata
......@@ -120,7 +120,8 @@ if %errorlevel% equ 0 (
echo [INFO] Conda environment named '%env_name%' already exists.
) else (
echo [INFO] Creating conda environment named '%env_name%'...
call conda create -y --name %env_name% python=3.10
echo [INFO] Installing dependencies from %ISAACLAB_PATH%\environment.yml
call conda env create -y --file %ISAACLAB_PATH%\environment.yml -n %env_name%
)
rem cache current paths for later
set "cache_pythonpath=%PYTHONPATH%"
......@@ -199,10 +200,6 @@ rem remove variables from environment during deactivation
echo $env:LD_LIBRARY_PATH="%cache_pythonpath%"
) > "%CONDA_PREFIX%\etc\conda\deactivate.d\unsetenv_vars.ps1"
rem install some extra dependencies
echo [INFO] Installing extra dependencies (this might take a few minutes)...
call conda install -c conda-forge -y importlib_metadata >nul 2>&1
rem deactivate the environment
call conda deactivate
rem add information to the user about alias
......
......@@ -141,7 +141,10 @@ setup_conda_env() {
echo -e "[INFO] Conda environment named '${env_name}' already exists."
else
echo -e "[INFO] Creating conda environment named '${env_name}'..."
conda create -y --name ${env_name} python=3.10
echo -e "[INFO] Installing dependencies from ${ISAACLAB_PATH}/environment.yml"
# Create environment from YAML file with specified name
conda env create -y --file ${ISAACLAB_PATH}/environment.yml -n ${env_name}
fi
# cache current paths for later
......@@ -208,10 +211,6 @@ setup_conda_env() {
'' >> ${CONDA_PREFIX}/etc/conda/deactivate.d/unsetenv.sh
fi
# install some extra dependencies
echo -e "[INFO] Installing extra dependencies (this might take a few minutes)..."
conda install -c conda-forge -y importlib_metadata &> /dev/null
# deactivate the environment
conda deactivate
# add information to the user about alias
......
......@@ -7,12 +7,17 @@
Script to add mimic annotations to demos to be used as source demos for mimic dataset generation.
"""
# Launching Isaac Sim Simulator first.
import argparse
# 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 pinocchio # noqa: F401
from isaaclab.app import AppLauncher
# Launching Isaac Sim Simulator first.
# add argparse arguments
parser = argparse.ArgumentParser(description="Annotate demonstrations for Isaac Lab environments.")
parser.add_argument("--task", type=str, default=None, help="Name of the task.")
......@@ -48,6 +53,7 @@ import isaaclab_mimic.envs # noqa: F401
# Only enables inputs if this script is NOT headless mode
if not args_cli.headless and not os.environ.get("HEADLESS", 0):
from isaaclab.devices import Se3Keyboard
from isaaclab.envs import ManagerBasedRLMimicEnv
from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg
from isaaclab.managers import RecorderTerm, RecorderTermCfg, TerminationTermCfg
......@@ -204,7 +210,8 @@ def main():
# no need to annotate the last subtask term signal, so remove it from the list
subtask_term_signal_names[eef_name].pop()
# reset environment
# reset environment - we do a full simulation reset to achieve full determinism
env.sim.reset()
env.reset()
# Only enables inputs if this script is NOT headless mode
......@@ -281,6 +288,7 @@ def replay_episode(
# read initial state and actions from the loaded episode
initial_state = episode.data["initial_state"]
actions = episode.data["actions"]
env.sim.reset()
env.recorder_manager.reset()
env.reset_to(initial_state, None, is_relative=True)
first_action = True
......
......@@ -7,6 +7,10 @@
Main data generation script.
"""
# 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 pinocchio # noqa: F401
"""Launch Isaac Sim Simulator first."""
import argparse
......
......@@ -3,12 +3,29 @@
#
# SPDX-License-Identifier: BSD-3-Clause
"""Script to play and evaluate a trained policy from robomimic."""
"""Script to play and evaluate a trained policy from robomimic.
This script loads a robomimic policy and plays it in an Isaac Lab environment.
Args:
task: Name of the environment.
checkpoint: Path to the robomimic policy checkpoint.
horizon: If provided, override the step horizon of each rollout.
num_rollouts: If provided, override the number of rollouts.
seed: If provided, overeride the default random seed.
norm_factor_min: If provided, minimum value of the action space normalization factor.
norm_factor_max: If provided, maximum value of the action space normalization factor.
"""
"""Launch Isaac Sim Simulator first."""
import argparse
# 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 pinocchio # noqa: F401
from isaaclab.app import AppLauncher
# add argparse arguments
......@@ -21,6 +38,13 @@ parser.add_argument("--checkpoint", type=str, default=None, help="Pytorch model
parser.add_argument("--horizon", type=int, default=800, help="Step horizon of each rollout.")
parser.add_argument("--num_rollouts", type=int, default=1, help="Number of rollouts.")
parser.add_argument("--seed", type=int, default=101, help="Random seed.")
parser.add_argument(
"--norm_factor_min", type=float, default=None, help="Optional: minimum value of the normalization factor."
)
parser.add_argument(
"--norm_factor_max", type=float, default=None, help="Optional: maximum value of the normalization factor."
)
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
......@@ -33,6 +57,7 @@ simulation_app = app_launcher.app
"""Rest everything follows."""
import copy
import gymnasium as gym
import torch
......@@ -43,19 +68,51 @@ from isaaclab_tasks.utils import parse_env_cfg
def rollout(policy, env, horizon, device):
policy.start_episode
"""Perform a single rollout of the policy in the environment.
Args:
policy: The robomimicpolicy to play.
env: The environment to play in.
horizon: The step horizon of each rollout.
device: The device to run the policy on.
Returns:
terminated: Whether the rollout terminated.
traj: The trajectory of the rollout.
"""
policy.start_episode()
obs_dict, _ = env.reset()
traj = dict(actions=[], obs=[], next_obs=[])
for i in range(horizon):
# Prepare observations
obs = obs_dict["policy"]
obs = copy.deepcopy(obs_dict["policy"])
for ob in obs:
obs[ob] = torch.squeeze(obs[ob])
# Check if environment image observations
if hasattr(env.cfg, "image_obs_list"):
# Process image observations for robomimic inference
for image_name in env.cfg.image_obs_list:
if image_name in obs_dict["policy"].keys():
# Convert from chw uint8 to hwc normalized float
image = torch.squeeze(obs_dict["policy"][image_name])
image = image.permute(2, 0, 1).clone().float()
image = image / 255.0
image = image.clip(0.0, 1.0)
obs[image_name] = image
traj["obs"].append(obs)
# Compute actions
actions = policy(obs)
# Unnormalize actions
if args_cli.norm_factor_min is not None and args_cli.norm_factor_max is not None:
actions = (
(actions + 1) * (args_cli.norm_factor_max - args_cli.norm_factor_min)
) / 2 + args_cli.norm_factor_min
actions = torch.from_numpy(actions).to(device=device).view(1, env.action_space.shape[1])
# Apply actions
......
......@@ -28,14 +28,19 @@
"""
The main entry point for training policies from pre-collected data.
Args:
algo: name of the algorithm to run.
task: name of the environment.
name: if provided, override the experiment name defined in the config
dataset: if provided, override the dataset path defined in the config
This file has been modified from the original version in the following ways:
This script loads dataset(s), creates a model based on the algorithm specified,
and trains the model. It supports training on various environments with multiple
algorithms from robomimic.
Args:
algo: Name of the algorithm to run.
task: Name of the environment.
name: If provided, override the experiment name defined in the config.
dataset: If provided, override the dataset path defined in the config.
log_dir: Directory to save logs.
normalize_training_actions: Whether to normalize actions in the training data.
This file has been modified from the original robomimic version to integrate with IsaacLab.
"""
"""Launch Isaac Sim Simulator first."""
......@@ -48,11 +53,16 @@ simulation_app = app_launcher.app
"""Rest everything follows."""
# Standard library imports
import argparse
# Third-party imports
import gymnasium as gym
import h5py
import json
import numpy as np
import os
import shutil
import sys
import time
import torch
......@@ -61,21 +71,77 @@ from collections import OrderedDict
from torch.utils.data import DataLoader
import psutil
# Robomimic imports
import robomimic.utils.env_utils as EnvUtils
import robomimic.utils.file_utils as FileUtils
import robomimic.utils.obs_utils as ObsUtils
import robomimic.utils.torch_utils as TorchUtils
import robomimic.utils.train_utils as TrainUtils
from robomimic.algo import algo_factory
from robomimic.config import config_factory
from robomimic.config import Config, config_factory
from robomimic.utils.log_utils import DataLogger, PrintLogger
# Needed so that environment is registered
# Isaac Lab imports (needed so that environment is registered)
import isaaclab_tasks # noqa: F401
def train(config, device):
"""Train a model using the algorithm."""
def normalize_hdf5_actions(config: Config, log_dir: str) -> str:
"""Normalizes actions in hdf5 dataset to [-1, 1] range.
Args:
config: The configuration object containing dataset path.
log_dir: Directory to save normalization parameters.
Returns:
Path to the normalized dataset.
"""
base, ext = os.path.splitext(config.train.data)
normalized_path = base + "_normalized" + ext
# Copy the original dataset
print(f"Creating normalized dataset at {normalized_path}")
shutil.copyfile(config.train.data, normalized_path)
# Open the new dataset and normalize the actions
with h5py.File(normalized_path, "r+") as f:
dataset_paths = [f"/data/demo_{str(i)}/actions" for i in range(len(f["data"].keys()))]
# Compute the min and max of the dataset
dataset = np.array(f[dataset_paths[0]]).flatten()
for i, path in enumerate(dataset_paths):
if i != 0:
data = np.array(f[path]).flatten()
dataset = np.append(dataset, data)
max = np.max(dataset)
min = np.min(dataset)
# Normalize the actions
for i, path in enumerate(dataset_paths):
data = np.array(f[path])
normalized_data = 2 * ((data - min) / (max - min)) - 1 # Scale to [-1, 1] range
del f[path]
f[path] = normalized_data
# Save the min and max values to log directory
with open(os.path.join(log_dir, "normalization_params.txt"), "w") as f:
f.write(f"min: {min}\n")
f.write(f"max: {max}\n")
return normalized_path
def train(config: Config, device: str, log_dir: str, ckpt_dir: str, video_dir: str):
"""Train a model using the algorithm specified in config.
Args:
config: Configuration object.
device: PyTorch device to use for training.
log_dir: Directory to save logs.
ckpt_dir: Directory to save checkpoints.
video_dir: Directory to save videos.
"""
# first set seeds
np.random.seed(config.train.seed)
torch.manual_seed(config.train.seed)
......@@ -83,7 +149,6 @@ def train(config, device):
print("\n============= New Training Run with Config =============")
print(config)
print("")
log_dir, ckpt_dir, video_dir = TrainUtils.get_exp_dir(config)
print(f">>> Saving logs into directory: {log_dir}")
print(f">>> Saving checkpoints into directory: {ckpt_dir}")
......@@ -278,8 +343,12 @@ def train(config, device):
data_logger.close()
def main(args):
"""Train a model on a task using a specified algorithm."""
def main(args: argparse.Namespace):
"""Train a model on a task using a specified algorithm.
Args:
args: Command line arguments.
"""
# load config
if args.task is not None:
# obtain the configuration entry point
......@@ -315,6 +384,11 @@ def main(args):
# change location of experiment directory
config.train.output_dir = os.path.abspath(os.path.join("./logs", args.log_dir, args.task))
log_dir, ckpt_dir, video_dir = TrainUtils.get_exp_dir(config)
if args.normalize_training_actions:
config.train.data = normalize_hdf5_actions(config, log_dir)
# get torch device
device = TorchUtils.get_torch_device(try_to_use_cuda=config.train.cuda)
......@@ -323,7 +397,7 @@ def main(args):
# catch error during training and print it
res_str = "finished run successfully!"
try:
train(config, device=device)
train(config, device, log_dir, ckpt_dir, video_dir)
except Exception as e:
res_str = f"run failed with error:\n{e}\n\n{traceback.format_exc()}"
print(res_str)
......@@ -351,6 +425,7 @@ if __name__ == "__main__":
parser.add_argument("--task", type=str, default=None, help="Name of the task.")
parser.add_argument("--algo", type=str, default=None, help="Name of the algorithm.")
parser.add_argument("--log_dir", type=str, default="robomimic", help="Path to log directory")
parser.add_argument("--normalize_training_actions", action="store_true", default=False, help="Normalize actions")
args = parser.parse_args()
......
......@@ -24,9 +24,25 @@ optional arguments:
"""Launch Isaac Sim Simulator first."""
# Standard library imports
import argparse
import contextlib
# Third-party imports
import gymnasium as gym
import numpy as np
import os
import time
import torch
# Omniverse logger
import omni.log
# 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 pinocchio # noqa: F401
# Isaac Lab AppLauncher
from isaaclab.app import AppLauncher
# add argparse arguments
......@@ -57,19 +73,15 @@ if "handtracking" in args_cli.teleop_device.lower():
app_launcher_args["xr"] = True
# launch the simulator
app_launcher = AppLauncher(app_launcher_args)
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app
"""Rest everything follows."""
import contextlib
import gymnasium as gym
import time
import torch
import omni.log
if "handtracking" in args_cli.teleop_device.lower():
from isaacsim.xr.openxr import OpenXRSpec
# Additional Isaac Lab imports that can only be imported after the simulator is running
from isaaclab.devices import OpenXRDevice, Se3Keyboard, Se3SpaceMouse
from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2Retargeter
from isaaclab.devices.openxr.retargeters.manipulator import GripperRetargeter, Se3AbsRetargeter, Se3RelRetargeter
from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg
......@@ -80,18 +92,23 @@ from isaaclab_tasks.utils.parse_cfg import parse_env_cfg
class RateLimiter:
"""Convenience class for enforcing rates in loops."""
def __init__(self, hz):
"""
def __init__(self, hz: int):
"""Initialize a RateLimiter with specified frequency.
Args:
hz (int): frequency to enforce
hz: Frequency to enforce in Hertz.
"""
self.hz = hz
self.last_time = time.time()
self.sleep_duration = 1.0 / hz
self.render_period = min(0.033, self.sleep_duration)
def sleep(self, env):
"""Attempt to sleep at the specified rate in hz."""
def sleep(self, env: gym.Env):
"""Attempt to sleep at the specified rate in hz.
Args:
env: Environment to render during sleep periods.
"""
next_wakeup_time = self.last_time + self.sleep_duration
while time.time() < next_wakeup_time:
time.sleep(self.render_period)
......@@ -105,16 +122,47 @@ class RateLimiter:
self.last_time += self.sleep_duration
def pre_process_actions(delta_pose: torch.Tensor, gripper_command: bool) -> torch.Tensor:
"""Pre-process actions for the environment."""
def pre_process_actions(
teleop_data: tuple[np.ndarray, bool] | list[tuple[np.ndarray, np.ndarray, np.ndarray]], num_envs: int, device: str
) -> torch.Tensor:
"""Convert teleop data to the format expected by the environment action space.
Args:
teleop_data: Data from the teleoperation device.
num_envs: Number of environments.
device: Device to create tensors on.
Returns:
Processed actions as a tensor.
"""
# compute actions based on environment
if "Reach" in args_cli.task:
delta_pose, gripper_command = teleop_data
# convert to torch
delta_pose = torch.tensor(delta_pose, dtype=torch.float, device=device).repeat(num_envs, 1)
# note: reach is the only one that uses a different action space
# compute actions
return delta_pose
elif "PickPlace-GR1T2" in args_cli.task:
(left_wrist_pose, right_wrist_pose, hand_joints) = teleop_data[0]
# Reconstruct actions_arms tensor with converted positions and rotations
actions = torch.tensor(
np.concatenate([
left_wrist_pose, # left ee pose
right_wrist_pose, # right ee pose
hand_joints, # hand joint angles
]),
device=device,
dtype=torch.float32,
).unsqueeze(0)
# Concatenate arm poses and hand joint angles
return actions
else:
# resolve gripper command
gripper_vel = torch.zeros((delta_pose.shape[0], 1), dtype=torch.float, device=delta_pose.device)
delta_pose, gripper_command = teleop_data
# convert to torch
delta_pose = torch.tensor(delta_pose, dtype=torch.float, device=device).repeat(num_envs, 1)
gripper_vel = torch.zeros((delta_pose.shape[0], 1), dtype=torch.float, device=device)
gripper_vel[:] = -1 if gripper_command else 1
# compute actions
return torch.concat([delta_pose, gripper_vel], dim=1)
......@@ -210,7 +258,7 @@ def main():
nonlocal running_recording_instance
running_recording_instance = False
def create_teleop_device(device_name: str):
def create_teleop_device(device_name: str, env: gym.Env):
"""Create and configure teleoperation device for robot control.
Args:
......@@ -224,10 +272,32 @@ def main():
DeviceBase: Configured teleoperation device ready for robot control
"""
device_name = device_name.lower()
nonlocal running_recording_instance
if device_name == "keyboard":
return Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5)
elif device_name == "spacemouse":
return Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5)
elif "dualhandtracking_abs" in device_name and "GR1T2" in env.cfg.env_name:
# Create GR1T2 retargeter with desired configuration
gr1t2_retargeter = GR1T2Retargeter(
enable_visualization=True,
num_open_xr_hand_joints=2 * (int(OpenXRSpec.HandJointEXT.XR_HAND_JOINT_LITTLE_TIP_EXT) + 1),
device=env.unwrapped.device,
hand_joint_names=env.scene["robot"].data.joint_names[-22:],
)
# Create hand tracking device with retargeter
device = OpenXRDevice(
env_cfg.xr,
hand=OpenXRDevice.Hand.BOTH,
retargeters=[gr1t2_retargeter],
)
device.add_callback("RESET", reset_recording_instance)
device.add_callback("START", start_recording_instance)
device.add_callback("STOP", stop_recording_instance)
running_recording_instance = False
return device
elif "handtracking" in device_name:
# Create Franka retargeter with desired configuration
if "_abs" in device_name:
......@@ -247,20 +317,20 @@ def main():
device.add_callback("START", start_recording_instance)
device.add_callback("STOP", stop_recording_instance)
nonlocal running_recording_instance
running_recording_instance = False
return device
else:
raise ValueError(
f"Invalid device interface '{device_name}'. Supported: 'keyboard', 'spacemouse', 'handtracking',"
" 'handtracking_abs'."
" 'handtracking_abs', 'dualhandtracking_abs'."
)
teleop_interface = create_teleop_device(args_cli.teleop_device)
teleop_interface = create_teleop_device(args_cli.teleop_device, env)
teleop_interface.add_callback("R", reset_recording_instance)
print(teleop_interface)
# reset before starting
env.sim.reset()
env.reset()
teleop_interface.reset()
......@@ -269,15 +339,13 @@ def main():
success_step_count = 0
with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode():
while simulation_app.is_running():
# get keyboard command
delta_pose, gripper_command = teleop_interface.advance()
# convert to torch
delta_pose = torch.tensor(delta_pose, dtype=torch.float, device=env.device).repeat(env.num_envs, 1)
# compute actions based on environment
actions = pre_process_actions(delta_pose, gripper_command)
# get data from teleop device
teleop_data = teleop_interface.advance()
# perform action on environment
if running_recording_instance:
# compute actions based on environment
actions = pre_process_actions(teleop_data, env.num_envs, env.device)
env.step(actions)
else:
env.sim.render()
......@@ -296,6 +364,7 @@ def main():
success_step_count = 0
if should_reset_recording_instance:
env.sim.reset()
env.recorder_manager.reset()
env.reset()
should_reset_recording_instance = False
......
......@@ -7,8 +7,13 @@
"""Launch Isaac Sim Simulator first."""
import argparse
# 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 pinocchio # noqa: F401
from isaaclab.app import AppLauncher
# add argparse arguments
......
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.36.6"
version = "0.36.7"
# Description
title = "Isaac Lab framework for Robot Learning"
......
Changelog
---------
0.36.6 (2025-04-09)
0.36.7 (2025-04-09)
~~~~~~~~~~~~~~~~~~~
Changed
......@@ -12,7 +12,7 @@ Changed
the cuda device, which results in NCCL errors on distributed setups.
0.36.5 (2025-04-01)
0.36.6 (2025-04-01)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -21,7 +21,7 @@ Fixed
* Adds check in RecorderManager to ensure that the success indicator is only set if the termination manager is present.
0.36.4 (2025-03-24)
0.36.5 (2025-03-24)
~~~~~~~~~~~~~~~~~~~
Changed
......@@ -31,7 +31,7 @@ Changed
the default settings will be used from the experience files and the double definition is removed.
0.36.3 (2025-03-17)
0.36.4 (2025-03-17)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -41,8 +41,17 @@ Fixed
:attr:`effort_limit` is set.
0.36.2 (2025-03-12)
~~~~~~~~~~~~~~~~~~~
0.36.3 (2025-03-10)
~~~~~~~~~~~~~~~~~~~~
Changed
^^^^^^^
* Added the PinkIKController controller class that interfaces Isaac Lab with the Pink differential inverse kinematics solver to allow control of multiple links in a robot using a single solver.
0.36.2 (2025-03-07)
~~~~~~~~~~~~~~~~~~~~
Changed
^^^^^^^
......
......@@ -15,3 +15,5 @@ from .differential_ik import DifferentialIKController
from .differential_ik_cfg import DifferentialIKControllerCfg
from .operational_space import OperationalSpaceController
from .operational_space_cfg import OperationalSpaceControllerCfg
from .pink_ik import PinkIKController
from .pink_ik_cfg import PinkIKControllerCfg
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Pink IK controller implementation for IsaacLab.
This module provides integration between Pink inverse kinematics solver and IsaacLab.
Pink is a differentiable inverse kinematics solver framework that provides task-space control capabilities.
"""
import numpy as np
import torch
import qpsolvers
from pink import build_ik
from pink.configuration import Configuration
from pinocchio.robot_wrapper import RobotWrapper
from .pink_ik_cfg import PinkIKControllerCfg
class PinkIKController:
"""Integration of Pink IK controller with Isaac Lab.
The Pink IK controller is available at: https://github.com/stephane-caron/pink
"""
def __init__(self, cfg: PinkIKControllerCfg, device: str):
"""Initialize the Pink IK Controller.
Args:
cfg: The configuration for the controller.
device: The device to use for computations (e.g., 'cuda:0').
"""
# Initialize the robot model from URDF and mesh files
self.robot_wrapper = RobotWrapper.BuildFromURDF(cfg.urdf_path, cfg.mesh_path, root_joint=None)
self.pink_configuration = Configuration(
self.robot_wrapper.model, self.robot_wrapper.data, self.robot_wrapper.q0
)
# Set the default targets for each task from the configuration
for task in cfg.variable_input_tasks:
task.set_target_from_configuration(self.pink_configuration)
for task in cfg.fixed_input_tasks:
task.set_target_from_configuration(self.pink_configuration)
# Select the solver for the inverse kinematics
self.solver = qpsolvers.available_solvers[0]
if "quadprog" in qpsolvers.available_solvers:
self.solver = "quadprog"
# Map joint names from Isaac Lab to Pink's joint conventions
pink_joint_names = self.robot_wrapper.model.names.tolist()[1:] # Skip the root and universal joints
isaac_lab_joint_names = cfg.joint_names
# Create reordering arrays for joint indices
self.isaac_lab_to_pink_ordering = [isaac_lab_joint_names.index(pink_joint) for pink_joint in pink_joint_names]
self.pink_to_isaac_lab_ordering = [
pink_joint_names.index(isaac_lab_joint) for isaac_lab_joint in isaac_lab_joint_names
]
self.cfg = cfg
self.device = device
"""
Operations.
"""
def reorder_array(self, input_array: list[float], reordering_array: list[int]) -> list[float]:
"""Reorder the input array based on the provided ordering.
Args:
input_array: The array to reorder.
reordering_array: The indices to use for reordering.
Returns:
Reordered array.
"""
return [input_array[i] for i in reordering_array]
def initialize(self):
"""Initialize the internals of the controller.
This method is called during setup but before the first compute call.
"""
pass
def compute(
self,
curr_joint_pos: np.ndarray,
dt: float,
) -> torch.Tensor:
"""Compute the target joint positions based on current state and tasks.
Args:
curr_joint_pos: The current joint positions.
dt: The time step for computing joint position changes.
Returns:
The target joint positions as a tensor.
"""
# Initialize joint positions for Pink, including the root and universal joints
joint_positions_pink = np.array(self.reorder_array(curr_joint_pos, self.isaac_lab_to_pink_ordering))
# Update Pink's robot configuration with the current joint positions
self.pink_configuration.update(joint_positions_pink)
problem = build_ik(self.pink_configuration, self.cfg.variable_input_tasks + self.cfg.fixed_input_tasks, dt)
result = qpsolvers.solve_problem(problem, solver=self.solver)
Delta_q = result.x
# Delta_q being None means the solver could not find a solution
if Delta_q is None:
# Print warning and return the current joint positions as the target
# Not using omni.log since its not available in CI during docs build
print("Warning: IK quadratic solver could not find a solution! Did not update the target joint positions.")
return torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32)
# Discard the first 6 values (for root and universal joints)
pink_joint_angle_changes = Delta_q
# Reorder the joint angle changes back to Isaac Lab conventions
joint_vel_isaac_lab = torch.tensor(
self.reorder_array(pink_joint_angle_changes, self.pink_to_isaac_lab_ordering),
device=self.device,
dtype=torch.float,
)
# Add the velocity changes to the current joint positions to get the target joint positions
target_joint_pos = torch.add(
joint_vel_isaac_lab, torch.tensor(curr_joint_pos, device=self.device, dtype=torch.float32)
)
return target_joint_pos
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Configuration for Pink IK controller."""
from dataclasses import MISSING
from pink.tasks import FrameTask
from isaaclab.utils import configclass
@configclass
class PinkIKControllerCfg:
"""Configuration settings for the Pink IK Controller.
The Pink IK controller can be found at: https://github.com/stephane-caron/pink
"""
urdf_path: str | None = None
"""Path to the robot's URDF file. This file is used by Pinocchio's `robot_wrapper.BuildFromURDF` to load the robot model."""
mesh_path: str | None = None
"""Path to the mesh files associated with the robot. These files are also loaded by Pinocchio's `robot_wrapper.BuildFromURDF`."""
num_hand_joints: int = 0
"""The number of hand joints in the robot. The action space for the controller contains the pose_dim(7)*num_controlled_frames + num_hand_joints.
The last num_hand_joints values of the action are the hand joint angles."""
variable_input_tasks: list[FrameTask] = MISSING
"""
A list of tasks for the Pink IK controller. These tasks are controllable by the env action.
These tasks can be used to control the pose of a frame or the angles of joints.
For more details, visit: https://github.com/stephane-caron/pink
"""
fixed_input_tasks: list[FrameTask] = MISSING
"""
A list of tasks for the Pink IK controller. These tasks are fixed and not controllable by the env action.
These tasks can be used to fix the pose of a frame or the angles of joints to a desired configuration.
For more details, visit: https://github.com/stephane-caron/pink
"""
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."""
articulation_name: str = "robot"
"""The name of the articulation USD asset in the scene."""
base_link_name: str = "base_link"
"""The name of the base link in the USD asset."""
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Helper functions for Isaac Lab controllers.
This module provides utility functions to help with controller implementations.
"""
import os
from isaacsim.core.utils.extensions import enable_extension
enable_extension("isaacsim.asset.exporter.urdf")
import omni.log
from nvidia.srl.from_usd.to_urdf import UsdToUrdf
def convert_usd_to_urdf(usd_path: str, output_path: str, force_conversion: bool = True) -> tuple[str, str]:
"""Convert a USD file to URDF format.
Args:
usd_path: Path to the USD file to convert.
output_path: Directory to save the converted URDF and mesh files.
force_conversion: Whether to force the conversion even if the URDF and mesh files already exist.
Returns:
A tuple containing the paths to the URDF file and the mesh directory.
"""
usd_to_urdf_kwargs = {
"node_names_to_remove": None,
"edge_names_to_remove": None,
"root": None,
"parent_link_is_body_1": None,
}
urdf_output_dir = os.path.join(output_path, "urdf")
urdf_file_name = os.path.basename(usd_path).split(".")[0] + ".urdf"
urdf_output_path = urdf_output_dir + "/" + urdf_file_name
urdf_meshes_output_dir = os.path.join(output_path, "meshes")
if not os.path.exists(urdf_output_path) or not os.path.exists(urdf_meshes_output_dir) or force_conversion:
usd_to_urdf = UsdToUrdf.init_from_file(usd_path, **usd_to_urdf_kwargs)
os.makedirs(urdf_output_dir, exist_ok=True)
os.makedirs(urdf_meshes_output_dir, exist_ok=True)
output_path = usd_to_urdf.save_to_file(
urdf_output_path=urdf_output_path,
visualize_collision_meshes=False,
mesh_dir=urdf_meshes_output_dir,
mesh_path_prefix="",
)
# The current version of the usd to urdf converter creates "inf" effort,
# This has to be replaced with a max value for the urdf to be valid
# Open the file for reading and writing
with open(urdf_output_path) as file:
# Read the content of the file
content = file.read()
# Replace all occurrences of 'inf' with '0'
content = content.replace("inf", "0.")
# Open the file again to write the modified content
with open(urdf_output_path, "w") as file:
# Write the modified content back to the file
file.write(content)
return urdf_output_path, urdf_meshes_output_dir
def change_revolute_to_fixed(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 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()
for joint in fixed_joints:
old_str = f'<joint name="{joint}" type="revolute">'
new_str = f'<joint name="{joint}" type="fixed">'
if verbose:
omni.log.warn(f"Replacing {joint} with fixed joint")
omni.log.warn(old_str)
omni.log.warn(new_str)
if old_str not in content:
omni.log.warn(f"Error: Could not find revolute joint named '{joint}' in URDF file")
content = content.replace(old_str, new_str)
with open(urdf_path, "w") as file:
file.write(content)
......@@ -4,6 +4,7 @@
# SPDX-License-Identifier: BSD-3-Clause
"""Retargeters for mapping input device data to robot commands."""
from .humanoid.fourier.gr1t2_retargeter import GR1T2Retargeter
from .manipulator.gripper_retargeter import GripperRetargeter
from .manipulator.se3_abs_retargeter import Se3AbsRetargeter
from .manipulator.se3_rel_retargeter import Se3RelRetargeter
retargeting:
finger_tip_link_names:
- GR1T2_fourier_hand_6dof_L_thumb_distal_link
- GR1T2_fourier_hand_6dof_L_index_intermediate_link
- GR1T2_fourier_hand_6dof_L_middle_intermediate_link
- GR1T2_fourier_hand_6dof_L_ring_intermediate_link
- GR1T2_fourier_hand_6dof_L_pinky_intermediate_link
low_pass_alpha: 0.2
scaling_factor: 1.0
target_joint_names:
- L_index_proximal_joint
- L_middle_proximal_joint
- L_pinky_proximal_joint
- L_ring_proximal_joint
- L_index_intermediate_joint
- L_middle_intermediate_joint
- L_pinky_intermediate_joint
- L_ring_intermediate_joint
- L_thumb_proximal_yaw_joint
- L_thumb_proximal_pitch_joint
- L_thumb_distal_joint
- L_thumb_distal_joint
type: DexPilot
urdf_path: /tmp/GR1_T2_left_hand.urdf
wrist_link_name: l_hand_base_link
retargeting:
finger_tip_link_names:
- GR1T2_fourier_hand_6dof_R_thumb_distal_link
- GR1T2_fourier_hand_6dof_R_index_intermediate_link
- GR1T2_fourier_hand_6dof_R_middle_intermediate_link
- GR1T2_fourier_hand_6dof_R_ring_intermediate_link
- GR1T2_fourier_hand_6dof_R_pinky_intermediate_link
low_pass_alpha: 0.2
scaling_factor: 1.0
target_joint_names:
- R_index_proximal_joint
- R_middle_proximal_joint
- R_pinky_proximal_joint
- R_ring_proximal_joint
- R_index_intermediate_joint
- R_middle_intermediate_joint
- R_pinky_intermediate_joint
- R_ring_intermediate_joint
- R_thumb_proximal_yaw_joint
- R_thumb_proximal_pitch_joint
- R_thumb_distal_joint
type: DexPilot
urdf_path: /tmp/GR1_T2_right_hand.urdf
wrist_link_name: r_hand_base_link
# Copyright (c) 2025, The Isaac Lab Project Developers.
# 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, -1, 0],
[-1, 0, 0],
[0, 0, -1],
])
_OPERATOR2MANO_LEFT = np.array([
[0, -1, 0],
[-1, 0, 0],
[0, 0, -1],
])
_LEFT_HAND_JOINT_NAMES = [
"L_index_proximal_joint",
"L_index_intermediate_joint",
"L_middle_proximal_joint",
"L_middle_intermediate_joint",
"L_pinky_proximal_joint",
"L_pinky_intermediate_joint",
"L_ring_proximal_joint",
"L_ring_intermediate_joint",
"L_thumb_proximal_yaw_joint",
"L_thumb_proximal_pitch_joint",
"L_thumb_distal_joint",
]
_RIGHT_HAND_JOINT_NAMES = [
"R_index_proximal_joint",
"R_index_intermediate_joint",
"R_middle_proximal_joint",
"R_middle_intermediate_joint",
"R_pinky_proximal_joint",
"R_pinky_intermediate_joint",
"R_ring_proximal_joint",
"R_ring_intermediate_joint",
"R_thumb_proximal_yaw_joint",
"R_thumb_proximal_pitch_joint",
"R_thumb_distal_joint",
]
class GR1TR2DexRetargeting:
"""A class for hand retargeting with GR1Fourier.
Handles retargeting of OpenXRhand tracking data to GR1T2 robot hand joint angles.
"""
def __init__(
self,
hand_joint_names: list[str],
right_hand_config_filename: str = "fourier_hand_right_dexpilot.yml",
left_hand_config_filename: str = "fourier_hand_left_dexpilot.yml",
left_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/GR1T2_assets/GR1_T2_left_hand.urdf",
right_hand_urdf_path: str = f"{ISAACLAB_NUCLEUS_DIR}/Mimic/GR1T2_assets/GR1_T2_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("[GR1T2DexRetargeter] 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 in range(len(_HAND_JOINTS_INDEX)):
joint = hand_joints[_HAND_JOINTS_INDEX[i]]
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:]
wrist_rot = R.from_quat(xr_wrist_quat, scalar_first=True).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
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import numpy as np
import torch
import isaaclab.sim as sim_utils
import isaaclab.utils.math as PoseUtils
from isaaclab.devices import OpenXRDevice
from isaaclab.devices.retargeter_base import RetargeterBase
from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg
from .gr1_t2_dex_retargeting_utils import GR1TR2DexRetargeting
class GR1T2Retargeter(RetargeterBase):
"""Retargets OpenXR hand tracking data to GR1T2 hand end-effector commands.
This retargeter maps hand tracking data from OpenXR to joint commands for the GR1T2 robot's hands.
It handles both left and right hands, converting poses of the hands in OpenXR format joint angles for the GR1T2 robot's hands.
"""
def __init__(
self,
enable_visualization: bool = False,
num_open_xr_hand_joints: int = 100,
device: torch.device = torch.device("cuda:0"),
hand_joint_names: list[str] = [],
):
"""Initialize the GR1T2 hand retargeter.
Args:
enable_visualization: If True, visualize tracked hand joints
num_open_xr_hand_joints: Number of joints tracked by OpenXR
device: PyTorch device for computations
hand_joint_names: List of robot hand joint names
"""
self._hand_joint_names = hand_joint_names
self._hands_controller = GR1TR2DexRetargeting(self._hand_joint_names)
# Initialize visualization if enabled
self._enable_visualization = enable_visualization
self._num_open_xr_hand_joints = num_open_xr_hand_joints
self._device = device
if self._enable_visualization:
marker_cfg = VisualizationMarkersCfg(
prim_path="/Visuals/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, joint_poses: dict[str, np.ndarray]) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Convert hand joint poses to robot end-effector commands.
Args:
joint_poses: Dictionary containing hand joint poses
Returns:
tuple containing:
Left wrist pose
Right wrist pose in USD frame
Retargeted hand joint angles
"""
# Access the left and right hand data using the enum key
left_hand_poses = joint_poses[OpenXRDevice.Hand.LEFT]
right_hand_poses = joint_poses[OpenXRDevice.Hand.RIGHT]
left_wrist = left_hand_poses.get("wrist")
right_wrist = right_hand_poses.get("wrist")
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._device))
# Create array of zeros with length matching number of joint names
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
return left_wrist, self._retarget_abs(right_wrist), retargeted_hand_joints
def _retarget_abs(self, wrist: np.ndarray) -> np.ndarray:
"""Handle absolute pose retargeting.
Args:
wrist: Wrist pose data from OpenXR
Returns:
Retargeted wrist pose in USD control frame
"""
# Convert wrist data in openxr frame to usd control frame
# Create pose object for openxr_right_wrist_in_world
# Note: The pose utils require torch tensors
wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32)
wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32)
openxr_right_wrist_in_world = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat))
# The usd control frame is 180 degrees rotated around z axis wrt to the openxr frame
# This was determined through trial and error
zero_pos = torch.zeros(3, dtype=torch.float32)
# 180 degree rotation around z axis
z_axis_rot_quat = torch.tensor([0, 0, 0, 1], dtype=torch.float32)
usd_right_roll_link_in_openxr_right_wrist = PoseUtils.make_pose(
zero_pos, PoseUtils.matrix_from_quat(z_axis_rot_quat)
)
# Convert wrist pose in openxr frame to usd control frame
usd_right_roll_link_in_world = PoseUtils.pose_in_A_to_pose_in_B(
usd_right_roll_link_in_openxr_right_wrist, openxr_right_wrist_in_world
)
# extract position and rotation
usd_right_roll_link_in_world_pos, usd_right_roll_link_in_world_mat = PoseUtils.unmake_pose(
usd_right_roll_link_in_world
)
usd_right_roll_link_in_world_quat = PoseUtils.quat_from_matrix(usd_right_roll_link_in_world_mat)
return np.concatenate([usd_right_roll_link_in_world_pos, usd_right_roll_link_in_world_quat])
......@@ -5,7 +5,7 @@
from dataclasses import MISSING
from isaaclab.controllers import DifferentialIKControllerCfg, OperationalSpaceControllerCfg
from isaaclab.controllers import DifferentialIKControllerCfg, OperationalSpaceControllerCfg, PinkIKControllerCfg
from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg
from isaaclab.utils import configclass
......@@ -250,6 +250,30 @@ class DifferentialInverseKinematicsActionCfg(ActionTermCfg):
"""The configuration for the differential IK controller."""
@configclass
class PinkInverseKinematicsActionCfg(ActionTermCfg):
"""Configuration for Pink inverse kinematics action term.
This configuration is used to define settings for the Pink inverse kinematics action term,
which is a inverse kinematics framework.
"""
class_type: type[ActionTerm] = task_space_actions.PinkInverseKinematicsAction
"""Specifies the action term class type for Pink inverse kinematics action."""
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."""
@configclass
class OperationalSpaceControllerActionCfg(ActionTermCfg):
"""Configuration for operational space controller action term.
......
......@@ -5,6 +5,7 @@
from __future__ import annotations
import copy
import torch
from collections.abc import Sequence
from typing import TYPE_CHECKING
......@@ -17,6 +18,7 @@ import isaaclab.utils.string as string_utils
from isaaclab.assets.articulation import Articulation
from isaaclab.controllers.differential_ik import DifferentialIKController
from isaaclab.controllers.operational_space import OperationalSpaceController
from isaaclab.controllers.pink_ik import PinkIKController
from isaaclab.managers.action_manager import ActionTerm
from isaaclab.sensors import ContactSensor, ContactSensorCfg, FrameTransformer, FrameTransformerCfg
from isaaclab.sim.utils import find_matching_prims
......@@ -27,6 +29,191 @@ if TYPE_CHECKING:
from . import actions_cfg
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
"""
cfg: actions_cfg.PinkInverseKinematicsActionCfg
"""Configuration for the Pink Inverse Kinematics action term."""
_asset: Articulation
"""The articulation asset to which the action term is applied."""
def __init__(self, cfg: actions_cfg.PinkInverseKinematicsActionCfg, env: ManagerBasedEnv):
"""Initialize the Pink Inverse Kinematics action term.
Args:
cfg: The configuration for this action term.
env: The environment in which the action term will be applied.
"""
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.cfg.pink_controlled_joint_names
)
self.cfg.controller.joint_names = self._pink_controlled_joint_names
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."
self._ik_controllers = []
for _ in range(env.num_envs):
self._ik_controllers.append(PinkIKController(cfg=copy.deepcopy(self.cfg.controller), device=self.device))
# Create tensors to store raw and processed actions
self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device)
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,
]
# 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
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])
)
# """
# Properties.
# """
@property
def hand_joint_dim(self) -> int:
"""Dimension for hand joint positions."""
return self.cfg.controller.num_hand_joints
@property
def position_dim(self) -> int:
"""Dimension for position (x, y, z)."""
return 3
@property
def orientation_dim(self) -> int:
"""Dimension for orientation (w, x, y, z)."""
return 4
@property
def pose_dim(self) -> int:
"""Total pose dimension (position + orientation)."""
return self.position_dim + self.orientation_dim
@property
def action_dim(self) -> int:
"""Dimension of the action space (based on number of tasks and pose dimension)."""
# The tasks for all the controllers are the same, hence just using the first one to calculate the action_dim
return len(self._ik_controllers[0].cfg.variable_input_tasks) * self.pose_dim + self.hand_joint_dim
@property
def raw_actions(self) -> torch.Tensor:
"""Get the raw actions tensor."""
return self._raw_actions
@property
def processed_actions(self) -> torch.Tensor:
"""Get the processed actions tensor."""
return self._processed_actions
# """
# Operations.
# """
def process_actions(self, actions: torch.Tensor):
"""Process the input actions and set targets for each task.
Args:
actions: The input actions tensor.
"""
# Store the raw actions
self._raw_actions[:] = actions
self._processed_actions[:] = self.raw_actions
# Make a copy of actions before modifying so that raw actions are not modified
actions_clone = actions.clone()
# 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)
# 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)
)
controlled_frame_in_base_link_frame_pos, controlled_frame_in_base_link_frame_mat = math_utils.unmake_pose(
controlled_frame_in_base_link_frame
)
# Loop through each task and set the target
for env_index, ik_controller in enumerate(self._ik_controllers):
for task_index, task in enumerate(ik_controller.cfg.variable_input_tasks):
target = task.transform_target_to_world
target.translation = controlled_frame_in_base_link_frame_pos[task_index, env_index, :].cpu().numpy()
target.rotation = controlled_frame_in_base_link_frame_mat[task_index, env_index, :].cpu().numpy()
task.set_target(target)
def apply_actions(self):
# start_time = time.time() # Capture the time before the step
"""Apply the computed joint positions based on the inverse kinematics solution."""
all_envs_joint_pos_des = []
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)
# 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._asset.set_joint_position_target(all_envs_joint_pos_des, self._joint_ids)
def reset(self, env_ids: Sequence[int] | None = None) -> None:
"""Reset the action term for specified environments.
Args:
env_ids: A list of environment IDs to reset. If None, all environments are reset.
"""
self._raw_actions[env_ids] = torch.zeros(self.action_dim, device=self.device)
class DifferentialInverseKinematicsAction(ActionTerm):
r"""Inverse Kinematics action term.
......@@ -324,7 +511,7 @@ class OperationalSpaceControllerAction(ActionTerm):
if not self._task_frame_transformer.is_initialized:
self._task_frame_transformer._initialize_impl()
self._task_frame_transformer._is_initialized = True
# create tensor for task frame pose wrt the root frame
# create tensor for task frame pose in the root frame
self._task_frame_pose_b = torch.zeros(self.num_envs, 7, device=self.device)
else:
# create an empty reference for task frame pose
......
......@@ -97,7 +97,7 @@ def retrieve_file_path(path: str, download_dir: str | None = None, force_downloa
# check if file already exists locally
if not os.path.isfile(target_path) or force_download:
# copy file to local machine
result = omni.client.copy(path.replace(os.sep, "/"), target_path)
result = omni.client.copy(path.replace(os.sep, "/"), target_path, omni.client.CopyBehavior.OVERWRITE)
if result != omni.client.Result.OK and force_download:
raise RuntimeError(f"Unable to copy file: '{path}'. Is the Nucleus Server running?")
return os.path.abspath(target_path)
......
......@@ -1561,16 +1561,15 @@ def create_rotation_matrix_from_view(
return R.transpose(1, 2)
def make_pose(pos, rot):
"""
Make homogeneous pose matrices from a set of translation vectors and rotation matrices.
def make_pose(pos: torch.Tensor, rot: torch.Tensor) -> torch.Tensor:
"""Creates transformation matrices from positions and rotation matrices.
Args:
pos (torch.Tensor): batch of position vectors with last dimension of 3
rot (torch.Tensor): batch of rotation matrices with last 2 dimensions of (3, 3)
pos: Batch of position vectors with last dimension of 3.
rot: Batch of rotation matrices with last 2 dimensions of (3, 3).
Returns:
pose (torch.Tensor): batch of pose matrices with last 2 dimensions of (4, 4)
Batch of pose matrices with last 2 dimensions of (4, 4).
"""
assert isinstance(pos, torch.Tensor), "Input must be a torch tensor"
assert isinstance(rot, torch.Tensor), "Input must be a torch tensor"
......@@ -1583,33 +1582,31 @@ def make_pose(pos, rot):
return pose
def unmake_pose(pose):
"""
Split homogeneous pose matrices back into translation vectors and rotation matrices.
def unmake_pose(pose: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]:
"""Splits transformation matrices into positions and rotation matrices.
Args:
pose (torch.Tensor): batch of pose matrices with last 2 dimensions of (4, 4)
pose: Batch of pose matrices with last 2 dimensions of (4, 4).
Returns:
pos (torch.Tensor): batch of position vectors with last dimension of 3
rot (torch.Tensor): batch of rotation matrices with last 2 dimensions of (3, 3)
Tuple containing:
- Batch of position vectors with last dimension of 3.
- Batch of rotation matrices with last 2 dimensions of (3, 3).
"""
assert isinstance(pose, torch.Tensor), "Input must be a torch tensor"
return pose[..., :3, 3], pose[..., :3, :3]
def pose_inv(pose):
"""
Computes the inverse of homogeneous pose matrices.
def pose_inv(pose: torch.Tensor) -> torch.Tensor:
"""Computes the inverse of transformation matrices.
Note that the inverse of a pose matrix is the following:
[R t; 0 1]^-1 = [R.T -R.T*t; 0 1]
The inverse of a pose matrix [R t; 0 1] is [R.T -R.T*t; 0 1].
Args:
pose (torch.Tensor): batch of pose matrices with last 2 dimensions of (4, 4)
pose: Batch of pose matrices with last 2 dimensions of (4, 4).
Returns:
inv_pose (torch.Tensor): batch of inverse pose matrices with last 2 dimensions of (4, 4)
Batch of inverse pose matrices with last 2 dimensions of (4, 4).
"""
assert isinstance(pose, torch.Tensor), "Input must be a torch tensor"
num_axes = len(pose.shape)
......@@ -1617,7 +1614,7 @@ def pose_inv(pose):
inv_pose = torch.zeros_like(pose)
# take transpose of last 2 dimensions
# Take transpose of last 2 dimensions
inv_pose[..., :3, :3] = pose[..., :3, :3].transpose(-1, -2)
# note: PyTorch matmul wants shapes [..., 3, 3] x [..., 3, 1] -> [..., 3, 1] so we add a dimension and take it away after
......@@ -1626,35 +1623,40 @@ def pose_inv(pose):
return inv_pose
def pose_in_A_to_pose_in_B(pose_in_A, pose_A_in_B):
"""
Converts homogeneous matrices corresponding to a point C in frame A
to homogeneous matrices corresponding to the same point C in frame B.
def pose_in_A_to_pose_in_B(pose_in_A: torch.Tensor, pose_A_in_B: torch.Tensor) -> torch.Tensor:
"""Converts poses from one coordinate frame to another.
Transforms matrices representing point C in frame A
to matrices representing the same point C in frame B.
Example usage:
frame_C_in_B = pose_in_A_to_pose_in_B(frame_C_in_A, frame_A_in_B)
Args:
pose_in_A (torch.Tensor): batch of homogeneous matrices corresponding to the pose of C in frame A
pose_A_in_B (torch.Tensor): batch of homogeneous matrices corresponding to the pose of A in frame B
pose_in_A: Batch of transformation matrices of point C in frame A.
pose_A_in_B: Batch of transformation matrices of frame A in frame B.
Returns:
pose_in_B (torch.Tensor): batch of homogeneous matrices corresponding to the pose of C in frame B
Batch of transformation matrices of point C in frame B.
"""
assert isinstance(pose_in_A, torch.Tensor), "Input must be a torch tensor"
assert isinstance(pose_A_in_B, torch.Tensor), "Input must be a torch tensor"
return torch.matmul(pose_A_in_B, pose_in_A)
def quat_slerp(q1, q2, tau):
"""
Spherical linear interpolation (SLERP) between two quaternions.
This function does NOT support batch processing.
def quat_slerp(q1: torch.Tensor, q2: torch.Tensor, tau: float) -> torch.Tensor:
"""Performs spherical linear interpolation (SLERP) between two quaternions.
This function does not support batch processing.
Args:
q1 (torch.Tensor): The first quaternion (w, x, y, z) format.
q2 (torch.Tensor): The second quaternion (w, x, y, z) format.
tau (float): Interpolation coefficient between 0 (q1) and 1 (q2).
q1: First quaternion in (w, x, y, z) format.
q2: Second quaternion in (w, x, y, z) format.
tau: Interpolation coefficient between 0 (q1) and 1 (q2).
Returns:
torch.Tensor: The interpolated quaternion (w, x, y, z) format.
Interpolated quaternion in (w, x, y, z) format.
"""
assert isinstance(q1, torch.Tensor), "Input must be a torch tensor"
assert isinstance(q2, torch.Tensor), "Input must be a torch tensor"
......@@ -1666,7 +1668,7 @@ def quat_slerp(q1, q2, tau):
if abs(abs(d) - 1.0) < torch.finfo(q1.dtype).eps * 4.0:
return q1
if d < 0.0:
# invert rotation
# Invert rotation
d = -d
q2 *= -1.0
angle = torch.acos(torch.clamp(d, -1, 1))
......@@ -1679,24 +1681,24 @@ def quat_slerp(q1, q2, tau):
return q1
def interpolate_rotations(R1, R2, num_steps, axis_angle=True):
"""
Interpolate between two rotation matrices.
def interpolate_rotations(R1: torch.Tensor, R2: torch.Tensor, num_steps: int, axis_angle: bool = True) -> torch.Tensor:
"""Interpolates between two rotation matrices.
Args:
R1 (torch.Tensor): The first rotation matrix (4x4).
R2 (torch.Tensor): The second rotation matrix (4x4).
num_steps (int): The number of desired interpolated rotations (excluding start and end).
axis_angle (bool, optional): If True, interpolate in axis-angle representation. Else, use slerp. Defaults to True.
R1: First rotation matrix. (4x4).
R2: Second rotation matrix. (4x4).
num_steps: Number of desired interpolated rotations (excluding start and end).
axis_angle: If True, interpolate in axis-angle representation;
otherwise use slerp. Defaults to True.
Returns:
torch.Tensor: A stack of interpolated rotation matrices (shape: (num_steps + 1, 4, 4)),
Stack of interpolated rotation matrices of shape (num_steps + 1, 4, 4),
including the start and end rotations.
"""
assert isinstance(R1, torch.Tensor), "Input must be a torch tensor"
assert isinstance(R2, torch.Tensor), "Input must be a torch tensor"
if axis_angle:
# delta rotation expressed as axis-angle
# Delta rotation expressed as axis-angle
delta_rot_mat = torch.matmul(R2, R1.transpose(-1, -2))
delta_quat = quat_from_matrix(delta_rot_mat)
delta_axis_angle = axis_angle_from_quat(delta_quat)
......@@ -1704,15 +1706,15 @@ def interpolate_rotations(R1, R2, num_steps, axis_angle=True):
# Grab angle
delta_angle = torch.linalg.norm(delta_axis_angle)
# fix the axis, and chunk the angle up into steps
# Fix the axis, and chunk the angle up into steps
rot_step_size = delta_angle / num_steps
# convert into delta rotation matrices, and then convert to absolute rotations
# Convert into delta rotation matrices, and then convert to absolute rotations
if delta_angle < 0.05:
# small angle - don't bother with interpolation
# Small angle - don't bother with interpolation
rot_steps = torch.stack([R2 for _ in range(num_steps)])
else:
# make sure that axis is a unit vector
# Make sure that axis is a unit vector
delta_axis = delta_axis_angle / delta_angle
delta_rot_steps = [
matrix_from_quat(quat_from_angle_axis(i * rot_step_size, delta_axis)) for i in range(num_steps)
......@@ -1725,29 +1727,33 @@ def interpolate_rotations(R1, R2, num_steps, axis_angle=True):
[matrix_from_quat(quat_slerp(q1, q2, tau=float(i) / num_steps)) for i in range(num_steps)]
)
# add in endpoint
# Add in endpoint
rot_steps = torch.cat([rot_steps, R2[None]], dim=0)
return rot_steps
def interpolate_poses(pose_1, pose_2, num_steps=None, step_size=None, perturb=False):
"""
Linear interpolation between two poses.
def interpolate_poses(
pose_1: torch.Tensor,
pose_2: torch.Tensor,
num_steps: int = None,
step_size: float = None,
perturb: bool = False,
) -> tuple[torch.Tensor, int]:
"""Performs linear interpolation between two poses.
Args:
pose_1 (torch.tensor): 4x4 start pose
pose_2 (torch.tensor): 4x4 end pose
num_steps (int): if provided, specifies the number of desired interpolated points (not excluding
the start and end points). Passing 0 corresponds to no interpolation, and passing None
means that @step_size must be provided to determine the number of interpolated points.
step_size (float): if provided, will be used to infer the number of steps, by taking the norm
of the delta position vector, and dividing it by the step size
perturb (bool): if True, randomly move all the interpolated position points in a uniform, non-overlapping grid.
pose_1: 4x4 start pose.
pose_2: 4x4 end pose.
num_steps: If provided, specifies the number of desired interpolated points.
Passing 0 corresponds to no interpolation. If None, step_size must be provided.
step_size: If provided, determines number of steps based on distance between poses.
perturb: If True, randomly perturbs interpolated position points.
Returns:
pose_steps (torch.tensor): array of shape (N + 2, 3) corresponding to the interpolated pose path, where N is @num_steps
num_steps (int): the number of interpolated points (N) in the path
Tuple containing:
- Array of shape (N + 2, 4, 4) corresponding to the interpolated pose path.
- Number of interpolated points (N) in the path.
"""
assert isinstance(pose_1, torch.Tensor), "Input must be a torch tensor"
assert isinstance(pose_2, torch.Tensor), "Input must be a torch tensor"
......@@ -1757,7 +1763,7 @@ def interpolate_poses(pose_1, pose_2, num_steps=None, step_size=None, perturb=Fa
pos2, rot2 = unmake_pose(pose_2)
if num_steps == 0:
# skip interpolation
# Skip interpolation
return (
torch.cat([pos1[None], pos2[None]], dim=0),
torch.cat([rot1[None], rot2[None]], dim=0),
......@@ -1769,51 +1775,48 @@ def interpolate_poses(pose_1, pose_2, num_steps=None, step_size=None, perturb=Fa
assert torch.norm(delta_pos) > 0
num_steps = math.ceil(torch.norm(delta_pos) / step_size)
num_steps += 1 # include starting pose
num_steps += 1 # Include starting pose
assert num_steps >= 2
# linear interpolation of positions
# Linear interpolation of positions
pos_step_size = delta_pos / num_steps
grid = torch.arange(num_steps, dtype=torch.float32)
if perturb:
# move the interpolation grid points by up to a half-size forward or backward
# Move interpolation grid points by up to half-size forward or backward
perturbations = torch.rand(num_steps - 2) - 0.5
grid[1:-1] += perturbations
pos_steps = torch.stack([pos1 + grid[i] * pos_step_size for i in range(num_steps)])
# add in endpoint
# Add in endpoint
pos_steps = torch.cat([pos_steps, pos2[None]], dim=0)
# interpolate the rotations too
# Interpolate rotations
rot_steps = interpolate_rotations(R1=rot1, R2=rot2, num_steps=num_steps, axis_angle=True)
pose_steps = make_pose(pos_steps, rot_steps)
return pose_steps, num_steps - 1
def transform_poses_from_frame_A_to_frame_B(src_poses, frame_A, frame_B):
"""
Transform a source data segment (object-centric subtask segment from source demonstration) such that
the relative poses between the target eef pose frame and the object frame are preserved. Recall that
each object-centric subtask segment corresponds to one object, and consists of a sequence of
target eef poses.
def transform_poses_from_frame_A_to_frame_B(
src_poses: torch.Tensor, frame_A: torch.Tensor, frame_B: torch.Tensor
) -> torch.Tensor:
"""Transforms poses from one coordinate frame to another preserving relative poses.
Args:
src_poses (torch.tensor): Input pose sequence (shape [T, 4, 4]) from the source demonstration
frame_A (torch.tensor): 4x4 frame A pose
frame_B (torch.tensor): 4x4 frame B pose
src_poses: Input pose sequence (shape [T, 4, 4]) from source demonstration.
frame_A: 4x4 frame A pose.
frame_B: 4x4 frame B pose.
Returns:
transformed_eef_poses (torch.tensor): transformed pose sequence (shape [T, 4, 4])
Transformed pose sequence of shape [T, 4, 4].
"""
# transform source end effector poses to be relative to source object frame
# Transform source end effector poses to be relative to source object frame
src_poses_rel_frame_B = pose_in_A_to_pose_in_B(
pose_in_A=src_poses,
pose_A_in_B=pose_inv(frame_B[None]),
)
# apply relative poses to current object frame to obtain new target eef poses
# Apply relative poses to current object frame to obtain new target eef poses
transformed_poses = pose_in_A_to_pose_in_B(
pose_in_A=src_poses_rel_frame_B,
pose_A_in_B=frame_A[None],
......@@ -1821,15 +1824,14 @@ def transform_poses_from_frame_A_to_frame_B(src_poses, frame_A, frame_B):
return transformed_poses
def generate_random_rotation(rot_boundary=(2 * math.pi)):
"""
Generates a random rotation matrix using Euler angles.
def generate_random_rotation(rot_boundary: float = (2 * math.pi)) -> torch.Tensor:
"""Generates a random rotation matrix using Euler angles.
Args:
rot_boundary (float): The range for random rotation angles around each axis (x, y, z).
rot_boundary: Range for random rotation angles around each axis (x, y, z).
Returns:
torch.tensor: A 3x3 rotation matrix.
3x3 rotation matrix.
"""
angles = torch.rand(3) * rot_boundary
Rx = torch.tensor(
......@@ -1849,29 +1851,27 @@ def generate_random_rotation(rot_boundary=(2 * math.pi)):
return R
def generate_random_translation(pos_boundary=1):
"""
Generates a random translation vector.
def generate_random_translation(pos_boundary: float = 1) -> torch.Tensor:
"""Generates a random translation vector.
Args:
pos_boundary (float): The range for random translation values in 3D space.
pos_boundary: Range for random translation values in 3D space.
Returns:
torch.tensor: A 3-element translation vector.
3-element translation vector.
"""
return torch.rand(3) * 2 * pos_boundary - pos_boundary # Random translation in 3D space
def generate_random_transformation_matrix(pos_boundary=1, rot_boundary=(2 * math.pi)):
"""
Generates a random transformation matrix combining rotation and translation.
def generate_random_transformation_matrix(pos_boundary: float = 1, rot_boundary: float = (2 * math.pi)) -> torch.Tensor:
"""Generates a random transformation matrix combining rotation and translation.
Args:
pos_boundary (float): The range for random translation values.
rot_boundary (float): The range for random rotation angles.
pos_boundary: Range for random translation values.
rot_boundary: Range for random rotation angles.
Returns:
torch.tensor: A 4x4 transformation matrix.
4x4 transformation matrix.
"""
R = generate_random_rotation(rot_boundary)
translation = generate_random_translation(pos_boundary)
......
......@@ -38,6 +38,8 @@ INSTALL_REQUIRES = [
"pillow==11.0.0",
# livestream
"starlette==0.46.0",
"pin-pink==3.1.0", # required by isaaclab.isaaclab.controllers.pink_ik
"dex-retargeting==0.4.6", # required by isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1_t2_dex_retargeting_utils
]
PYTORCH_INDEX_URL = ["https://download.pytorch.org/whl/cu118"]
......
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Launch Isaac Sim Simulator first."""
# Import pinocchio in the main script to force the use of the dependencies installed by IsaacLab and not the one installed by Isaac Sim
# pinocchio is required by the Pink IK controller
import pinocchio # noqa: F401
from isaaclab.app import AppLauncher, run_tests
# launch omniverse app
simulation_app = AppLauncher(headless=True).app
"""Rest everything follows."""
import contextlib
import gymnasium as gym
import torch
import unittest
import isaaclab_tasks # noqa: F401
from isaaclab_tasks.utils.parse_cfg import parse_env_cfg
class TestPinkIKController(unittest.TestCase):
"""Test fixture for the Pink IK controller with the GR1T2 humanoid robot.
This test validates that the Pink IK controller can accurately track commanded
end-effector poses for a humanoid robot. It specifically:
1. Creates a GR1T2 humanoid robot with the Pink IK controller
2. Sends target pose commands to the left and right hand roll links
3. Checks that the observed poses of the links match the target poses within tolerance
4. Tests adaptability by moving the hands up and down multiple times
The test succeeds when the controller can accurately converge to each new target
position, demonstrating both accuracy and adaptability to changing targets.
"""
def setUp(self):
# End effector position mean square error tolerance
self.pose_tolerance = 1e-2
# Number of environments
self.num_envs = 1
# Number of joints in the 2 robot hands
self.num_joints_in_robot_hands = 22
# Number of steps to wait for controller convergence
self.num_steps_controller_convergence = 25
self.num_times_to_move_hands_up = 3
self.num_times_to_move_hands_down = 6
# Create starting setpoints with respect to the env origin frame
# These are the setpoints for the forward kinematics result of the
# InitialStateCfg specified in `PickPlaceGR1T2EnvCfg`
y_axis_z_axis_90_rot_quaternion = [0.5, 0.5, -0.5, 0.5]
left_hand_roll_link_pos = [-0.23, 0.28, 1.1]
self.left_hand_roll_link_pose = left_hand_roll_link_pos + y_axis_z_axis_90_rot_quaternion
right_hand_roll_link_pos = [0.16, 0.26, 1.13]
self.right_hand_roll_link_pose = right_hand_roll_link_pos + y_axis_z_axis_90_rot_quaternion
"""
Test fixtures.
"""
def test_gr1t2_ik_pose_abs(self):
"""Test IK controller for GR1T2 humanoid."""
env_name = "Isaac-PickPlace-GR1T2-Abs-v0"
device = "cuda:0"
env_cfg = parse_env_cfg(env_name, device=device, num_envs=self.num_envs)
# create environment from loaded config
env = gym.make(env_name, cfg=env_cfg).unwrapped
# reset before starting
obs, _ = env.reset()
num_runs = 0 # Counter for the number of runs
move_hands_up = True
test_counter = 0
# simulate environment -- run everything in inference mode
with contextlib.suppress(KeyboardInterrupt) and torch.inference_mode():
while simulation_app.is_running() and not simulation_app.is_exiting():
num_runs += 1
setpoint_poses = self.left_hand_roll_link_pose + self.right_hand_roll_link_pose
actions = setpoint_poses + [0.0] * self.num_joints_in_robot_hands
actions = torch.tensor(actions, device=device)
actions = torch.stack([actions for _ in range(env.num_envs)])
obs, _, _, _, _ = env.step(actions)
left_hand_roll_link_pose_obs = obs["policy"]["robot_links_state"][
:, env.scene["robot"].data.body_names.index("left_hand_roll_link"), :7
]
right_hand_roll_link_pose_obs = obs["policy"]["robot_links_state"][
:, env.scene["robot"].data.body_names.index("right_hand_roll_link"), :7
]
# The setpoints are wrt the env origin frame
# The observations are also wrt the env origin frame
left_hand_roll_link_feedback = left_hand_roll_link_pose_obs
left_hand_roll_link_setpoint = (
torch.tensor(self.left_hand_roll_link_pose, device=device).unsqueeze(0).repeat(env.num_envs, 1)
)
left_hand_roll_link_error = left_hand_roll_link_setpoint - left_hand_roll_link_feedback
right_hand_roll_link_feedback = right_hand_roll_link_pose_obs
right_hand_roll_link_setpoint = (
torch.tensor(self.right_hand_roll_link_pose, device=device).unsqueeze(0).repeat(env.num_envs, 1)
)
right_hand_roll_link_error = right_hand_roll_link_setpoint - right_hand_roll_link_feedback
if num_runs % self.num_steps_controller_convergence == 0:
torch.testing.assert_close(
torch.mean(torch.abs(left_hand_roll_link_error), dim=1),
torch.zeros(env.num_envs, device="cuda:0"),
rtol=0.0,
atol=self.pose_tolerance,
)
torch.testing.assert_close(
torch.mean(torch.abs(right_hand_roll_link_error), dim=1),
torch.zeros(env.num_envs, device="cuda:0"),
rtol=0.0,
atol=self.pose_tolerance,
)
# Change the setpoints to move the hands up and down as per the counter
test_counter += 1
if test_counter == self.num_times_to_move_hands_up:
move_hands_up = False
elif test_counter == self.num_times_to_move_hands_down:
move_hands_up = True
# Test is done after moving the hands up and then down
break
if move_hands_up:
self.left_hand_roll_link_pose[0] += 0.05
self.left_hand_roll_link_pose[2] += 0.05
self.right_hand_roll_link_pose[0] += 0.05
self.right_hand_roll_link_pose[2] += 0.05
else:
self.left_hand_roll_link_pose[0] -= 0.05
self.left_hand_roll_link_pose[2] -= 0.05
self.right_hand_roll_link_pose[0] -= 0.05
self.right_hand_roll_link_pose[2] -= 0.05
env.close()
if __name__ == "__main__":
run_tests()
[package]
# Semantic Versioning is used: https://semver.org/
version = "0.2.1"
version = "0.2.2"
# Description
title = "Isaac Lab Assets"
......
Changelog
---------
0.2.2 (2025-03-10)
~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added configuration for the Fourier GR1T2 robot.
0.2.1 (2025-01-14)
~~~~~~~~~~~~~~~~~~
......
......@@ -12,6 +12,7 @@ from .ant import *
from .anymal import *
from .cart_double_pendulum import *
from .cartpole import *
from .fourier import *
from .franka import *
from .humanoid import *
from .humanoid_28 import *
......
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Configuration for the Fourier Robots.
The following configuration parameters are available:
* :obj:`GR1T2_CFG`: The GR1T2 humanoid.
Reference: https://www.fftai.com/products-gr1
"""
import torch
import isaaclab.sim as sim_utils
from isaaclab.actuators import ImplicitActuatorCfg
from isaaclab.assets import ArticulationCfg
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
##
# Configuration
##
GR1T2_CFG = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(
usd_path=(
f"{ISAAC_NUCLEUS_DIR}/Robots/FourierIntelligence/GR-1/GR1T2_fourier_hand_6dof/GR1T2_fourier_hand_6dof.usd"
),
activate_contact_sensors=True,
rigid_props=sim_utils.RigidBodyPropertiesCfg(
disable_gravity=True,
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=True, solver_position_iteration_count=8, solver_velocity_iteration_count=4
),
),
init_state=ArticulationCfg.InitialStateCfg(
pos=(0.0, 0.0, 0.95),
joint_pos={".*": 0.0},
joint_vel={".*": 0.0},
),
actuators={
"head": ImplicitActuatorCfg(
joint_names_expr=[
"head_.*",
],
effort_limit=None,
velocity_limit=None,
stiffness=None,
damping=None,
),
"trunk": ImplicitActuatorCfg(
joint_names_expr=[
"waist_.*",
],
effort_limit=None,
velocity_limit=None,
stiffness=None,
damping=None,
),
"legs": ImplicitActuatorCfg(
joint_names_expr=[
".*_hip_.*",
".*_knee_.*",
".*_ankle_.*",
],
effort_limit=None,
velocity_limit=None,
stiffness=None,
damping=None,
),
"right-arm": ImplicitActuatorCfg(
joint_names_expr=[
"right_shoulder_.*",
"right_elbow_.*",
"right_wrist_.*",
],
effort_limit=torch.inf,
velocity_limit=torch.inf,
stiffness=None,
damping=None,
armature=0.0,
),
"left-arm": ImplicitActuatorCfg(
joint_names_expr=[
"left_shoulder_.*",
"left_elbow_.*",
"left_wrist_.*",
],
effort_limit=torch.inf,
velocity_limit=torch.inf,
stiffness=None,
damping=None,
armature=0.0,
),
"right-hand": ImplicitActuatorCfg(
joint_names_expr=[
"R_.*",
],
effort_limit=None,
velocity_limit=None,
stiffness=None,
damping=None,
),
"left-hand": ImplicitActuatorCfg(
joint_names_expr=[
"L_.*",
],
effort_limit=None,
velocity_limit=None,
stiffness=None,
damping=None,
),
},
)
"""Configuration for the GR1T2 Humanoid robot."""
[package]
# Semantic Versioning is used: https://semver.org/
version = "1.0.5"
version = "1.0.6"
# Description
category = "isaaclab"
......
Changelog
---------
1.0.6 (2025-03-10)
~~~~~~~~~~~~~~~~~~
Added
^^^^^^^
* Added :class:`FrankaCubeStackIKAbsMimicEnv` and support for the GR1T2 robot task (:class:`PickPlaceGR1T2MimicEnv`).
1.0.5 (2025-03-10)
~~~~~~~~~~~~~~~~~~
......
......@@ -12,6 +12,9 @@ from .franka_stack_ik_abs_mimic_env_cfg import FrankaCubeStackIKAbsMimicEnvCfg
from .franka_stack_ik_rel_blueprint_mimic_env_cfg import FrankaCubeStackIKRelBlueprintMimicEnvCfg
from .franka_stack_ik_rel_mimic_env import FrankaCubeStackIKRelMimicEnv
from .franka_stack_ik_rel_mimic_env_cfg import FrankaCubeStackIKRelMimicEnvCfg
from .franka_stack_ik_rel_visuomotor_mimic_env_cfg import FrankaCubeStackIKRelVisuomotorMimicEnvCfg
from .pickplace_gr1t2_mimic_env import PickPlaceGR1T2MimicEnv
from .pickplace_gr1t2_mimic_env_cfg import PickPlaceGR1T2MimicEnvCfg
##
# Inverse Kinematics - Relative Pose Control
......@@ -43,3 +46,21 @@ gym.register(
},
disable_env_checker=True,
)
gym.register(
id="Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-Mimic-v0",
entry_point="isaaclab_mimic.envs:FrankaCubeStackIKRelMimicEnv",
kwargs={
"env_cfg_entry_point": franka_stack_ik_rel_visuomotor_mimic_env_cfg.FrankaCubeStackIKRelVisuomotorMimicEnvCfg,
},
disable_env_checker=True,
)
gym.register(
id="Isaac-PickPlace-GR1T2-Abs-Mimic-v0",
entry_point="isaaclab_mimic.envs:PickPlaceGR1T2MimicEnv",
kwargs={
"env_cfg_entry_point": pickplace_gr1t2_mimic_env_cfg.PickPlaceGR1T2MimicEnvCfg,
},
disable_env_checker=True,
)
......@@ -36,7 +36,11 @@ class FrankaCubeStackIKRelMimicEnv(ManagerBasedRLMimicEnv):
return PoseUtils.make_pose(eef_pos, PoseUtils.matrix_from_quat(eef_quat))
def target_eef_pose_to_action(
self, target_eef_pose_dict: dict, gripper_action_dict: dict, noise: float | None = None, env_id: int = 0
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
......@@ -75,8 +79,8 @@ class FrankaCubeStackIKRelMimicEnv(ManagerBasedRLMimicEnv):
# add noise to action
pose_action = torch.cat([delta_position, delta_rotation], dim=0)
if noise is not None:
noise = noise * torch.randn_like(pose_action)
if action_noise_dict is not None:
noise = action_noise_dict["franka"] * torch.randn_like(pose_action)
pose_action += noise
pose_action = torch.clamp(pose_action, -1.0, 1.0)
......
......@@ -30,6 +30,7 @@ class FrankaCubeStackIKRelMimicEnvCfg(FrankaCubeStackEnvCfg, MimicEnvCfg):
self.datagen_config.generation_select_src_per_subtask = True
self.datagen_config.generation_transform_first_robot_pose = False
self.datagen_config.generation_interpolate_from_last_target_pose = True
self.datagen_config.generation_relative = True
self.datagen_config.max_num_failures = 25
self.datagen_config.seed = 1
......
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: Apache-2.0
from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig
from isaaclab.utils import configclass
from isaaclab_tasks.manager_based.manipulation.stack.config.franka.stack_ik_rel_visuomotor_env_cfg import (
FrankaCubeStackVisuomotorEnvCfg,
)
@configclass
class FrankaCubeStackIKRelVisuomotorMimicEnvCfg(FrankaCubeStackVisuomotorEnvCfg, MimicEnvCfg):
"""
Isaac Lab Mimic environment config class for Franka Cube Stack IK Rel Visuomotor env.
"""
def __post_init__(self):
# post init of parents
super().__post_init__()
# Override the existing values
self.datagen_config.name = "isaac_lab_franka_stack_ik_rel_visuomotor_D0"
self.datagen_config.generation_guarantee = True
self.datagen_config.generation_keep_failed = True
self.datagen_config.generation_num_trials = 10
self.datagen_config.generation_select_src_per_subtask = True
self.datagen_config.generation_transform_first_robot_pose = False
self.datagen_config.generation_interpolate_from_last_target_pose = True
self.datagen_config.generation_relative = True
self.datagen_config.max_num_failures = 25
self.datagen_config.seed = 1
# The following are the subtask configurations for the stack task.
subtask_configs = []
subtask_configs.append(
SubTaskConfig(
# Each subtask involves manipulation with respect to a single object frame.
object_ref="cube_2",
# 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="grasp_1",
# Specifies time offsets for data generation when splitting a trajectory into
# subtask segments. Random offsets are added to the termination boundary.
subtask_term_offset_range=(10, 20),
# 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.03,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=5,
# 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="cube_1",
# Corresponding key for the binary indicator in "datagen_info" for completion
subtask_term_signal="stack_1",
# Time offsets for data generation when splitting a trajectory
subtask_term_offset_range=(10, 20),
# 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.03,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=5,
# 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="cube_3",
# Corresponding key for the binary indicator in "datagen_info" for completion
subtask_term_signal="grasp_2",
# Time offsets for data generation when splitting a trajectory
subtask_term_offset_range=(10, 20),
# 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.03,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=5,
# 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="cube_2",
# End of final subtask does not need to be detected
subtask_term_signal=None,
# No time offsets for the final subtask
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.03,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=5,
# 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["franka"] = subtask_configs
# Copyright (c) 2025, The Isaac Lab Project Developers.
# 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 PickPlaceGR1T2MimicEnv(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.
noise: 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:25], "right": actions[:, 25:]}
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: Apache-2.0
from isaaclab.envs.mimic_env_cfg import MimicEnvCfg, SubTaskConfig
from isaaclab.utils import configclass
from isaaclab_tasks.manager_based.manipulation.pick_place.pickplace_gr1t2_env_cfg import PickPlaceGR1T2EnvCfg
@configclass
class PickPlaceGR1T2MimicEnvCfg(PickPlaceGR1T2EnvCfg, MimicEnvCfg):
def __post_init__(self):
# Calling post init of parents
super().__post_init__()
# Override the existing values
self.datagen_config.name = "demo_src_gr1t2_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
# The following are the subtask configurations for the stack task.
subtask_configs = []
subtask_configs.append(
SubTaskConfig(
# Each subtask involves manipulation with respect to a single object frame.
object_ref="object",
# This key corresponds to the binary indicator in "datagen_info" that signals
# when this subtask is finished (e.g., on a 0 to 1 edge).
subtask_term_signal="idle_right",
first_subtask_start_offset_range=(0, 0),
# Randomization range for starting index of the first subtask
subtask_term_offset_range=(0, 0),
# Selection strategy for the source subtask segment during data generation
# selection_strategy="nearest_neighbor_object",
selection_strategy="nearest_neighbor_object",
# Optional parameters for the selection strategy function
selection_strategy_kwargs={"nn_k": 3},
# Amount of action noise to apply during this subtask
action_noise=0.005,
# 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.005,
# Number of interpolation steps to bridge to this subtask segment
num_interpolation_steps=3,
# Additional fixed steps for the robot to reach the necessary pose
num_fixed_steps=0,
# If True, apply action noise during the interpolation phase and execution
apply_noise_during_interpolation=False,
)
)
self.subtask_configs["right"] = subtask_configs
subtask_configs = []
subtask_configs.append(
SubTaskConfig(
# Each subtask involves manipulation with respect to a single object frame.
object_ref="object",
# Corresponding key for the binary indicator in "datagen_info" for completion
subtask_term_signal=None,
# Time offsets for data generation when splitting a trajectory
subtask_term_offset_range=(0, 0),
# Selection strategy for source subtask segment
selection_strategy="nearest_neighbor_object",
# Optional parameters for the selection strategy function
selection_strategy_kwargs={"nn_k": 3},
# Amount of action noise to apply during this subtask
action_noise=0.005,
# 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
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.10.27"
version = "0.10.28"
# Description
title = "Isaac Lab Environments"
......
Changelog
---------
0.10.27 (2025-03-25)
0.10.28 (2025-03-25)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -10,7 +10,7 @@ Fixed
* Fixed environment test failure for ``Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0``.
0.10.26 (2025-03-18)
0.10.27 (2025-03-18)
~~~~~~~~~~~~~~~~~~~~
Added
......@@ -19,12 +19,21 @@ Added
* Added Gymnasium spaces showcase tasks (``Isaac-Cartpole-Showcase-*-Direct-v0``, and ``Isaac-Cartpole-Camera-Showcase-*-Direct-v0``).
0.10.25 (2025-03-10)
0.10.26 (2025-03-10)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added the ``Isaac-PickPlace-GR1T2-Abs-v0`` environment that implements a humanoid arm picking and placing a steering wheel task using the PinkIKController.
0.10.25 (2025-03-06)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^^^
* Added ``Isaac-Stack-Cube-Franka-IK-Rel-Blueprint-v0`` stacking environment with camera inputs.
......
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import gymnasium as gym
import os
from . import agents, pickplace_gr1t2_env_cfg
gym.register(
id="Isaac-PickPlace-GR1T2-Abs-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
kwargs={
"env_cfg_entry_point": pickplace_gr1t2_env_cfg.PickPlaceGR1T2EnvCfg,
"robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_low_dim.json"),
},
disable_env_checker=True,
)
{
"algo_name": "bc",
"experiment": {
"name": "bc_rnn_low_dim_gr1t2",
"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": []
}
}
}
}
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""This sub-module contains the functions that are specific to the lift environments."""
from isaaclab.envs.mdp import * # noqa: F401, F403
from .observations import * # noqa: F401, F403
from .terminations import * # noqa: F401, F403
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from __future__ import annotations
import torch
from typing import TYPE_CHECKING
if TYPE_CHECKING:
from isaaclab.envs import ManagerBasedRLEnv
def object_obs(
env: ManagerBasedRLEnv,
) -> torch.Tensor:
"""
Object observations (in world frame):
object pos,
object quat,
left_eef to object,
right_eef_to object,
"""
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_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins
right_eef_pos = body_pos_w[:, right_eef_idx] - env.scene.env_origins
object_pos = env.scene["object"].data.root_pos_w - env.scene.env_origins
object_quat = env.scene["object"].data.root_quat_w
left_eef_to_object = object_pos - left_eef_pos
right_eef_to_object = object_pos - right_eef_pos
return torch.cat(
(
object_pos,
object_quat,
left_eef_to_object,
right_eef_to_object,
),
dim=1,
)
def get_left_eef_pos(
env: ManagerBasedRLEnv,
) -> 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_pos = body_pos_w[:, left_eef_idx] - env.scene.env_origins
return left_eef_pos
def get_left_eef_quat(
env: ManagerBasedRLEnv,
) -> 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_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(
env: ManagerBasedRLEnv,
) -> 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]
return head_joint_states
def get_all_robot_link_state(
env: ManagerBasedRLEnv,
) -> torch.Tensor:
body_pos_w = env.scene["robot"].data.body_link_state_w[:, :, :]
all_robot_link_pos = body_pos_w
return all_robot_link_pos
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Common functions that can be used to activate certain terminations for the lift task.
The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable
the termination introduced by the function.
"""
from __future__ import annotations
import torch
from typing import TYPE_CHECKING
from isaaclab.assets import RigidObject
from isaaclab.managers import SceneEntityCfg
if TYPE_CHECKING:
from isaaclab.envs import ManagerBasedRLEnv
def task_done(
env: ManagerBasedRLEnv,
object_cfg: SceneEntityCfg = SceneEntityCfg("object"),
right_wrist_max_x: float = 0.23,
min_x: float = 0.40,
max_x: float = 1.1,
min_y: float = 0.3,
max_y: float = 0.6,
min_height: float = 1.13,
min_vel: float = 0.08,
) -> torch.Tensor:
"""Determine if the object placement task is complete.
This function checks whether all success conditions for the task have been met:
1. object is within the target x/y range
2. object is below a minimum height
3. object velocity is below threshold
4. Right robot wrist is retracted back towards body (past a given x pos threshold)
Args:
env: The RL environment instance.
object_cfg: Configuration for the object entity.
right_wrist_max_x: Maximum x position of the right wrist for task completion.
min_x: Minimum x position of the object for task completion.
max_x: Maximum x position of the object for task completion.
min_y: Minimum y position of the object for task completion.
max_y: Maximum y position of the object for task completion.
min_height: Minimum height (z position) of the object for task completion.
min_vel: Minimum velocity magnitude of the object for task completion.
Returns:
Boolean tensor indicating which environments have completed the task.
"""
# Get object entity from the scene
object: RigidObject = env.scene[object_cfg.name]
# Extract wheel position relative to environment origin
wheel_x = object.data.root_pos_w[:, 0] - env.scene.env_origins[:, 0]
wheel_y = object.data.root_pos_w[:, 1] - env.scene.env_origins[:, 1]
wheel_height = object.data.root_pos_w[:, 2] - env.scene.env_origins[:, 2]
wheel_vel = torch.abs(object.data.root_vel_w)
# 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_wrist_x = robot_body_pos_w[:, right_eef_idx, 0] - env.scene.env_origins[:, 0]
# Check all success conditions and combine with logical AND
done = wheel_x < max_x
done = torch.logical_and(done, wheel_x > min_x)
done = torch.logical_and(done, wheel_y < max_y)
done = torch.logical_and(done, wheel_y > min_y)
done = torch.logical_and(done, wheel_height < min_height)
done = torch.logical_and(done, right_wrist_x < right_wrist_max_x)
done = torch.logical_and(done, wheel_vel[:, 0] < min_vel)
done = torch.logical_and(done, wheel_vel[:, 1] < min_vel)
done = torch.logical_and(done, wheel_vel[:, 2] < min_vel)
return done
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import tempfile
from pink.tasks import FrameTask
import isaaclab.controllers.utils as ControllerUtils
import isaaclab.envs.mdp as base_mdp
import isaaclab.sim as sim_utils
from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
from isaaclab.controllers import PinkIKControllerCfg
from isaaclab.devices.openxr import XrCfg
from isaaclab.envs import ManagerBasedRLEnvCfg
from isaaclab.envs.mdp.actions.actions_cfg import PinkInverseKinematicsActionCfg
from isaaclab.managers import EventTermCfg as EventTerm
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
from . import mdp
from isaaclab_assets.robots.fourier import GR1T2_CFG # isort: skip
##
# Scene definition
##
@configclass
class ObjectTableSceneCfg(InteractiveSceneCfg):
# Table
packing_table = AssetBaseCfg(
prim_path="/World/envs/env_.*/PackingTable",
init_state=AssetBaseCfg.InitialStateCfg(pos=[0.0, 0.55, 0.0], 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
object = RigidObjectCfg(
prim_path="{ENV_REGEX_NS}/Object",
init_state=RigidObjectCfg.InitialStateCfg(pos=[-0.35, 0.40, 1.0413], rot=[1, 0, 0, 0]),
spawn=sim_utils.CylinderCfg(
radius=0.018,
height=0.35,
rigid_props=sim_utils.RigidBodyPropertiesCfg(),
mass_props=sim_utils.MassPropertiesCfg(mass=0.3),
collision_props=sim_utils.CollisionPropertiesCfg(),
visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.15, 0.15, 0.15), metallic=1.0),
physics_material=sim_utils.RigidBodyMaterialCfg(
friction_combine_mode="max",
restitution_combine_mode="min",
static_friction=0.9,
dynamic_friction=0.9,
restitution=0.0,
),
),
)
# Humanoid robot w/ arms higher
robot: ArticulationCfg = GR1T2_CFG.replace(
prim_path="/World/envs/env_.*/Robot",
init_state=ArticulationCfg.InitialStateCfg(
pos=(0, 0, 0.93),
rot=(0.7071, 0, 0, 0.7071),
joint_pos={
# right-arm
"right_shoulder_pitch_joint": 0.0,
"right_shoulder_roll_joint": 0.0,
"right_shoulder_yaw_joint": 0.0,
"right_elbow_pitch_joint": -1.5708,
"right_wrist_yaw_joint": 0.0,
"right_wrist_roll_joint": 0.0,
"right_wrist_pitch_joint": 0.0,
# left-arm
"left_shoulder_pitch_joint": 0.0,
"left_shoulder_roll_joint": 0.0,
"left_shoulder_yaw_joint": 0.0,
"left_elbow_pitch_joint": -1.5708,
"left_wrist_yaw_joint": 0.0,
"left_wrist_roll_joint": 0.0,
"left_wrist_pitch_joint": 0.0,
# --
"head_.*": 0.0,
"waist_.*": 0.0,
".*_hip_.*": 0.0,
".*_knee_.*": 0.0,
".*_ankle_.*": 0.0,
"R_.*": 0.0,
"L_.*": 0.0,
},
joint_vel={".*": 0.0},
),
)
# 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),
)
##
# MDP settings
##
@configclass
class ActionsCfg:
"""Action specifications for the MDP."""
pink_ik_cfg = PinkInverseKinematicsActionCfg(
pink_controlled_joint_names=[
"left_shoulder_pitch_joint",
"left_shoulder_roll_joint",
"left_shoulder_yaw_joint",
"left_elbow_pitch_joint",
"left_wrist_yaw_joint",
"left_wrist_roll_joint",
"left_wrist_pitch_joint",
"right_shoulder_pitch_joint",
"right_shoulder_roll_joint",
"right_shoulder_yaw_joint",
"right_elbow_pitch_joint",
"right_wrist_yaw_joint",
"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",
"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",
],
# 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=PinkIKControllerCfg(
articulation_name="robot",
base_link_name="base_link",
num_hand_joints=22,
variable_input_tasks=[
FrameTask(
"GR1T2_fourier_hand_6dof_left_hand_roll_link",
position_cost=1.0, # [cost] / [m]
orientation_cost=1.0, # [cost] / [rad]
lm_damping=10, # dampening for solver for step jumps
gain=0.1,
),
FrameTask(
"GR1T2_fourier_hand_6dof_right_hand_roll_link",
position_cost=1.0, # [cost] / [m]
orientation_cost=1.0, # [cost] / [rad]
lm_damping=10, # dampening for solver for step jumps
gain=0.1,
),
],
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]
# ),
],
),
)
@configclass
class ObservationsCfg:
"""Observation specifications for the MDP."""
@configclass
class PolicyCfg(ObsGroup):
"""Observations for policy group with state values."""
actions = ObsTerm(func=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=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)
hand_joint_state = ObsTerm(func=mdp.get_hand_state)
head_joint_state = ObsTerm(func=mdp.get_head_state)
object = ObsTerm(func=mdp.object_obs)
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=mdp.time_out, time_out=True)
success = DoneTerm(func=mdp.task_done)
@configclass
class EventCfg:
"""Configuration for events."""
reset_all = EventTerm(func=mdp.reset_scene_to_default, mode="reset")
reset_object = EventTerm(
func=mdp.reset_root_state_uniform,
mode="reset",
params={
"pose_range": {
"x": [-0.05, 0.0],
"y": [0.0, 0.05],
},
"velocity_range": {},
"asset_cfg": SceneEntityCfg("object", body_names=".*"),
},
)
@configclass
class PickPlaceGR1T2EnvCfg(ManagerBasedRLEnvCfg):
"""Configuration for the GR1T2 environment."""
# Scene settings
scene: ObjectTableSceneCfg = ObjectTableSceneCfg(num_envs=1, env_spacing=2.5, replicate_physics=True)
# Basic settings
observations: ObservationsCfg = ObservationsCfg()
actions: ActionsCfg = ActionsCfg()
# MDP settings
terminations: TerminationsCfg = TerminationsCfg()
events = EventCfg()
# Unused managers
commands = None
rewards = None
curriculum = None
# Position of the XR anchor in the world frame
xr: XrCfg = XrCfg(
anchor_pos=(0.0, 0.0, 0.0),
anchor_rot=(1.0, 0.0, 0.0, 0.0),
)
# Temporary directory for URDF files
temp_urdf_dir = tempfile.gettempdir()
def __post_init__(self):
"""Post initialization."""
# general settings
self.decimation = 5
self.episode_length_s = 20.0
# simulation settings
self.sim.dt = 1 / 60 # 100Hz
self.sim.render_interval = 2
# 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
......@@ -11,6 +11,7 @@ from . import (
stack_ik_rel_blueprint_env_cfg,
stack_ik_rel_env_cfg,
stack_ik_rel_instance_randomize_env_cfg,
stack_ik_rel_visuomotor_env_cfg,
stack_joint_pos_env_cfg,
stack_joint_pos_instance_randomize_env_cfg,
)
......@@ -56,6 +57,16 @@ gym.register(
disable_env_checker=True,
)
gym.register(
id="Isaac-Stack-Cube-Franka-IK-Rel-Visuomotor-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
kwargs={
"env_cfg_entry_point": stack_ik_rel_visuomotor_env_cfg.FrankaCubeStackVisuomotorEnvCfg,
"robomimic_bc_cfg_entry_point": os.path.join(agents.__path__[0], "robomimic/bc_rnn_image_84.json"),
},
disable_env_checker=True,
)
gym.register(
id="Isaac-Stack-Cube-Franka-IK-Abs-v0",
entry_point="isaaclab.envs:ManagerBasedRLEnv",
......
{
"algo_name": "bc",
"experiment": {
"name": "bc_rnn_image_franka_stack",
"validate": false,
"logging": {
"terminal_output_to_txt": true,
"log_tb": true
},
"save": {
"enabled": true,
"every_n_seconds": null,
"every_n_epochs": 20,
"epochs": [],
"on_best_validation": false,
"on_best_rollout_return": false,
"on_best_rollout_success_rate": true
},
"epoch_every_n_steps": 500,
"env": null,
"additional_envs": null,
"render": false,
"render_video": false,
"rollout": {
"enabled": false
}
},
"train": {
"data": null,
"num_data_workers": 4,
"hdf5_cache_mode": "low_dim",
"hdf5_use_swmr": true,
"hdf5_load_next_obs": false,
"hdf5_normalize_obs": false,
"hdf5_filter_key": null,
"hdf5_validation_filter_key": null,
"seq_length": 10,
"pad_seq_length": true,
"frame_stack": 1,
"pad_frame_stack": true,
"dataset_keys": [
"actions",
"rewards",
"dones"
],
"goal_mode": null,
"cuda": true,
"batch_size": 16,
"num_epochs": 600,
"seed": 101
},
"algo": {
"optim_params": {
"policy": {
"optimizer_type": "adam",
"learning_rate": {
"initial": 0.0001,
"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": [],
"gaussian": {
"enabled": false,
"fixed_std": false,
"init_std": 0.1,
"min_std": 0.01,
"std_activation": "softplus",
"low_noise_eval": true
},
"gmm": {
"enabled": true,
"num_modes": 5,
"min_std": 0.0001,
"std_activation": "softplus",
"low_noise_eval": true
},
"vae": {
"enabled": false,
"latent_dim": 14,
"latent_clip": null,
"kl_weight": 1.0,
"decoder": {
"is_conditioned": true,
"reconstruction_sum_across_elements": false
},
"prior": {
"learn": false,
"is_conditioned": false,
"use_gmm": false,
"gmm_num_modes": 10,
"gmm_learn_weights": false,
"use_categorical": false,
"categorical_dim": 10,
"categorical_gumbel_softmax_hard": false,
"categorical_init_temp": 1.0,
"categorical_temp_anneal_step": 0.001,
"categorical_min_temp": 0.3
},
"encoder_layer_dims": [
300,
400
],
"decoder_layer_dims": [
300,
400
],
"prior_layer_dims": [
300,
400
]
},
"rnn": {
"enabled": true,
"horizon": 10,
"hidden_dim": 1000,
"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": [
"eef_pos",
"eef_quat",
"gripper_pos"
],
"rgb": [
"table_cam",
"wrist_cam"
],
"depth": [],
"scan": []
},
"goal": {
"low_dim": [],
"rgb": [],
"depth": [],
"scan": []
}
},
"encoder": {
"low_dim": {
"core_class": null,
"core_kwargs": {},
"obs_randomizer_class": null,
"obs_randomizer_kwargs": {}
},
"rgb": {
"core_class": "VisualCore",
"core_kwargs": {
"feature_dimension": 64,
"flatten": true,
"backbone_class": "ResNet18Conv",
"backbone_kwargs": {
"pretrained": false,
"input_coord_conv": false
},
"pool_class": "SpatialSoftmax",
"pool_kwargs": {
"num_kp": 32,
"learnable_temperature": false,
"temperature": 1.0,
"noise_std": 0.0,
"output_variance": false
}
},
"obs_randomizer_class": "CropRandomizer",
"obs_randomizer_kwargs": {
"crop_height": 76,
"crop_width": 76,
"num_crops": 1,
"pos_enc": false
}
},
"depth": {
"core_class": "VisualCore",
"core_kwargs": {},
"obs_randomizer_class": null,
"obs_randomizer_kwargs": {}
},
"scan": {
"core_class": "ScanCore",
"core_kwargs": {},
"obs_randomizer_class": null,
"obs_randomizer_kwargs": {}
}
}
}
}
{
"algo_name": "bc",
"experiment": {
"name": "bc",
"name": "bc_rnn_low_dim_franka_stack",
"validate": false,
"logging": {
"terminal_output_to_txt": true,
......
# Copyright (c) 2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import isaaclab.sim as sim_utils
from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg
from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
from isaaclab.managers import ObservationGroupCfg as ObsGroup
from isaaclab.managers import ObservationTermCfg as ObsTerm
from isaaclab.managers import SceneEntityCfg
from isaaclab.sensors import CameraCfg
from isaaclab.utils import configclass
from ... import mdp
from . import stack_joint_pos_env_cfg
##
# Pre-defined configs
##
from isaaclab_assets.robots.franka import FRANKA_PANDA_HIGH_PD_CFG # isort: skip
@configclass
class ObservationsCfg:
"""Observation specifications for the MDP."""
@configclass
class PolicyCfg(ObsGroup):
"""Observations for policy group with state values."""
actions = ObsTerm(func=mdp.last_action)
joint_pos = ObsTerm(func=mdp.joint_pos_rel)
joint_vel = ObsTerm(func=mdp.joint_vel_rel)
object = ObsTerm(func=mdp.object_obs)
cube_positions = ObsTerm(func=mdp.cube_positions_in_world_frame)
cube_orientations = ObsTerm(func=mdp.cube_orientations_in_world_frame)
eef_pos = ObsTerm(func=mdp.ee_frame_pos)
eef_quat = ObsTerm(func=mdp.ee_frame_quat)
gripper_pos = ObsTerm(func=mdp.gripper_pos)
table_cam = ObsTerm(
func=mdp.image, params={"sensor_cfg": SceneEntityCfg("table_cam"), "data_type": "rgb", "normalize": False}
)
wrist_cam = ObsTerm(
func=mdp.image, params={"sensor_cfg": SceneEntityCfg("wrist_cam"), "data_type": "rgb", "normalize": False}
)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
@configclass
class SubtaskCfg(ObsGroup):
"""Observations for subtask group."""
grasp_1 = ObsTerm(
func=mdp.object_grasped,
params={
"robot_cfg": SceneEntityCfg("robot"),
"ee_frame_cfg": SceneEntityCfg("ee_frame"),
"object_cfg": SceneEntityCfg("cube_2"),
},
)
stack_1 = ObsTerm(
func=mdp.object_stacked,
params={
"robot_cfg": SceneEntityCfg("robot"),
"upper_object_cfg": SceneEntityCfg("cube_2"),
"lower_object_cfg": SceneEntityCfg("cube_1"),
},
)
grasp_2 = ObsTerm(
func=mdp.object_grasped,
params={
"robot_cfg": SceneEntityCfg("robot"),
"ee_frame_cfg": SceneEntityCfg("ee_frame"),
"object_cfg": SceneEntityCfg("cube_3"),
},
)
def __post_init__(self):
self.enable_corruption = False
self.concatenate_terms = False
# observation groups
policy: PolicyCfg = PolicyCfg()
subtask_terms: SubtaskCfg = SubtaskCfg()
@configclass
class FrankaCubeStackVisuomotorEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg):
observations: ObservationsCfg = ObservationsCfg()
def __post_init__(self):
# post init of parent
super().__post_init__()
# Set Franka as robot
# We switch here to a stiffer PD controller for IK tracking to be better.
self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
# Set actions for the specific robot type (franka)
self.actions.arm_action = DifferentialInverseKinematicsActionCfg(
asset_name="robot",
joint_names=["panda_joint.*"],
body_name="panda_hand",
controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"),
scale=0.5,
body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]),
)
# Set cameras
# Set wrist camera
self.scene.wrist_cam = CameraCfg(
prim_path="{ENV_REGEX_NS}/Robot/panda_hand/wrist_cam",
update_period=0.0,
height=84,
width=84,
data_types=["rgb", "distance_to_image_plane"],
spawn=sim_utils.PinholeCameraCfg(
focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 2)
),
offset=CameraCfg.OffsetCfg(
pos=(0.13, 0.0, -0.15), rot=(-0.70614, 0.03701, 0.03701, -0.70614), convention="ros"
),
)
# Set table view camera
self.scene.table_cam = CameraCfg(
prim_path="{ENV_REGEX_NS}/table_cam",
update_period=0.0,
height=84,
width=84,
data_types=["rgb", "distance_to_image_plane"],
spawn=sim_utils.PinholeCameraCfg(
focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 2)
),
offset=CameraCfg.OffsetCfg(
pos=(1.0, 0.0, 0.4), rot=(0.35355, -0.61237, -0.61237, 0.35355), convention="ros"
),
)
# Set settings for camera rendering
self.rerender_on_reset = True
self.sim.render.antialiasing_mode = "OFF" # disable dlss
# List of image observations in policy observations
self.image_obs_list = ["table_cam", "wrist_cam"]
......@@ -5,6 +5,13 @@
"""Launch Isaac Sim Simulator first."""
# Omniverse logger
import omni.log
# 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 pinocchio # noqa: F401
from isaaclab.app import AppLauncher, run_tests
# launch the simulator
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment