diff --git a/examples/isaaclab_bimanual_robot/Readme.md b/examples/isaaclab_bimanual_robot/Readme.md new file mode 100644 index 000000000..a359f937e --- /dev/null +++ b/examples/isaaclab_bimanual_robot/Readme.md @@ -0,0 +1,145 @@ + + +# Bimanual Robot Example using IsaacLab + +This directory contains minimal usage examples for teleoperating bimanual robot arms in IsaacLab. + +##UR bimanual +![ur bimanual](./images/bimanual_ur.png) + + +## Prerequisites + + +### 1.Isaac Teleop + +Follow [Install Isaac Teleop](https://isaac-sim.github.io/IsaacLab/main/source/how-to/cloudxr_teleoperation.html#cloudxr-teleoperation) to install Isaac Teleop. + +After installation, start the ClourXR server: + +```bash +python -m isaacteleop.cloudxr +``` + +### 2.Isaac Lab + +Follow [Install Isaac Lab](https://isaac-sim.github.io/IsaacLab/main/source/setup/installation/index.html) to install Isaac Lab with virtual environment `env_isaaclab`. + +After installation, activate the virtual environment: + +```bash +source /env_isaaclab/bin/activate +``` + +### 3. Install this repository to `env_isaaclab` + +```bash +cd examples/isaaclab_bimanual_robot +python -m pip install -e . + +## If you use uv: +# uv pip install -e . +``` + +## How to run + +1. Source cloudxr env + +```bash +source ~/.cloudxr/run/cloudxr.env +``` + +2. Run the task + +List the two environments: + +```bash +python scripts/list_envs.py +``` + +And you should see: + +``` ++--------+------------------------------+---------------------------------+-----------------------------------------------------------------------------------------------+ +| S. No. | Task Name | Entry Point | Config | ++--------+------------------------------+---------------------------------+-----------------------------------------------------------------------------------------------+ +| 1 | Template-FlexivRizon-Play-v0 | isaaclab.envs:ManagerBasedRLEnv | bimanual.tasks.manager_based.flexiv_rizon.flexiv_rizon_bimanual_env:FlexivRizonBimanualEnvCfg | +| 2 | Template-UR10-Play-v0 | isaaclab.envs:ManagerBasedRLEnv | bimanual.tasks.manager_based.ur10.ur10_bimanual_env:UR10BimanualEnvCfg | ++--------+------------------------------+---------------------------------+-----------------------------------------------------------------------------------------------+ +``` + +To examine the task environment only (without XR or Teleop): + +```bash +python scripts/zero_agent.py --task Template-UR10-Play-v0 --num_envs=1 + +python scripts/zero_agent.py --task Template-FlexivRizon-Play-v0 --num_envs=1 +``` + +Teleoperation with XR controllers: + +```bash +python scripts/teleop_se3_agent_bimanual_xr.py \ + --task Template-UR10-Play-v0 \ + --teleop_device motion_controllers \ + --num_envs 1 \ + --xr \ + --reverse_rotation_yz + +python scripts/teleop_se3_agent_bimanual_xr.py \ + --task Template-FlexivRizon-Play-v0 \ + --teleop_device motion_controllers \ + --num_envs 1 \ + --xr \ + --sensitivity 0.5 \ + --enable_gripper +``` + +Connect you XR devices before running the task using the [Isaac Teleop Web client](https://nvidia.github.io/IsaacTeleop/client/). + + +In IsaacLab UI, click the `Start AR` button in the bottom right panel. + + + +Key bindings: +- Press any button on the left controller to start teleoperation +- Move the left controller to control the left arm +- Move the right controller to control the right arm +- Press x or y buttons on the left controller to reset + + +Record data: + +```bash +python scripts/record_se3_agent_bimanual_xr.py \ + --task Template-UR10-Play-v0 \ + --teleop_device motion_controllers \ + --num_envs 1 \ + --xr \ + --dataset_file ./datasets/dataset.hdf5 + +# or with flexiv rizon +python scripts/record_se3_agent_bimanual_xr.py \ + --task Template-FlexivRizon-Play-v0 \ + --teleop_device motion_controllers \ + --num_envs 1 \ + --xr \ + --sensitivity 0.5 \ + --enable_gripper \ + --dataset_file ./datasets/dataset_flexiv.hdf5 +``` + +![flexiv rizon teleoperation](./images/bimanual_flexiv.png) + +Key bindings: +- Press any button on the left controller to start teleoperation +- Move the left controller to control the left arm +- Move the right controller to control the right arm +- Press x or y buttons on the left controller to reset +- Press a or b buttons on the right controller to save the trajectory and reset env + +The dataset will be saved to `./datasets/` folder. \ No newline at end of file diff --git a/examples/isaaclab_bimanual_robot/datasets/.gitignore b/examples/isaaclab_bimanual_robot/datasets/.gitignore new file mode 100644 index 000000000..c9302cf93 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/datasets/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this .gitignore file +!.gitignore diff --git a/examples/isaaclab_bimanual_robot/images/bimanual_flexiv.png b/examples/isaaclab_bimanual_robot/images/bimanual_flexiv.png new file mode 100644 index 000000000..f025fd05f Binary files /dev/null and b/examples/isaaclab_bimanual_robot/images/bimanual_flexiv.png differ diff --git a/examples/isaaclab_bimanual_robot/images/bimanual_ur.png b/examples/isaaclab_bimanual_robot/images/bimanual_ur.png new file mode 100644 index 000000000..bbd090ecf Binary files /dev/null and b/examples/isaaclab_bimanual_robot/images/bimanual_ur.png differ diff --git a/examples/isaaclab_bimanual_robot/images/start_xr.png b/examples/isaaclab_bimanual_robot/images/start_xr.png new file mode 100644 index 000000000..c965642ed Binary files /dev/null and b/examples/isaaclab_bimanual_robot/images/start_xr.png differ diff --git a/examples/isaaclab_bimanual_robot/pyproject.toml b/examples/isaaclab_bimanual_robot/pyproject.toml new file mode 100644 index 000000000..d9110cab7 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/pyproject.toml @@ -0,0 +1,158 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +[project] +name = "bimanual" +version = "0.1.0" + +[project.optional-dependencies] +data = [ + "lerobot", +] + +[tool.setuptools.packages.find] +where = ["source/"] + + + +[tool.ruff] +line-length = 120 +target-version = "py310" + +# Exclude directories +extend-exclude = [ + "logs", + "_isaac_sim", + ".vscode", + "_*", + ".git", + "assets", + "datasets", +] + +[tool.ruff.lint] +# Enable flake8 rules and other useful ones +select = [ + "E", # pycodestyle errors + "W", # pycodestyle warnings + "F", # pyflakes + "I", # isort + "UP", # pyupgrade + "C90", # mccabe complexity + # "D", # pydocstyle + "SIM", # flake8-simplify + "RET", # flake8-return +] + +# Ignore specific rules (matching your flake8 config) +ignore = [ + "E402", # Module level import not at top of file + "D401", # First line should be in imperative mood + "RET504", # Unnecessary variable assignment before return statement + "RET505", # Unnecessary elif after return statement + "SIM102", # Use a single if-statement instead of nested if-statements + "SIM103", # Return the negated condition directly + "SIM108", # Use ternary operator instead of if-else statement + "SIM117", # Merge with statements for context managers + "SIM118", # Use {key} in {dict} instead of {key} in {dict}.keys() + "UP006", # Use 'dict' instead of 'Dict' type annotation + "UP018", # Unnecessary `float` call (rewrite as a literal) +] + +[tool.ruff.lint.per-file-ignores] +"__init__.py" = ["F401"] # Allow unused imports in __init__.py files + +[tool.ruff.lint.mccabe] +max-complexity = 30 + +[tool.ruff.lint.pydocstyle] +convention = "google" + +[tool.ruff.lint.isort] + +# Custom import sections with separate sections for each Isaac Lab extension +section-order = [ + "future", + "standard-library", + "third-party", + # Group omniverse extensions separately since they are run-time dependencies + # which are pulled in by Isaac Lab extensions + "omniverse-extensions", + # Group Isaac Lab extensions together since they are all part of the Isaac Lab project + "isaaclab", + "isaaclab-contrib", + "isaaclab-rl", + "isaaclab-mimic", + "isaaclab-tasks", + "isaaclab-assets", + # First-party is reserved for project templates + "first-party", + "local-folder", +] + +[tool.ruff.lint.isort.sections] +# Define what belongs in each custom section + +"omniverse-extensions" = [ + "isaacsim", + "omni", + "pxr", + "carb", + "usdrt", + "Semantics", + "curobo", +] + +"isaaclab" = ["isaaclab"] +"isaaclab-assets" = ["isaaclab_assets"] +"isaaclab-contrib" = ["isaaclab_contrib"] +"isaaclab-rl" = ["isaaclab_rl"] +"isaaclab-mimic" = ["isaaclab_mimic"] +"isaaclab-tasks" = ["isaaclab_tasks"] + +[tool.ruff.format] + +docstring-code-format = true + +[tool.pyright] + +include = ["source", "scripts"] +exclude = [ + "**/__pycache__", + "**/_isaac_sim", + "**/docs", + "**/logs", + ".git", + ".vscode", +] + +typeCheckingMode = "basic" +pythonVersion = "3.11" +pythonPlatform = "Linux" +enableTypeIgnoreComments = true + +# This is required as the CI pre-commit does not download the module (i.e. numpy, torch, prettytable) +# Therefore, we have to ignore missing imports +reportMissingImports = "none" +# This is required to ignore for type checks of modules with stubs missing. +reportMissingModuleSource = "none" # -> most common: prettytable in mdp managers + +reportGeneralTypeIssues = "none" # -> raises 218 errors (usage of literal MISSING in dataclasses) +reportOptionalMemberAccess = "warning" # -> raises 8 errors +reportPrivateUsage = "warning" + + +[tool.codespell] +skip = '*.usd,*.usda,*.usdz,*.svg,*.png,_isaac_sim*,*.bib,*.css,*/_build' +quiet-level = 0 +# the world list should always have words in lower case +ignore-words-list = "haa,slq,collapsable,buss,reacher,thirdparty" + + +[tool.pytest.ini_options] + +markers = [ + "isaacsim_ci: mark test to run in isaacsim ci", +] diff --git a/examples/isaaclab_bimanual_robot/scripts/list_envs.py b/examples/isaaclab_bimanual_robot/scripts/list_envs.py new file mode 100644 index 000000000..abff690e9 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/scripts/list_envs.py @@ -0,0 +1,72 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +Script to print all the available environments in Isaac Lab. + +The script iterates over all registered environments and stores the details in a table. +It prints the name of the environment, the entry point and the config file. + +All the environments are registered in the `Bimanual` extension. They start +with `Template-` in their name. +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="List Isaac Lab environments.") +parser.add_argument("--keyword", type=str, default=None, help="Keyword to filter environments.") +# parse the arguments +args_cli = parser.parse_args() + +# launch omniverse app +app_launcher = AppLauncher(headless=True) +simulation_app = app_launcher.app + + +"""Rest everything follows.""" + +import gymnasium as gym +from prettytable import PrettyTable + +import bimanual.tasks # noqa: F401 + + +def main(): + """Print all environments registered in `Bimanual` extension.""" + # print all the available environments + table = PrettyTable(["S. No.", "Task Name", "Entry Point", "Config"]) + table.title = "Available Environments in Isaac Lab" + # set alignment of table columns + table.align["Task Name"] = "l" + table.align["Entry Point"] = "l" + table.align["Config"] = "l" + + # count of environments + index = 0 + # acquire all Isaac environments names + for task_spec in gym.registry.values(): + if "Template-" in task_spec.id and (args_cli.keyword is None or args_cli.keyword in task_spec.id): + # add details to table + table.add_row([index + 1, task_spec.id, task_spec.entry_point, task_spec.kwargs["env_cfg_entry_point"]]) + # increment count + index += 1 + + print(table) + + +if __name__ == "__main__": + try: + # run the main function + main() + except Exception as e: + raise e + finally: + # close the app + simulation_app.close() diff --git a/examples/isaaclab_bimanual_robot/scripts/record_se3_agent_bimanual_xr.py b/examples/isaaclab_bimanual_robot/scripts/record_se3_agent_bimanual_xr.py new file mode 100644 index 000000000..b9a8c7b2d --- /dev/null +++ b/examples/isaaclab_bimanual_robot/scripts/record_se3_agent_bimanual_xr.py @@ -0,0 +1,406 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to run teleoperation with Isaac Lab manipulation environments. + +Supports multiple input devices (e.g., keyboard, spacemouse, gamepad) and devices +configured within the environment (including OpenXR-based hand tracking or motion +controllers).""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse +from collections.abc import Callable + +from isaaclab.app import AppLauncher +import time +import numpy as np +import os + + +# add argparse arguments +parser = argparse.ArgumentParser(description="Teleoperation for Isaac Lab environments.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") +parser.add_argument( + "--teleop_device", + type=str, + default="keyboard", + help=( + "Teleop device. Set here (legacy) or via the environment config. If using the environment config, pass the" + " device key/name defined under 'teleop_devices' (it can be a custom name, not necessarily 'handtracking')." + " Built-ins: keyboard, spacemouse, gamepad. Not all tasks support all built-ins." + ), +) +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--sensitivity", type=float, default=1.0, help="Sensitivity factor.") +parser.add_argument("--enable_gripper", action="store_true", default=False, help="Enable gripper control.") +parser.add_argument("--reverse_rotation_yz", action="store_true", default=False, help="Reverse rotation yz.") +parser.add_argument( + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) + +parser.add_argument( + "--dataset_file", type=str, default="./datasets/dataset.hdf5", help="File path to export recorded demos." +) + +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +app_launcher_args = vars(args_cli) + +if args_cli.enable_pinocchio: + # Import pinocchio before AppLauncher to force the use of the version installed by IsaacLab and + # not the one installed by Isaac Sim pinocchio is required by the Pink IK controllers and the + # GR1T2 retargeter + import pinocchio # noqa: F401 +if "handtracking" in args_cli.teleop_device.lower(): + app_launcher_args["xr"] = True + +# launch omniverse app +app_launcher = AppLauncher(app_launcher_args) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + + +import logging + +import gymnasium as gym +import torch + +from isaaclab.devices import Se3Gamepad, Se3GamepadCfg, Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from isaaclab.devices.openxr import remove_camera_configs +from isaaclab.devices.teleop_device_factory import create_teleop_device +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.manager_based.manipulation.lift import mdp +from isaaclab_tasks.utils import parse_env_cfg + +if args_cli.enable_pinocchio: + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 + import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + +import bimanual.tasks # noqa: F401 +from bimanual.tasks.manager_based.common.retargeter import BimanualOpenXRRetargeter + +from isaaclab.devices.device_base import DeviceBase + +from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg +from isaaclab.managers import DatasetExportMode + + +# import logger +logger = logging.getLogger(__name__) + + +def main() -> None: + """ + Run teleoperation with an Isaac Lab manipulation environment. + + Creates the environment, sets up teleoperation interfaces and callbacks, + and runs the main simulation loop until the application is closed. + + Returns: + None + """ + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs) + env_cfg.env_name = args_cli.task + if not isinstance(env_cfg, ManagerBasedRLEnvCfg): + raise ValueError( + "Teleoperation is only supported for ManagerBasedRLEnv environments. " + f"Received environment config type: {type(env_cfg).__name__}" + ) + # modify configuration + env_cfg.terminations.time_out = None + if "Lift" in args_cli.task: + # set the resampling time range to large number to avoid resampling + env_cfg.commands.object_pose.resampling_time_range = (1.0e9, 1.0e9) + # add termination condition for reaching the goal otherwise the environment won't reset + env_cfg.terminations.object_reached_goal = DoneTerm(func=mdp.object_reached_goal) + + # /World/envs/env_.*/berkeley_table/BerkeleyRobotTable/overhead_camera. + if args_cli.xr: + if not args_cli.enable_cameras: + env_cfg = remove_camera_configs(env_cfg) + env_cfg.sim.render.antialiasing_mode = "DLSS" + + # modify configuration such that the environment runs indefinitely until + # the goal is reached or other termination conditions are met + env_cfg.terminations.time_out = None + env_cfg.observations.policy.concatenate_terms = False + + # set up dataset recording + # get directory path and file name (without extension) from cli arguments + output_dir = os.path.dirname(args_cli.dataset_file) + output_file_name = os.path.splitext(os.path.basename(args_cli.dataset_file))[0] + + # create directory if it does not exist + if not os.path.exists(output_dir): + os.makedirs(output_dir) + print(f"Created output directory: {output_dir}") + + env_cfg.recorders = ActionStateRecorderManagerCfg() + env_cfg.recorders.dataset_export_dir_path = output_dir + env_cfg.recorders.dataset_filename = output_file_name + env_cfg.recorders.dataset_export_mode = DatasetExportMode.EXPORT_ALL + + # no success conditions for now + env_cfg.terminations.success = None + + try: + # create environment + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + # check environment name (for reach , we don't allow the gripper) + if "Reach" in args_cli.task: + logger.warning( + f"The environment '{args_cli.task}' does not support gripper control. The device command will be" + " ignored." + ) + except Exception as e: + logger.error(f"Failed to create environment: {e}") + simulation_app.close() + return + + # Flags for controlling teleoperation flow + should_reset_recording_instance = False + teleoperation_active = True + + # Callback handlers + def reset_recording_instance() -> None: + """ + Reset the environment to its initial state. + + Sets a flag to reset the environment on the next simulation step. + + Returns: + None + """ + nonlocal should_reset_recording_instance + should_reset_recording_instance = True + print("Reset triggered - Environment will reset on next step") + + def start_teleoperation() -> None: + """ + Activate teleoperation control of the robot. + + Enables the application of teleoperation commands to the environment. + + Returns: + None + """ + nonlocal teleoperation_active + teleoperation_active = True + print("Teleoperation activated") + + def stop_teleoperation() -> None: + """ + Deactivate teleoperation control of the robot. + + Disables the application of teleoperation commands to the environment. + + Returns: + None + """ + nonlocal teleoperation_active + teleoperation_active = False + print("Teleoperation deactivated") + + # Create device config if not already in env_cfg + teleoperation_callbacks: dict[str, Callable[[], None]] = { + "R": reset_recording_instance, + "START": start_teleoperation, + "STOP": stop_teleoperation, + "RESET": reset_recording_instance, + } + + # For hand tracking devices, add additional callbacks + if args_cli.xr: + # Default to inactive for hand tracking + teleoperation_active = False + else: + # Always active for other devices + teleoperation_active = True + + # Create teleop device from config if present, otherwise create manually + teleop_interface = None + try: + if hasattr(env_cfg, "teleop_devices") and args_cli.teleop_device in env_cfg.teleop_devices.devices: + teleop_interface = create_teleop_device( + args_cli.teleop_device, env_cfg.teleop_devices.devices, teleoperation_callbacks + ) + else: + logger.warning( + f"No teleop device '{args_cli.teleop_device}' found in environment config. Creating default." + ) + # Create fallback teleop device + sensitivity = args_cli.sensitivity + if args_cli.teleop_device.lower() == "keyboard": + teleop_interface = Se3Keyboard( + Se3KeyboardCfg(pos_sensitivity=0.05 * sensitivity, rot_sensitivity=0.05 * sensitivity) + ) + elif args_cli.teleop_device.lower() == "spacemouse": + teleop_interface = Se3SpaceMouse( + Se3SpaceMouseCfg(pos_sensitivity=0.05 * sensitivity, rot_sensitivity=0.05 * sensitivity) + ) + elif args_cli.teleop_device.lower() == "gamepad": + teleop_interface = Se3Gamepad( + Se3GamepadCfg(pos_sensitivity=0.1 * sensitivity, rot_sensitivity=0.1 * sensitivity) + ) + else: + logger.error(f"Unsupported teleop device: {args_cli.teleop_device}") + logger.error("Configure the teleop device in the environment config.") + env.close() + simulation_app.close() + return + + # Add callbacks to fallback device + for key, callback in teleoperation_callbacks.items(): + try: + teleop_interface.add_callback(key, callback) + except (ValueError, TypeError) as e: + logger.warning(f"Failed to add callback for key {key}: {e}") + except Exception as e: + logger.error(f"Failed to create teleop device: {e}") + env.close() + simulation_app.close() + return + + if teleop_interface is None: + logger.error("Failed to create teleop interface") + env.close() + simulation_app.close() + return + + print(f"Using teleop device: {teleop_interface}") + + # reset environment + env.reset() + teleop_interface.reset() + + # render once + env.sim.render() + + # init controller + retargeter = BimanualOpenXRRetargeter(env, env.sim.device) + + print("Teleoperation started.") + + start_button_pressed = False + # simulate environment + while simulation_app.is_running(): + try: + # run everything in inference mode + with torch.inference_mode(): + # get device command + raw_data = teleop_interface._get_raw_data() + left_controller_data = np.array(raw_data.get(DeviceBase.TrackingTarget.CONTROLLER_LEFT, np.array([]))) + right_controller_data = np.array(raw_data.get(DeviceBase.TrackingTarget.CONTROLLER_RIGHT, np.array([]))) + # robot root pose from observation/scene + logger.debug("[time] %s", time.time()) + logger.debug("[left_controller_data]: %s", left_controller_data) + logger.debug("[right_controller_data]: %s", right_controller_data) + + if left_controller_data.shape[0] == 0 or right_controller_data.shape[0] == 0: + env.sim.render() + continue + + logger.debug("[left_controller_data] buttons: %s", np.sum(left_controller_data[1])) + logger.debug("[right_controller_data] buttons: %s", np.sum(right_controller_data[1])) + + # add an initialization condition; press left trigger to start + if np.sum(left_controller_data[1]) > 0: + start_button_pressed = True + + if not start_button_pressed: + env.sim.render() + continue + + if not retargeter._initialized: + ctrl_left_pos_init = left_controller_data[0, 0:3] + ctrl_left_quat_init = left_controller_data[0, 3:7] + ctrl_right_pos_init = right_controller_data[0, 0:3] + ctrl_right_quat_init = right_controller_data[0, 3:7] + retargeter.reset( + ctrl_left_pos_init, + ctrl_left_quat_init, + ctrl_right_pos_init, + ctrl_right_quat_init, + ) + + ctrl_left_pos_cur = torch.tensor(left_controller_data[0, 0:3], device=env.sim.device) + ctrl_left_quat_cur = torch.tensor(left_controller_data[0, 3:7], device=env.sim.device) + ctrl_right_pos_cur = torch.tensor(right_controller_data[0, 0:3], device=env.sim.device) + ctrl_right_quat_cur = torch.tensor(right_controller_data[0, 3:7], device=env.sim.device) + + # obtain gripper commands + if args_cli.enable_gripper: + gripper_left = 1.0 if np.sum(left_controller_data[1]) > 0 else -1.0 + gripper_right = 1.0 if np.sum(right_controller_data[1]) > 0 else -1.0 + else: + gripper_left = None + gripper_right = None + + compute_action = retargeter.compute_action( + ctrl_left_pos_cur, + ctrl_left_quat_cur, + ctrl_right_pos_cur, + ctrl_right_quat_cur, + gripper_left=gripper_left, + gripper_right=gripper_right, + scale=args_cli.sensitivity, + reverse_yz=args_cli.reverse_rotation_yz, + ) + + obs, _, _, _, _ = env.step(compute_action) + + # press x or y to reset + successful = False + if np.sum(left_controller_data[1][4:6]) > 0: + should_reset_recording_instance = True + + # press a or b to reset env and save recording + if np.sum(right_controller_data[1][4:6]) > 0: + should_reset_recording_instance = True + successful = True + + if should_reset_recording_instance: + if successful: + env.recorder_manager.export_episodes(env_ids=[0]) # export the current episode before resetting + + env.sim.reset() + env.recorder_manager.reset() + env.reset() + teleop_interface.reset() + + env.sim.render() + + start_button_pressed = False + retargeter._initialized = False + + should_reset_recording_instance = False + print("Environment reset complete") + + except Exception as e: + logger.error(f"Error during simulation step: {e}") + break + + # close the simulator + env.close() + print("Environment closed") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/examples/isaaclab_bimanual_robot/scripts/teleop_se3_agent_bimanual_xr.py b/examples/isaaclab_bimanual_robot/scripts/teleop_se3_agent_bimanual_xr.py new file mode 100644 index 000000000..fb86fb0d4 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/scripts/teleop_se3_agent_bimanual_xr.py @@ -0,0 +1,350 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to run teleoperation with Isaac Lab manipulation environments. + +Supports multiple input devices (e.g., keyboard, spacemouse, gamepad) and devices +configured within the environment (including OpenXR-based hand tracking or motion +controllers).""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse +from collections.abc import Callable + +from isaaclab.app import AppLauncher +import time +import numpy as np + + + +# add argparse arguments +parser = argparse.ArgumentParser(description="Teleoperation for Isaac Lab environments.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") +parser.add_argument( + "--teleop_device", + type=str, + default="keyboard", + help=( + "Teleop device. Set here (legacy) or via the environment config. If using the environment config, pass the" + " device key/name defined under 'teleop_devices' (it can be a custom name, not necessarily 'handtracking')." + " Built-ins: keyboard, spacemouse, gamepad. Not all tasks support all built-ins." + ), +) +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +parser.add_argument("--sensitivity", type=float, default=1.0, help="Sensitivity factor.") +parser.add_argument("--reverse_rotation_yz", action="store_true", default=False, help="Reverse rotation yz.") +parser.add_argument("--enable_gripper", action="store_true", default=False, help="Enable gripper control.") +parser.add_argument( + "--enable_pinocchio", + action="store_true", + default=False, + help="Enable Pinocchio.", +) +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +app_launcher_args = vars(args_cli) + +if args_cli.enable_pinocchio: + # Import pinocchio before AppLauncher to force the use of the version installed by IsaacLab and + # not the one installed by Isaac Sim pinocchio is required by the Pink IK controllers and the + # GR1T2 retargeter + import pinocchio # noqa: F401 +if "handtracking" in args_cli.teleop_device.lower(): + app_launcher_args["xr"] = True + +# launch omniverse app +app_launcher = AppLauncher(app_launcher_args) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + + +import logging + +import gymnasium as gym +import torch + +from isaaclab.devices import Se3Gamepad, Se3GamepadCfg, Se3Keyboard, Se3KeyboardCfg, Se3SpaceMouse, Se3SpaceMouseCfg +from isaaclab.devices.openxr import remove_camera_configs +from isaaclab.devices.teleop_device_factory import create_teleop_device +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.manager_based.manipulation.lift import mdp +from isaaclab_tasks.utils import parse_env_cfg + +if args_cli.enable_pinocchio: + import isaaclab_tasks.manager_based.locomanipulation.pick_place # noqa: F401 + import isaaclab_tasks.manager_based.manipulation.pick_place # noqa: F401 + +import bimanual.tasks # noqa: F401 +from bimanual.tasks.manager_based.common.retargeter import BimanualOpenXRRetargeter + + +from isaaclab.utils.math import subtract_frame_transforms +from isaaclab.devices.device_base import DeviceBase + +# import logger +logger = logging.getLogger(__name__) + + +def main() -> None: + """ + Run teleoperation with an Isaac Lab manipulation environment. + + Creates the environment, sets up teleoperation interfaces and callbacks, + and runs the main simulation loop until the application is closed. + + Returns: + None + """ + # parse configuration + env_cfg = parse_env_cfg(args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs) + env_cfg.env_name = args_cli.task + if not isinstance(env_cfg, ManagerBasedRLEnvCfg): + raise ValueError( + "Teleoperation is only supported for ManagerBasedRLEnv environments. " + f"Received environment config type: {type(env_cfg).__name__}" + ) + # modify configuration + env_cfg.terminations.time_out = None + if "Lift" in args_cli.task: + # set the resampling time range to large number to avoid resampling + env_cfg.commands.object_pose.resampling_time_range = (1.0e9, 1.0e9) + # add termination condition for reaching the goal otherwise the environment won't reset + env_cfg.terminations.object_reached_goal = DoneTerm(func=mdp.object_reached_goal) + + if args_cli.xr: + env_cfg = remove_camera_configs(env_cfg) + env_cfg.sim.render.antialiasing_mode = "DLSS" + + try: + # create environment + env = gym.make(args_cli.task, cfg=env_cfg).unwrapped + # check environment name (for reach , we don't allow the gripper) + if "Reach" in args_cli.task: + logger.warning( + f"The environment '{args_cli.task}' does not support gripper control. The device command will be" + " ignored." + ) + except Exception as e: + logger.error(f"Failed to create environment: {e}") + simulation_app.close() + return + + # Flags for controlling teleoperation flow + should_reset_recording_instance = False + teleoperation_active = True + + # Callback handlers + def reset_recording_instance() -> None: + """ + Reset the environment to its initial state. + + Sets a flag to reset the environment on the next simulation step. + + Returns: + None + """ + nonlocal should_reset_recording_instance + should_reset_recording_instance = True + print("Reset triggered - Environment will reset on next step") + + def start_teleoperation() -> None: + """ + Activate teleoperation control of the robot. + + Enables the application of teleoperation commands to the environment. + + Returns: + None + """ + nonlocal teleoperation_active + teleoperation_active = True + print("Teleoperation activated") + + def stop_teleoperation() -> None: + """ + Deactivate teleoperation control of the robot. + + Disables the application of teleoperation commands to the environment. + + Returns: + None + """ + nonlocal teleoperation_active + teleoperation_active = False + print("Teleoperation deactivated") + + # Create device config if not already in env_cfg + teleoperation_callbacks: dict[str, Callable[[], None]] = { + "R": reset_recording_instance, + "START": start_teleoperation, + "STOP": stop_teleoperation, + "RESET": reset_recording_instance, + } + + # For hand tracking devices, add additional callbacks + if args_cli.xr: + # Default to inactive for hand tracking + teleoperation_active = False + else: + # Always active for other devices + teleoperation_active = True + + # Create teleop device from config if present, otherwise create manually + teleop_interface = None + try: + if hasattr(env_cfg, "teleop_devices") and args_cli.teleop_device in env_cfg.teleop_devices.devices: + teleop_interface = create_teleop_device( + args_cli.teleop_device, env_cfg.teleop_devices.devices, teleoperation_callbacks + ) + else: + logger.warning( + f"No teleop device '{args_cli.teleop_device}' found in environment config. Creating default." + ) + # Create fallback teleop device + sensitivity = args_cli.sensitivity + if args_cli.teleop_device.lower() == "keyboard": + teleop_interface = Se3Keyboard( + Se3KeyboardCfg(pos_sensitivity=0.05 * sensitivity, rot_sensitivity=0.05 * sensitivity) + ) + elif args_cli.teleop_device.lower() == "spacemouse": + teleop_interface = Se3SpaceMouse( + Se3SpaceMouseCfg(pos_sensitivity=0.05 * sensitivity, rot_sensitivity=0.05 * sensitivity) + ) + elif args_cli.teleop_device.lower() == "gamepad": + teleop_interface = Se3Gamepad( + Se3GamepadCfg(pos_sensitivity=0.1 * sensitivity, rot_sensitivity=0.1 * sensitivity) + ) + else: + logger.error(f"Unsupported teleop device: {args_cli.teleop_device}") + logger.error("Configure the teleop device in the environment config.") + env.close() + simulation_app.close() + return + + # Add callbacks to fallback device + for key, callback in teleoperation_callbacks.items(): + try: + teleop_interface.add_callback(key, callback) + except (ValueError, TypeError) as e: + logger.warning(f"Failed to add callback for key {key}: {e}") + except Exception as e: + logger.error(f"Failed to create teleop device: {e}") + env.close() + simulation_app.close() + return + + if teleop_interface is None: + logger.error("Failed to create teleop interface") + env.close() + simulation_app.close() + return + + print(f"Using teleop device: {teleop_interface}") + + # reset environment + env.reset() + teleop_interface.reset() + + # render once + env.sim.render() + + # init controller + retargeter = BimanualOpenXRRetargeter(env, env.sim.device) + + print("Teleoperation started.") + + + start_button_pressed = False + # simulate environment + while simulation_app.is_running(): + try: + # run everything in inference mode + with torch.inference_mode(): + # get device command + # action = teleop_interface.advance() + + raw_data = teleop_interface._get_raw_data() + left_controller_data = np.array(raw_data.get(DeviceBase.TrackingTarget.CONTROLLER_LEFT, np.array([]))) + right_controller_data = np.array(raw_data.get(DeviceBase.TrackingTarget.CONTROLLER_RIGHT, np.array([]))) + + if left_controller_data.shape[0] == 0 or right_controller_data.shape[0] == 0: + env.sim.render() + continue + + # add an initialization condition; press left trigger to start + if np.sum(left_controller_data[1]) > 0: + start_button_pressed = True + + if not start_button_pressed: + env.sim.render() + continue + + # init retargeter if not initialized, record the initial positions and orientations of the controllers + if not retargeter._initialized: + ctrl_left_pos_init = left_controller_data[0, 0:3] + ctrl_left_quat_init = left_controller_data[0, 3:7] + ctrl_right_pos_init = right_controller_data[0, 0:3] + ctrl_right_quat_init = right_controller_data[0, 3:7] + retargeter.reset(ctrl_left_pos_init, ctrl_left_quat_init, ctrl_right_pos_init, ctrl_right_quat_init) + + # obtain current positions and orientations of the controllers + ctrl_left_pos_current = torch.tensor(left_controller_data[0, 0:3], device=env.sim.device) + ctrl_left_quat_current = torch.tensor(left_controller_data[0, 3:7], device=env.sim.device) + ctrl_right_pos_current = torch.tensor(right_controller_data[0, 0:3], device=env.sim.device) + ctrl_right_quat_current = torch.tensor(right_controller_data[0, 3:7], device=env.sim.device) + + # obtain gripper commands + if args_cli.enable_gripper: + gripper_left = 1.0 if np.sum(left_controller_data[1]) > 0 else -1.0 + gripper_right = 1.0 if np.sum(right_controller_data[1]) > 0 else -1.0 + else: + gripper_left = None + gripper_right = None + + # compute action + compute_action = retargeter.compute_action( + ctrl_left_pos_current, ctrl_left_quat_current, ctrl_right_pos_current, ctrl_right_quat_current, + gripper_left=gripper_left, gripper_right=gripper_right, + scale=args_cli.sensitivity, # smaller scale to make it slower and less sensitive + reverse_yz=args_cli.reverse_rotation_yz, + ) + + obs, _, _, _, _= env.step(compute_action) + + # press x or y to reset + if np.sum(left_controller_data[1][4:6]) > 0: + # reset environment + env.reset() + teleop_interface.reset() + + # render once + env.sim.render() + + start_button_pressed = False + retargeter._initialized = False + + + except Exception as e: + logger.error(f"Error during simulation step: {e}") + break + + # close the simulator + env.close() + print("Environment closed") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/examples/isaaclab_bimanual_robot/scripts/zero_agent.py b/examples/isaaclab_bimanual_robot/scripts/zero_agent.py new file mode 100644 index 000000000..7102cb801 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/scripts/zero_agent.py @@ -0,0 +1,73 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Script to run an environment with zero action agent.""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from isaaclab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Zero agent for Isaac Lab environments.") +parser.add_argument( + "--disable_fabric", action="store_true", default=False, help="Disable fabric and use USD I/O operations." +) +parser.add_argument("--num_envs", type=int, default=None, help="Number of environments to simulate.") +parser.add_argument("--task", type=str, default=None, help="Name of the task.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import gymnasium as gym +import torch + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.utils import parse_env_cfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +import bimanual.tasks # noqa: F401 + + +def main(): + """Zero actions agent with Isaac Lab environment.""" + # parse configuration + env_cfg = parse_env_cfg( + args_cli.task, device=args_cli.device, num_envs=args_cli.num_envs, use_fabric=not args_cli.disable_fabric + ) + # create environment + env = gym.make(args_cli.task, cfg=env_cfg) + + # print info (this is vectorized environment) + print(f"[INFO]: Gym observation space: {env.observation_space}") + print(f"[INFO]: Gym action space: {env.action_space}") + # reset environment + env.reset() + # simulate environment + while simulation_app.is_running(): + # run everything in inference mode + with torch.inference_mode(): + # compute zero actions + actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device) + # apply actions + env.step(actions) + + # close the simulator + env.close() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/__init__.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/__init__.py new file mode 100644 index 000000000..13df3c321 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Package containing task implementations for the extension.""" + +## +# Register Gym environments. +## + +from isaaclab_tasks.utils import import_packages + +# The blacklist is used to prevent importing configs from sub-packages +_BLACKLIST_PKGS = ["utils", ".mdp"] +# Import all configs in this package +import_packages(__name__, _BLACKLIST_PKGS) diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/__init__.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/__init__.py new file mode 100644 index 000000000..65d6e5a24 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym # noqa: F401 diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/base_env_cfg.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/base_env_cfg.py new file mode 100644 index 000000000..86e1d5494 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/base_env_cfg.py @@ -0,0 +1,211 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING +import os + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices import DevicesCfg +from isaaclab.devices.gamepad import Se3GamepadCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import ActionTermCfg as ActionTerm +from isaaclab.managers import CurriculumTermCfg as CurrTerm +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 RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.utils import configclass +from isaaclab.utils.noise import AdditiveUniformNoiseCfg as Unoise + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR + +## +# Scene definition +## + +ASSET_ROOT = os.path.join(os.path.dirname(__file__), "./assets") + + +@configclass +class ReachSceneCfg(InteractiveSceneCfg): + """Configuration for the scene with a robotic arm.""" + + # World + ground = AssetBaseCfg( + prim_path="/World/ground", + spawn=sim_utils.GroundPlaneCfg(size=(2, 2), visible=False), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 0)), + ) + + # Table + table = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/table", + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.70, 0.5, 0.82], rot=[0.7071, 0.0, 0.0, 0.7071]), + spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"), + ) + + # Robots + left_robot: ArticulationCfg = MISSING + right_robot: ArticulationCfg = MISSING + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg(color=(0.75, 0.75, 0.75), intensity=1000.0), + ) + + +## +# MDP settings +## + + +@configclass +class BimanualActionsCfg: + left_arm_action: ActionTerm = MISSING + left_gripper_action: ActionTerm | None = None + right_arm_action: ActionTerm = MISSING + right_gripper_action: ActionTerm | None = None + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + joint_pos = ObsTerm( + func=mdp.joint_pos_rel, + noise=Unoise(n_min=-0.01, n_max=0.01), + params={"asset_cfg": SceneEntityCfg("left_robot")}, + ) + joint_vel = ObsTerm( + func=mdp.joint_vel_rel, + noise=Unoise(n_min=-0.01, n_max=0.01), + params={"asset_cfg": SceneEntityCfg("left_robot")}, + ) + actions = ObsTerm(func=mdp.last_action) + + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + reset_left_robot_joints = EventTerm( + func=mdp.reset_joints_by_scale, + mode="reset", + params={ + "position_range": (1.0, 1.0), + "velocity_range": (0.0, 0.0), + "asset_cfg": SceneEntityCfg("left_robot"), + }, + ) + + reset_right_robot_joints = EventTerm( + func=mdp.reset_joints_by_scale, + mode="reset", + params={ + "position_range": (1.0, 1.0), + "velocity_range": (0.0, 0.0), + "asset_cfg": SceneEntityCfg("right_robot"), + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + # action penalty + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-0.0001) + joint_vel = RewTerm( + func=mdp.joint_vel_l2, + weight=-0.0001, + params={"asset_cfg": SceneEntityCfg("left_robot")}, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + action_rate = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "action_rate", "weight": -0.005, "num_steps": 4500} + ) + + joint_vel = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "joint_vel", "weight": -0.001, "num_steps": 4500} + ) + + +## +# Environment configuration +## +@configclass +class BaseEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the reach end-effector pose tracking environment.""" + + # Scene settings + scene: ReachSceneCfg = ReachSceneCfg(num_envs=4096, env_spacing=2.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions = BimanualActionsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 2 + self.sim.render_interval = self.decimation + self.episode_length_s = 12.0 + self.viewer.eye = (3.5, 3.5, 3.5) + # simulation settings + self.sim.dt = 1.0 / 60.0 + + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": Se3KeyboardCfg( + gripper_term=False, + sim_device=self.sim.device, + ), + "gamepad": Se3GamepadCfg( + gripper_term=False, + sim_device=self.sim.device, + ), + "spacemouse": Se3SpaceMouseCfg( + gripper_term=False, + sim_device=self.sim.device, + ), + }, + ) diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/dummy_retargeter.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/dummy_retargeter.py new file mode 100644 index 000000000..5c84c29bd --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/dummy_retargeter.py @@ -0,0 +1,59 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from dataclasses import dataclass + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + + +class DummyRetargeter(RetargeterBase): + """Dummy retargeter that maps motion controller inputs to robot commands. + """ + + def __init__(self, cfg: DummyRetargeterCfg): + """Initialize the retargeter.""" + super().__init__(cfg) + self._sim_device = cfg.sim_device + self._hand_joint_names = cfg.hand_joint_names + self._enable_visualization = cfg.enable_visualization + + if cfg.hand_joint_names is None: + raise ValueError("hand_joint_names must be provided") + + # Initialize visualization if enabled + if self._enable_visualization: + marker_cfg = VisualizationMarkersCfg( + prim_path="/Visuals/g1_controller_markers", + markers={ + "joint": sim_utils.SphereCfg( + radius=0.01, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ), + }, + ) + self._markers = VisualizationMarkers(marker_cfg) + + def retarget(self, data: dict) -> torch.Tensor: + """Convert controller inputs to robot commands. + """ + pass + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + +@dataclass +class DummyRetargeterCfg(RetargeterCfg): + """Configuration for the Dummy retargeter.""" + + enable_visualization: bool = False + hand_joint_names: list[str] | None = None # List of robot hand joint names + retargeter_type: type[RetargeterBase] = DummyRetargeter diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/retargeter.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/retargeter.py new file mode 100644 index 000000000..aaa4d4736 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/common/retargeter.py @@ -0,0 +1,158 @@ +from isaaclab.utils.math import quat_mul, quat_inv, quat_apply +import torch + +from dataclasses import dataclass + +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg + + +class BimanualOpenXRRetargeter(RetargeterBase): + def __init__(self, env, device): + self.env = env + self.device = device + self._initialized = False + + def reset( + self, + ctrl_left_pos_init: list[float], + ctrl_left_quat_init: list[float], + ctrl_right_pos_init: list[float], + ctrl_right_quat_init: list[float], + ): + """Call this once after env.reset(), before the control loop.""" + + left_arm_action_term = self.env.action_manager.get_term("left_arm_action") + ee_pos_curr, ee_quat_curr = left_arm_action_term._compute_frame_pose() + + right_arm_action_term = self.env.action_manager.get_term("right_arm_action") + ee_pos_curr2, ee_quat_curr2 = right_arm_action_term._compute_frame_pose() + + # --- Controller initial poses (OpenXR world frame) --- + self.ctrl_left_pos_init = torch.tensor(ctrl_left_pos_init, device=self.device) + self.ctrl_right_pos_init = torch.tensor(ctrl_right_pos_init, device=self.device) + self.ctrl_left_quat_init = torch.tensor(ctrl_left_quat_init, device=self.device) # [w,x,y,z] + self.ctrl_right_quat_init = torch.tensor(ctrl_right_quat_init, device=self.device) + + # --- Robot initial EE poses (in each robot's base frame) --- + robot_left = self.env.scene["left_robot"] + robot_right = self.env.scene["right_robot"] + + # EE frame is tracked by FrameTransformer + # shape: (N, 3) and (N, 4) — take env 0 + self.ee_left_pos_init, self.ee_left_quat_init = ee_pos_curr[0], ee_quat_curr[0] + self.ee_right_pos_init, self.ee_right_quat_init = ee_pos_curr2[0], ee_quat_curr2[0] + + # Cache robot base orientation (fixed-base robot, constant after reset) + self.robot_left_quat_w = robot_left.data.root_quat_w[0] # (4,) [w,x,y,z] + self.robot_right_quat_w = robot_right.data.root_quat_w[0] + + self._initialized = True + + def compute_action( + self, + ctrl_left_pos_w: torch.Tensor, # (3,) current left controller pos in world + ctrl_left_quat_w: torch.Tensor, # (4,) [w,x,y,z] + ctrl_right_pos_w: torch.Tensor, + ctrl_right_quat_w: torch.Tensor, + gripper_left: float = None, + gripper_right: float = None, + scale: float = 1.0, + reverse_yz: bool = True, + ) -> torch.Tensor: + """Returns action tensor of shape (1, 14) = [left_7D, right_7D].""" + + has_gripper = gripper_left is not None and gripper_right is not None + + left = self._delta_action( + ctrl_left_pos_w, + ctrl_left_quat_w, + self.ctrl_left_pos_init, + self.ctrl_left_quat_init, + self.ee_left_pos_init, + self.ee_left_quat_init, + self.robot_left_quat_w, + scale, + reverse_yz, + ) + right = self._delta_action( + ctrl_right_pos_w, + ctrl_right_quat_w, + self.ctrl_right_pos_init, + self.ctrl_right_quat_init, + self.ee_right_pos_init, + self.ee_right_quat_init, + self.robot_right_quat_w, + scale, + reverse_yz, + ) + + if has_gripper: + return torch.cat( + [ + left, + torch.tensor([gripper_left], device=self.device), + right, + torch.tensor([gripper_right], device=self.device), + ] + ).unsqueeze(0) + else: + return torch.cat([left, right]).unsqueeze(0) + + def _delta_action( + self, + ctrl_pos_w: torch.Tensor, # current controller pos in world + ctrl_quat_w: torch.Tensor, # current controller quat in world [w,x,y,z] + ctrl_pos_init: torch.Tensor, + ctrl_quat_init: torch.Tensor, + ee_pos_init_b: torch.Tensor, # robot EE initial pos in base frame + ee_quat_init_b: torch.Tensor, # robot EE initial quat in base frame + robot_quat_w: torch.Tensor, # robot base orientation in world [w,x,y,z] + scale: float = 1.0, # scale factor for the delta position + reverse_yz: bool = True, # swap y and z dimensions of delta_quat [w, x, y, z] -> swap indices 2 and 3, negate Z + ) -> torch.Tensor: + # 1. Position delta in world frame + delta_pos_w = ctrl_pos_w - ctrl_pos_init # (3,) + + # 2. Rotate delta into robot base frame + # (robot_quat_w rotates world→base via its inverse) + delta_pos_b = quat_apply(quat_inv(robot_quat_w.unsqueeze(0)), delta_pos_w.unsqueeze(0)).squeeze(0) + + # 3. Desired EE position = initial + scaled delta + desired_pos_b = ee_pos_init_b + delta_pos_b * scale + + # 4. Rotation delta: how much the controller rotated from its initial pose + delta_quat = quat_mul(quat_inv(ctrl_quat_init.unsqueeze(0)), ctrl_quat_w.unsqueeze(0)).squeeze(0) + + # 5. Apply rotation delta to initial EE orientation + # Interchange Y and Z dimensions of delta_quat [w, x, y, z] -> swap indices 2 and 3, negate Z + if reverse_yz: + delta_quat = delta_quat * torch.tensor([1.0, -1.0, 1.0, 1.0], device=delta_quat.device) + delta_quat = delta_quat[[0, 1, 3, 2]] + else: # reverse_xy + delta_quat = delta_quat * torch.tensor([1.0, 1.0, -1.0, 1.0], device=delta_quat.device) + delta_quat = delta_quat[[0, 2, 1, 3]] + desired_quat_b = quat_mul(delta_quat.unsqueeze(0), ee_quat_init_b.unsqueeze(0)).squeeze(0) + + return torch.cat([desired_pos_b, desired_quat_b]) # (7,) + + def retarget( + self, + ctrl_left_pos_w, + ctrl_left_quat_w, + ctrl_right_pos_w, + ctrl_right_quat_w, + gripper_left, + gripper_right, + scale=1.0, + ): + return self.compute_action( + ctrl_left_pos_w, ctrl_left_quat_w, ctrl_right_pos_w, ctrl_right_quat_w, gripper_left, gripper_right, scale + ) + + +@dataclass +class BimanualOpenXRRetargeterCfg(RetargeterCfg): + """Configuration for the bimanual UR5E OpenXR retargeter.""" + + enable_visualization: bool = False + retargeter_type: type[RetargeterBase] = BimanualOpenXRRetargeter diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/__init__.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/__init__.py new file mode 100644 index 000000000..943129baf --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +gym.register( + id="Template-FlexivRizon-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flexiv_rizon_bimanual_env:FlexivRizonBimanualEnvCfg", + }, +) diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/flexiv_rizon_bimanual_env.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/flexiv_rizon_bimanual_env.py new file mode 100644 index 000000000..00db0def2 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/flexiv_rizon_bimanual_env.py @@ -0,0 +1,172 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np +import torch + +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp + + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from .ik.task_space_actions import FlexivRizonDifferentialInverseKinematicsActionCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.sensors import CameraCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +import isaaclab.sim as sim_utils + +## +# Pre-defined configs +## +from .flexiv_rizon_robots import FLEXIV_RIZON_4S_CFG # isort: skip +from bimanual.tasks.manager_based.common.base_env_cfg import BaseEnvCfg +from bimanual.tasks.manager_based.common.dummy_retargeter import DummyRetargeterCfg + +from .ik.differential_ik_cfg import FlexivRizonDifferentialIKControllerCfg + +## +# Environment configuration +## + + +@configclass +class FlexivRizonBimanualEnvCfg(BaseEnvCfg): + def __init__(self): + super().__init__() + + self.scene.table.init_state = AssetBaseCfg.InitialStateCfg( + pos=[0.70, 0.5, 0.82], + rot=[0.0, 0.0, 0.0, -1.0] + ) + + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.left_robot = FLEXIV_RIZON_4S_CFG.replace( + prim_path="{ENV_REGEX_NS}/left_robot", + init_state=FLEXIV_RIZON_4S_CFG.InitialStateCfg( + pos=(0.0, 0.0, 1.0), + rot=(0.7071, 0, 0.7071, 0.0), + joint_pos={ + "joint1": 0.0, + "joint2": 1.5708, + "joint3": 0.0, + "joint4": 1.5708, + "joint5": 0.0, + "joint6": 0.0, + "joint7": 0.0, + } + ), + ) + self.scene.right_robot = FLEXIV_RIZON_4S_CFG.replace( + prim_path="{ENV_REGEX_NS}/right_robot", + init_state=FLEXIV_RIZON_4S_CFG.InitialStateCfg( + pos=(0.0, 0.8, 1.0), + rot=(0.7071, 0, 0.7071, 0.0), + joint_pos={ + "joint1": 0.0, + "joint2": 1.5708, + "joint3": 0.0, + "joint4": 1.5708, + "joint5": 0.0, + "joint6": 0.0, + "joint7": 0.0, + } + ), + ) + + # Replace joint-position arm action with differential IK (relative pose) + # NOTE: body_offset.pos is the TCP offset from ee_link along its z-axis (~130 mm for Robotiq 2F-85). + # Verify against the actual USD if needed. + self.actions.left_arm_action = FlexivRizonDifferentialInverseKinematicsActionCfg( + asset_name="left_robot", + joint_names=[ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ], + body_name="gripper_base", + controller=FlexivRizonDifferentialIKControllerCfg( + command_type="pose", + use_relative_mode=False, + ik_method="dls", + ik_params={"lambda_val": 0.05}, + joint_weights=torch.tensor([10.0, 10.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + ), + scale=1.0, + body_offset=FlexivRizonDifferentialInverseKinematicsActionCfg.OffsetCfg(pos=(0.0, 0.0, 0.2)), + ) + + self.actions.right_arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="right_robot", + joint_names=[ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ], + body_name="gripper_base", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + scale=1.0, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.2]), + ) + + # [Optional] Gripper control + self.actions.left_gripper_action = mdp.BinaryJointVelocityActionCfg( + asset_name="left_robot", + joint_names=["finger_joint"], + open_command_expr={"finger_joint": 0.1}, # rad/s, opening direction + close_command_expr={"finger_joint": -0.1}, # rad/s, closing direction — tune this value + ) + + self.actions.right_gripper_action = mdp.BinaryJointVelocityActionCfg( + asset_name="right_robot", + joint_names=["finger_joint"], + open_command_expr={"finger_joint": 0.1}, # rad/s, opening direction + close_command_expr={"finger_joint": -0.1}, # rad/s, closing direction — tune this value + ) + + + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.02, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "spacemouse": Se3SpaceMouseCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "motion_controllers": OpenXRDeviceCfg( + retargeters=[ + DummyRetargeterCfg( + enable_visualization=True, + sim_device=self.sim.device, + hand_joint_names=self.actions.left_arm_action.joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg= XrCfg( + anchor_pos = (1.5, 0.4, -0.2), + anchor_rot = (0, 0.7071, 0, 0.7071) + ), + ), + } + ) diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/flexiv_rizon_robots.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/flexiv_rizon_robots.py new file mode 100644 index 000000000..e4ee28450 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/flexiv_rizon_robots.py @@ -0,0 +1,79 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Configuration for the Universal Robots. + +The following configuration parameters are available: + +* :obj:`UR10_LEFT_CFG`: The UR10 arm without a gripper (left side). +* :obj:`UR10_RIGHT_CFG`: The UR10 arm without a gripper (right side). + +Reference: https://github.com/ros-industrial/universal_robot +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +ASSET_URL = "https://github.com/flexivrobotics/isaac_sim_ws/raw/refs/heads/main/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4s_with_Grav.usd" +# ASSET_URL = "/home/linfan/Downloads/Rizon4s_with_Grav_without_gripper.usd" + +FLEXIV_RIZON_4S_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=ASSET_URL, + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.0), + rot=(0.0, 0.0, 0.0, 1.0), + joint_pos={ + "joint1": 0.0, + "joint2": 0.0, + "joint3": 0.0, + "joint4": 0.0, + "joint5": 0.0, + "joint6": 0.0, + "joint7": 0.0, + }, + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[ + "joint1", + "joint2", + "joint3", + "joint4", + "joint5", + "joint6", + "joint7", + ], + effort_limit_sim=4000.0, + stiffness=40000.0, + damping=400.0, + velocity_limit=2.0, + ), + "finger_joint": ImplicitActuatorCfg( + joint_names_expr=[ + "finger_joint", + "right_outer_knuckle_joint", + "left_inner_knuckle_joint", + "right_inner_knuckle_joint", + "left_outer_finger_joint", + "right_outer_finger_joint", + # "left_finguer_tip_joint", + # "right_finguer_tip_joint", + ], + stiffness=0.0, + damping=5.0, + velocity_limit=0.5, # rad/s — hard cap on closing speed + ), + }, +) + diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/__init__.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/differential_ik.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/differential_ik.py new file mode 100644 index 000000000..35843220f --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/differential_ik.py @@ -0,0 +1,252 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.utils.math import apply_delta_pose, compute_pose_error + +if TYPE_CHECKING: + from .differential_ik_cfg import FlexivRizonDifferentialIKControllerCfg + + +class FlexivRizonDifferentialIKController: + r"""Differential inverse kinematics (IK) controller. + + This controller is based on the concept of differential inverse kinematics [1, 2] which is a method for computing + the change in joint positions that yields the desired change in pose. + + .. math:: + + \Delta \mathbf{q} &= \mathbf{J}^{\dagger} \Delta \mathbf{x} \\ + \mathbf{q}_{\text{desired}} &= \mathbf{q}_{\text{current}} + \Delta \mathbf{q} + + where :math:`\mathbf{J}^{\dagger}` is the pseudo-inverse of the Jacobian matrix :math:`\mathbf{J}`, + :math:`\Delta \mathbf{x}` is the desired change in pose, and :math:`\mathbf{q}_{\text{current}}` + is the current joint positions. + + To deal with singularity in Jacobian, the following methods are supported for computing inverse of the Jacobian: + + - "pinv": Moore-Penrose pseudo-inverse + - "svd": Adaptive singular-value decomposition (SVD) + - "trans": Transpose of matrix + - "dls": Damped version of Moore-Penrose pseudo-inverse (also called Levenberg-Marquardt) + + + .. caution:: + The controller does not assume anything about the frames of the current and desired end-effector pose, + or the joint-space velocities. It is up to the user to ensure that these quantities are given + in the correct format. + + Reference: + + 1. `Robot Dynamics Lecture Notes `_ + by Marco Hutter (ETH Zurich) + 2. `Introduction to Inverse Kinematics `_ + by Samuel R. Buss (University of California, San Diego) + + """ + + def __init__(self, cfg: FlexivRizonDifferentialIKControllerCfg, num_envs: int, device: str): + """Initialize the controller. + + Args: + cfg: The configuration for the controller. + num_envs: The number of environments. + device: The device to use for computations. + """ + # store inputs + self.cfg = cfg + self.num_envs = num_envs + self._device = device + # create buffers + self.ee_pos_des = torch.zeros(self.num_envs, 3, device=self._device) + self.ee_quat_des = torch.zeros(self.num_envs, 4, device=self._device) + # -- input command + self._command = torch.zeros(self.num_envs, self.action_dim, device=self._device) + + # import ipdb; ipdb.set_trace() + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + """Dimension of the controller's input command.""" + if self.cfg.command_type == "position": + return 3 # (x, y, z) + elif self.cfg.command_type == "pose" and self.cfg.use_relative_mode: + return 6 # (dx, dy, dz, droll, dpitch, dyaw) + else: + return 7 # (x, y, z, qw, qx, qy, qz) + + """ + Operations. + """ + + def reset(self, env_ids: torch.Tensor = None): + """Reset the internals. + + Args: + env_ids: The environment indices to reset. If None, then all environments are reset. + """ + pass + + def set_command( + self, command: torch.Tensor, ee_pos: torch.Tensor | None = None, ee_quat: torch.Tensor | None = None + ): + """Set target end-effector pose command. + + Based on the configured command type and relative mode, the method computes the desired end-effector pose. + It is up to the user to ensure that the command is given in the correct frame. The method only + applies the relative mode if the command type is ``position_rel`` or ``pose_rel``. + + Args: + command: The input command in shape (N, 3) or (N, 6) or (N, 7). + ee_pos: The current end-effector position in shape (N, 3). + This is only needed if the command type is ``position_rel`` or ``pose_rel``. + ee_quat: The current end-effector orientation (w, x, y, z) in shape (N, 4). + This is only needed if the command type is ``position_*`` or ``pose_rel``. + + Raises: + ValueError: If the command type is ``position_*`` and :attr:`ee_quat` is None. + ValueError: If the command type is ``position_rel`` and :attr:`ee_pos` is None. + ValueError: If the command type is ``pose_rel`` and either :attr:`ee_pos` or :attr:`ee_quat` is None. + """ + # store command + self._command[:] = command + # compute the desired end-effector pose + if self.cfg.command_type == "position": + # we need end-effector orientation even though we are in position mode + # this is only needed for display purposes + if ee_quat is None: + raise ValueError("End-effector orientation can not be None for `position_*` command type!") + # compute targets + if self.cfg.use_relative_mode: + if ee_pos is None: + raise ValueError("End-effector position can not be None for `position_rel` command type!") + self.ee_pos_des[:] = ee_pos + self._command + self.ee_quat_des[:] = ee_quat + else: + self.ee_pos_des[:] = self._command + self.ee_quat_des[:] = ee_quat + else: + # compute targets + if self.cfg.use_relative_mode: + if ee_pos is None or ee_quat is None: + raise ValueError( + "Neither end-effector position nor orientation can be None for `pose_rel` command type!" + ) + self.ee_pos_des, self.ee_quat_des = apply_delta_pose(ee_pos, ee_quat, self._command) + else: + self.ee_pos_des = self._command[:, 0:3] + self.ee_quat_des = self._command[:, 3:7] + + def compute( + self, ee_pos: torch.Tensor, ee_quat: torch.Tensor, jacobian: torch.Tensor, joint_pos: torch.Tensor + ) -> torch.Tensor: + """Computes the target joint positions that will yield the desired end effector pose. + + Args: + ee_pos: The current end-effector position in shape (N, 3). + ee_quat: The current end-effector orientation in shape (N, 4). + jacobian: The geometric jacobian matrix in shape (N, 6, num_joints). + joint_pos: The current joint positions in shape (N, num_joints). + + Returns: + The target joint positions commands in shape (N, num_joints). + """ + joint_weights = self.cfg.joint_weights.to(self._device) + if joint_weights is not None: + scale = joint_weights.float().pow(-0.5) # (num_joints,) + jacobian = jacobian * scale[None, None, :] + + # compute the delta in joint-space + if "position" in self.cfg.command_type: + position_error = self.ee_pos_des - ee_pos + jacobian_pos = jacobian[:, 0:3] + delta_joint_pos = self._compute_delta_joint_pos(delta_pose=position_error, jacobian=jacobian_pos) + else: + position_error, axis_angle_error = compute_pose_error( + ee_pos, ee_quat, self.ee_pos_des, self.ee_quat_des, rot_error_type="axis_angle" + ) + pose_error = torch.cat((position_error, axis_angle_error), dim=1) + delta_joint_pos = self._compute_delta_joint_pos(delta_pose=pose_error, jacobian=jacobian) + + if joint_weights is not None: + delta_joint_pos = delta_joint_pos * scale + + # return the desired joint positions + return joint_pos + delta_joint_pos + + """ + Helper functions. + """ + + def _compute_delta_joint_pos(self, delta_pose: torch.Tensor, jacobian: torch.Tensor) -> torch.Tensor: + """Computes the change in joint position that yields the desired change in pose. + + The method uses the Jacobian mapping from joint-space velocities to end-effector velocities + to compute the delta-change in the joint-space that moves the robot closer to a desired + end-effector position. + + Args: + delta_pose: The desired delta pose in shape (N, 3) or (N, 6). + jacobian: The geometric jacobian matrix in shape (N, 3, num_joints) or (N, 6, num_joints). + + Returns: + The desired delta in joint space. Shape is (N, num-jointsß). + """ + if self.cfg.ik_params is None: + raise RuntimeError(f"Inverse-kinematics parameters for method '{self.cfg.ik_method}' is not defined!") + # compute the delta in joint-space + if self.cfg.ik_method == "pinv": # Jacobian pseudo-inverse + # parameters + k_val = self.cfg.ik_params["k_val"] + # computation + jacobian_pinv = torch.linalg.pinv(jacobian) + delta_joint_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_joint_pos = delta_joint_pos.squeeze(-1) + elif self.cfg.ik_method == "svd": # adaptive SVD + # parameters + k_val = self.cfg.ik_params["k_val"] + min_singular_value = self.cfg.ik_params["min_singular_value"] + # computation + # U: 6xd, S: dxd, V: d x num-joint + U, S, Vh = torch.linalg.svd(jacobian) + S_inv = 1.0 / S + S_inv = torch.where(min_singular_value < S, S_inv, torch.zeros_like(S_inv)) + jacobian_pinv = ( + torch.transpose(Vh, dim0=1, dim1=2)[:, :, :6] + @ torch.diag_embed(S_inv) + @ torch.transpose(U, dim0=1, dim1=2) + ) + delta_joint_pos = k_val * jacobian_pinv @ delta_pose.unsqueeze(-1) + delta_joint_pos = delta_joint_pos.squeeze(-1) + elif self.cfg.ik_method == "trans": # Jacobian transpose + # parameters + k_val = self.cfg.ik_params["k_val"] + # computation + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + delta_joint_pos = k_val * jacobian_T @ delta_pose.unsqueeze(-1) + delta_joint_pos = delta_joint_pos.squeeze(-1) + elif self.cfg.ik_method == "dls": # damped least squares + # parameters + lambda_val = self.cfg.ik_params["lambda_val"] + # computation + jacobian_T = torch.transpose(jacobian, dim0=1, dim1=2) + lambda_matrix = (lambda_val**2) * torch.eye(n=jacobian.shape[1], device=self._device) + delta_joint_pos = ( + jacobian_T @ torch.inverse(jacobian @ jacobian_T + lambda_matrix) @ delta_pose.unsqueeze(-1) + ) + delta_joint_pos = delta_joint_pos.squeeze(-1) + else: + raise ValueError(f"Unsupported inverse-kinematics method: {self.cfg.ik_method}") + + return delta_joint_pos diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/differential_ik_cfg.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/differential_ik_cfg.py new file mode 100644 index 000000000..f1ad211ee --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/differential_ik_cfg.py @@ -0,0 +1,74 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import torch + +from dataclasses import MISSING +from typing import Literal + +from isaaclab.utils import configclass + +from .differential_ik import FlexivRizonDifferentialIKController + + +@configclass +class FlexivRizonDifferentialIKControllerCfg: + """Configuration for differential inverse kinematics controller.""" + + class_type: type = FlexivRizonDifferentialIKController + """The associated controller class.""" + + command_type: Literal["position", "pose"] = MISSING + """Type of task-space command to control the articulation's body. + + If "position", then the controller only controls the position of the articulation's body. + Otherwise, the controller controls the pose of the articulation's body. + """ + + use_relative_mode: bool = False + """Whether to use relative mode for the controller. Defaults to False. + + If True, then the controller treats the input command as a delta change in the position/pose. + Otherwise, the controller treats the input command as the absolute position/pose. + """ + + ik_method: Literal["pinv", "svd", "trans", "dls"] = MISSING + """Method for computing inverse of Jacobian.""" + + ik_params: dict[str, float] | None = None + """Parameters for the inverse-kinematics method. Defaults to None, in which case the default + parameters for the method are used. + + - Moore-Penrose pseudo-inverse ("pinv"): + - "k_val": Scaling of computed delta-joint positions (default: 1.0). + - Adaptive Singular Value Decomposition ("svd"): + - "k_val": Scaling of computed delta-joint positions (default: 1.0). + - "min_singular_value": Single values less than this are suppressed to zero (default: 1e-5). + - Jacobian transpose ("trans"): + - "k_val": Scaling of computed delta-joint positions (default: 1.0). + - Damped Moore-Penrose pseudo-inverse ("dls"): + - "lambda_val": Damping coefficient (default: 0.01). + """ + joint_weights: torch.Tensor | None = None + + + def __post_init__(self): + # check valid input + if self.command_type not in ["position", "pose"]: + raise ValueError(f"Unsupported inverse-kinematics command: {self.command_type}.") + if self.ik_method not in ["pinv", "svd", "trans", "dls"]: + raise ValueError(f"Unsupported inverse-kinematics method: {self.ik_method}.") + # default parameters for different inverse kinematics approaches. + default_ik_params = { + "pinv": {"k_val": 1.0}, + "svd": {"k_val": 1.0, "min_singular_value": 1e-5}, + "trans": {"k_val": 1.0}, + "dls": {"lambda_val": 0.01}, + } + # update parameters for IK-method if not provided + ik_params = default_ik_params[self.ik_method].copy() + if self.ik_params is not None: + ik_params.update(self.ik_params) + self.ik_params = ik_params diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/task_space_actions.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/task_space_actions.py new file mode 100644 index 000000000..b6ba928c7 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/flexiv_rizon/ik/task_space_actions.py @@ -0,0 +1,818 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from pxr import UsdPhysics + +import isaaclab.utils.math as math_utils +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.managers.action_manager import ActionTerm +from isaaclab.sensors import ContactSensor, ContactSensorCfg, FrameTransformer, FrameTransformerCfg +from isaaclab.sim.utils import find_matching_prims + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + from isaaclab.envs.utils.io_descriptors import GenericActionIODescriptor + + from . import actions_cfg + +# import logger +logger = logging.getLogger(__name__) + +from dataclasses import MISSING + +from isaaclab.utils import configclass +from isaaclab.managers.action_manager import ActionTerm, ActionTermCfg + +from .differential_ik import FlexivRizonDifferentialIKController +from .differential_ik_cfg import FlexivRizonDifferentialIKControllerCfg + +class FlexivRizonDifferentialInverseKinematicsAction(ActionTerm): + r"""Inverse Kinematics action term. + + This action term performs pre-processing of the raw actions using scaling transformation. + + .. math:: + \text{action} = \text{scaling} \times \text{input action} + \text{joint position} = J^{-} \times \text{action} + + where :math:`\text{scaling}` is the scaling applied to the input action, and :math:`\text{input action}` + is the input action from the user, :math:`J` is the Jacobian over the articulation's actuated joints, + and \text{joint position} is the desired joint position command for the articulation's joints. + """ + + cfg: FlexivRizonDifferentialInverseKinematicsActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _scale: torch.Tensor + """The scaling factor applied to the input action. Shape is (1, action_dim).""" + _clip: torch.Tensor + """The clip applied to the input action.""" + + def __init__(self, cfg: FlexivRizonDifferentialInverseKinematicsActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + + # resolve the joints over which the action term is applied + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + self._num_joints = len(self._joint_ids) + # parse the body index + body_ids, body_names = self._asset.find_bodies(self.cfg.body_name) + if len(body_ids) != 1: + raise ValueError( + f"Expected one match for the body name: {self.cfg.body_name}. Found {len(body_ids)}: {body_names}." + ) + # save only the first body index + self._body_idx = body_ids[0] + self._body_name = body_names[0] + # check if articulation is fixed-base + # if fixed-base then the jacobian for the base is not computed + # this means that number of bodies is one less than the articulation's number of bodies + if self._asset.is_fixed_base: + self._jacobi_body_idx = self._body_idx - 1 + self._jacobi_joint_ids = self._joint_ids + else: + self._jacobi_body_idx = self._body_idx + self._jacobi_joint_ids = [i + 6 for i in self._joint_ids] + + # log info for debugging + logger.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + logger.info( + f"Resolved body name for the action term {self.__class__.__name__}: {self._body_name} [{self._body_idx}]" + ) + # Avoid indexing across all joints for efficiency + if self._num_joints == self._asset.num_joints: + self._joint_ids = slice(None) + + # create the differential IK controller + self._ik_controller = FlexivRizonDifferentialIKController( + cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device + ) + + # create tensors for 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) + + # save the scale as tensors + self._scale = torch.zeros((self.num_envs, self.action_dim), device=self.device) + self._scale[:] = torch.tensor(self.cfg.scale, device=self.device) + + # convert the fixed offsets to torch tensors of batched shape + if self.cfg.body_offset is not None: + self._offset_pos = torch.tensor(self.cfg.body_offset.pos, device=self.device).repeat(self.num_envs, 1) + self._offset_rot = torch.tensor(self.cfg.body_offset.rot, device=self.device).repeat(self.num_envs, 1) + else: + self._offset_pos, self._offset_rot = None, None + + # parse clip + if self.cfg.clip is not None: + if isinstance(cfg.clip, dict): + self._clip = torch.tensor([[-float("inf"), float("inf")]], device=self.device).repeat( + self.num_envs, self.action_dim, 1 + ) + index_list, _, value_list = string_utils.resolve_matching_names_values(self.cfg.clip, self._joint_names) + self._clip[:, index_list] = torch.tensor(value_list, device=self.device) + else: + raise ValueError(f"Unsupported clip type: {type(cfg.clip)}. Supported types are dict.") + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + return self._ik_controller.action_dim + + @property + def raw_actions(self) -> torch.Tensor: + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + return self._processed_actions + + @property + def jacobian_w(self) -> torch.Tensor: + return self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._jacobi_joint_ids] + + @property + def jacobian_b(self) -> torch.Tensor: + jacobian = self.jacobian_w + base_rot = self._asset.data.root_quat_w + base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot)) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + return jacobian + + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the pink inverse kinematics action. + It adds the following information to the base descriptor: + - body_name: The name of the body. + - joint_names: The names of the joints. + - scale: The scale of the action term. + - clip: The clip of the action term. + - controller_cfg: The configuration of the controller. + - body_offset: The offset of the body. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "TaskSpaceAction" + self._IO_descriptor.body_name = self._body_name + self._IO_descriptor.joint_names = self._joint_names + self._IO_descriptor.scale = self._scale + if self.cfg.clip is not None: + self._IO_descriptor.clip = self.cfg.clip + else: + self._IO_descriptor.clip = None + self._IO_descriptor.extras["controller_cfg"] = self.cfg.controller.__dict__ + self._IO_descriptor.extras["body_offset"] = self.cfg.body_offset.__dict__ + return self._IO_descriptor + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + # store the raw actions + self._raw_actions[:] = actions + self._processed_actions[:] = self.raw_actions * self._scale + if self.cfg.clip is not None: + self._processed_actions = torch.clamp( + self._processed_actions, min=self._clip[:, :, 0], max=self._clip[:, :, 1] + ) + # obtain quantities from simulation + ee_pos_curr, ee_quat_curr = self._compute_frame_pose() + # set command into controller + self._ik_controller.set_command(self._processed_actions, ee_pos_curr, ee_quat_curr) + + def apply_actions(self): + # obtain quantities from simulation + ee_pos_curr, ee_quat_curr = self._compute_frame_pose() + joint_pos = self._asset.data.joint_pos[:, self._joint_ids] + # compute the delta in joint-space + if ee_quat_curr.norm() != 0: + jacobian = self._compute_frame_jacobian() + joint_pos_des = self._ik_controller.compute(ee_pos_curr, ee_quat_curr, jacobian, joint_pos) + else: + joint_pos_des = joint_pos.clone() + # set the joint position command + self._asset.set_joint_position_target(joint_pos_des, self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + self._raw_actions[env_ids] = 0.0 + + """ + Helper functions. + """ + + def _compute_frame_pose(self) -> tuple[torch.Tensor, torch.Tensor]: + """Computes the pose of the target frame in the root frame. + + Returns: + A tuple of the body's position and orientation in the root frame. + """ + # obtain quantities from simulation + ee_pos_w = self._asset.data.body_pos_w[:, self._body_idx] + ee_quat_w = self._asset.data.body_quat_w[:, self._body_idx] + root_pos_w = self._asset.data.root_pos_w + root_quat_w = self._asset.data.root_quat_w + # compute the pose of the body in the root frame + ee_pose_b, ee_quat_b = math_utils.subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w) + # account for the offset + if self.cfg.body_offset is not None: + ee_pose_b, ee_quat_b = math_utils.combine_frame_transforms( + ee_pose_b, ee_quat_b, self._offset_pos, self._offset_rot + ) + + return ee_pose_b, ee_quat_b + + def _compute_frame_jacobian(self): + """Computes the geometric Jacobian of the target frame in the root frame. + + This function accounts for the target frame offset and applies the necessary transformations to obtain + the right Jacobian from the parent body Jacobian. + """ + # read the parent jacobian + jacobian = self.jacobian_b + # account for the offset + if self.cfg.body_offset is not None: + # Modify the jacobian to account for the offset + # -- translational part + # v_link = v_ee + w_ee x r_link_ee = v_J_ee * q + w_J_ee * q x r_link_ee + # = (v_J_ee + w_J_ee x r_link_ee ) * q + # = (v_J_ee - r_link_ee_[x] @ w_J_ee) * q + jacobian[:, 0:3, :] += torch.bmm(-math_utils.skew_symmetric_matrix(self._offset_pos), jacobian[:, 3:, :]) + # -- rotational part + # w_link = R_link_ee @ w_ee + jacobian[:, 3:, :] = torch.bmm(math_utils.matrix_from_quat(self._offset_rot), jacobian[:, 3:, :]) + + return jacobian + + +@configclass +class FlexivRizonDifferentialInverseKinematicsActionCfg(ActionTermCfg): + """Configuration for inverse differential kinematics action term. + + See :class:`FlexivRizonDifferentialInverseKinematicsAction` for more details. + """ + + @configclass + class OffsetCfg: + """The offset pose from parent frame to child frame.""" + + pos: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0).""" + rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0) + """Quaternion rotation ``(w, x, y, z)`` w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0).""" + + class_type: type[ActionTerm] = FlexivRizonDifferentialInverseKinematicsAction + + joint_names: list[str] = MISSING + """List of joint names or regex expressions that the action will be mapped to.""" + body_name: str = MISSING + """Name of the body or frame for which IK is performed.""" + body_offset: OffsetCfg | None = None + """Offset of target frame w.r.t. to the body frame. Defaults to None, in which case no offset is applied.""" + scale: float | tuple[float, ...] = 1.0 + """Scale factor for the action. Defaults to 1.0.""" + controller: FlexivRizonDifferentialIKControllerCfg = MISSING + """The configuration for the differential IK controller.""" + + +class OperationalSpaceControllerAction(ActionTerm): + r"""Operational space controller action term. + + This action term performs pre-processing of the raw actions for operational space control. + + """ + + cfg: actions_cfg.OperationalSpaceControllerActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _contact_sensor: ContactSensor = None + """The contact sensor for the end-effector body.""" + _task_frame_transformer: FrameTransformer = None + """The frame transformer for the task frame.""" + + def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + + self._sim_dt = env.sim.get_physics_dt() + + # resolve the joints over which the action term is applied + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + self._num_DoF = len(self._joint_ids) + # parse the ee body index + body_ids, body_names = self._asset.find_bodies(self.cfg.body_name) + if len(body_ids) != 1: + raise ValueError( + f"Expected one match for the ee body name: {self.cfg.body_name}. Found {len(body_ids)}: {body_names}." + ) + # save only the first ee body index + self._ee_body_idx = body_ids[0] + self._ee_body_name = body_names[0] + # check if articulation is fixed-base + # if fixed-base then the jacobian for the base is not computed + # this means that number of bodies is one less than the articulation's number of bodies + if self._asset.is_fixed_base: + self._jacobi_ee_body_idx = self._ee_body_idx - 1 + self._jacobi_joint_idx = self._joint_ids + else: + self._jacobi_ee_body_idx = self._ee_body_idx + self._jacobi_joint_idx = [i + 6 for i in self._joint_ids] + + # log info for debugging + logger.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + logger.info( + f"Resolved ee body name for the action term {self.__class__.__name__}:" + f" {self._ee_body_name} [{self._ee_body_idx}]" + ) + # Avoid indexing across all joints for efficiency + if self._num_DoF == self._asset.num_joints: + self._joint_ids = slice(None) + + # convert the fixed offsets to torch tensors of batched shape + if self.cfg.body_offset is not None: + self._offset_pos = torch.tensor(self.cfg.body_offset.pos, device=self.device).repeat(self.num_envs, 1) + self._offset_rot = torch.tensor(self.cfg.body_offset.rot, device=self.device).repeat(self.num_envs, 1) + else: + self._offset_pos, self._offset_rot = None, None + + # create contact sensor if any of the command is wrench_abs, and if stiffness is provided + if ( + "wrench_abs" in self.cfg.controller_cfg.target_types + and self.cfg.controller_cfg.contact_wrench_stiffness_task is not None + ): + self._contact_sensor_cfg = ContactSensorCfg(prim_path=self._asset.cfg.prim_path + "/" + self._ee_body_name) + self._contact_sensor = ContactSensor(self._contact_sensor_cfg) + if not self._contact_sensor.is_initialized: + self._contact_sensor._initialize_impl() + self._contact_sensor._is_initialized = True + + # Initialize the task frame transformer if a relative path for the RigidObject, representing the task frame, + # is provided. + if self.cfg.task_frame_rel_path is not None: + # The source RigidObject can be any child of the articulation asset (we will not use it), + # hence, we will use the first RigidObject child. + root_rigidbody_path = self._first_RigidObject_child_path() + task_frame_transformer_path = "/World/envs/env_.*/" + self.cfg.task_frame_rel_path + task_frame_transformer_cfg = FrameTransformerCfg( + prim_path=root_rigidbody_path, + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="task_frame", + prim_path=task_frame_transformer_path, + ), + ], + ) + self._task_frame_transformer = FrameTransformer(task_frame_transformer_cfg) + 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 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 + self._task_frame_pose_b = None + + # create the operational space controller + self._osc = OperationalSpaceController(cfg=self.cfg.controller_cfg, num_envs=self.num_envs, device=self.device) + + # create tensors for 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) + + # create tensors for the dynamic-related quantities + self._jacobian_b = torch.zeros(self.num_envs, 6, self._num_DoF, device=self.device) + self._mass_matrix = torch.zeros(self.num_envs, self._num_DoF, self._num_DoF, device=self.device) + self._gravity = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + + # create tensors for the ee states + self._ee_pose_w = torch.zeros(self.num_envs, 7, device=self.device) + self._ee_pose_b = torch.zeros(self.num_envs, 7, device=self.device) + self._ee_pose_b_no_offset = torch.zeros(self.num_envs, 7, device=self.device) # The original ee without offset + self._ee_vel_w = torch.zeros(self.num_envs, 6, device=self.device) + self._ee_vel_b = torch.zeros(self.num_envs, 6, device=self.device) + self._ee_force_w = torch.zeros(self.num_envs, 3, device=self.device) # Only the forces are used for now + self._ee_force_b = torch.zeros(self.num_envs, 3, device=self.device) # Only the forces are used for now + + # create tensors for the joint states + self._joint_pos = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + self._joint_vel = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + + # create the joint effort tensor + self._joint_efforts = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + + # save the scale as tensors + self._position_scale = torch.tensor(self.cfg.position_scale, device=self.device) + self._orientation_scale = torch.tensor(self.cfg.orientation_scale, device=self.device) + self._wrench_scale = torch.tensor(self.cfg.wrench_scale, device=self.device) + self._stiffness_scale = torch.tensor(self.cfg.stiffness_scale, device=self.device) + self._damping_ratio_scale = torch.tensor(self.cfg.damping_ratio_scale, device=self.device) + + # indexes for the various command elements (e.g., pose_rel, stifness, etc.) within the command tensor + self._pose_abs_idx = None + self._pose_rel_idx = None + self._wrench_abs_idx = None + self._stiffness_idx = None + self._damping_ratio_idx = None + self._resolve_command_indexes() + + # Nullspace position control joint targets + self._nullspace_joint_pos_target = None + self._resolve_nullspace_joint_pos_targets() + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + """Dimension of the action space of operational space control.""" + return self._osc.action_dim + + @property + def raw_actions(self) -> torch.Tensor: + """Raw actions for operational space control.""" + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + """Processed actions for operational space control.""" + return self._processed_actions + + @property + def jacobian_w(self) -> torch.Tensor: + return self._asset.root_physx_view.get_jacobians()[:, self._jacobi_ee_body_idx, :, self._jacobi_joint_idx] + + @property + def jacobian_b(self) -> torch.Tensor: + jacobian = self.jacobian_w + base_rot = self._asset.data.root_quat_w + base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot)) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + return jacobian + + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term. + + This descriptor is used to describe the action term of the pink inverse kinematics action. + It adds the following information to the base descriptor: + - body_name: The name of the body. + - joint_names: The names of the joints. + - position_scale: The scale of the position. + - orientation_scale: The scale of the orientation. + - wrench_scale: The scale of the wrench. + - stiffness_scale: The scale of the stiffness. + - damping_ratio_scale: The scale of the damping ratio. + - nullspace_joint_pos_target: The nullspace joint pos target. + - clip: The clip of the action term. + - controller_cfg: The configuration of the controller. + - body_offset: The offset of the body. + + Returns: + The IO descriptor of the action term. + """ + super().IO_descriptor + self._IO_descriptor.shape = (self.action_dim,) + self._IO_descriptor.dtype = str(self.raw_actions.dtype) + self._IO_descriptor.action_type = "TaskSpaceAction" + self._IO_descriptor.body_name = self._ee_body_name + self._IO_descriptor.joint_names = self._joint_names + self._IO_descriptor.position_scale = self.cfg.position_scale + self._IO_descriptor.orientation_scale = self.cfg.orientation_scale + self._IO_descriptor.wrench_scale = self.cfg.wrench_scale + self._IO_descriptor.stiffness_scale = self.cfg.stiffness_scale + self._IO_descriptor.damping_ratio_scale = self.cfg.damping_ratio_scale + self._IO_descriptor.nullspace_joint_pos_target = self.cfg.nullspace_joint_pos_target + if self.cfg.clip is not None: + self._IO_descriptor.clip = self.cfg.clip + else: + self._IO_descriptor.clip = None + self._IO_descriptor.extras["controller_cfg"] = self.cfg.controller_cfg.__dict__ + self._IO_descriptor.extras["body_offset"] = self.cfg.body_offset.__dict__ + return self._IO_descriptor + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + """Pre-processes the raw actions and sets them as commands for for operational space control. + + Args: + actions (torch.Tensor): The raw actions for operational space control. It is a tensor of + shape (``num_envs``, ``action_dim``). + """ + + # Update ee pose, which would be used by relative targets (i.e., pose_rel) + self._compute_ee_pose() + + # Update task frame pose w.r.t. the root frame. + self._compute_task_frame_pose() + + # Pre-process the raw actions for operational space control. + self._preprocess_actions(actions) + + # set command into controller + self._osc.set_command( + command=self._processed_actions, + current_ee_pose_b=self._ee_pose_b, + current_task_frame_pose_b=self._task_frame_pose_b, + ) + + def apply_actions(self): + """Computes the joint efforts for operational space control and applies them to the articulation.""" + + # Update the relevant states and dynamical quantities + self._compute_dynamic_quantities() + self._compute_ee_jacobian() + self._compute_ee_pose() + self._compute_ee_velocity() + self._compute_ee_force() + self._compute_joint_states() + # Calculate the joint efforts + self._joint_efforts[:] = self._osc.compute( + jacobian_b=self._jacobian_b, + current_ee_pose_b=self._ee_pose_b, + current_ee_vel_b=self._ee_vel_b, + current_ee_force_b=self._ee_force_b, + mass_matrix=self._mass_matrix, + gravity=self._gravity, + current_joint_pos=self._joint_pos, + current_joint_vel=self._joint_vel, + nullspace_joint_pos_target=self._nullspace_joint_pos_target, + ) + self._asset.set_joint_effort_target(self._joint_efforts, joint_ids=self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Resets the raw actions and the sensors if available. + + Args: + env_ids (Sequence[int] | None): The environment indices to reset. If ``None``, all environments are reset. + """ + self._raw_actions[env_ids] = 0.0 + if self._contact_sensor is not None: + self._contact_sensor.reset(env_ids) + if self._task_frame_transformer is not None: + self._task_frame_transformer.reset(env_ids) + + """ + Helper functions. + + """ + + def _first_RigidObject_child_path(self): + """Finds the first ``RigidObject`` child under the articulation asset. + + Raises: + ValueError: If no child ``RigidObject`` is found under the articulation asset. + + Returns: + str: The path to the first ``RigidObject`` child under the articulation asset. + """ + child_prims = find_matching_prims(self._asset.cfg.prim_path + "/.*") + rigid_child_prim = None + # Loop through the list and stop at the first RigidObject found + for prim in child_prims: + if prim.HasAPI(UsdPhysics.RigidBodyAPI): + rigid_child_prim = prim + break + if rigid_child_prim is None: + raise ValueError("No child rigid body found under the expression: '{self._asset.cfg.prim_path}'/.") + rigid_child_prim_path = rigid_child_prim.GetPath().pathString + # Remove the specific env index from the path string + rigid_child_prim_path = self._asset.cfg.prim_path + "/" + rigid_child_prim_path.split("/")[-1] + return rigid_child_prim_path + + def _resolve_command_indexes(self): + """Resolves the indexes for the various command elements within the command tensor. + + Raises: + ValueError: If any command index is left unresolved. + """ + # First iterate over the target types to find the indexes of the different command elements + cmd_idx = 0 + for target_type in self.cfg.controller_cfg.target_types: + if target_type == "pose_abs": + self._pose_abs_idx = cmd_idx + cmd_idx += 7 + elif target_type == "pose_rel": + self._pose_rel_idx = cmd_idx + cmd_idx += 6 + elif target_type == "wrench_abs": + self._wrench_abs_idx = cmd_idx + cmd_idx += 6 + else: + raise ValueError("Undefined target_type for OSC within OperationalSpaceControllerAction.") + # Then iterate over the impedance parameters depending on the impedance mode + if ( + self.cfg.controller_cfg.impedance_mode == "variable_kp" + or self.cfg.controller_cfg.impedance_mode == "variable" + ): + self._stiffness_idx = cmd_idx + cmd_idx += 6 + if self.cfg.controller_cfg.impedance_mode == "variable": + self._damping_ratio_idx = cmd_idx + cmd_idx += 6 + + # Check if any command is left unresolved + if self.action_dim != cmd_idx: + raise ValueError("Not all command indexes have been resolved.") + + def _resolve_nullspace_joint_pos_targets(self): + """Resolves the nullspace joint pos targets for the operational space controller. + + Raises: + ValueError: If the nullspace joint pos targets are set when null space control is not set to 'position'. + ValueError: If the nullspace joint pos targets are not set when null space control is set to 'position'. + ValueError: If an invalid value is set for nullspace joint pos targets. + """ + + if self.cfg.nullspace_joint_pos_target != "none" and self.cfg.controller_cfg.nullspace_control != "position": + raise ValueError("Nullspace joint targets can only be set when null space control is set to 'position'.") + + if self.cfg.nullspace_joint_pos_target == "none" and self.cfg.controller_cfg.nullspace_control == "position": + raise ValueError("Nullspace joint targets must be set when null space control is set to 'position'.") + + if self.cfg.nullspace_joint_pos_target == "zero" or self.cfg.nullspace_joint_pos_target == "none": + # Keep the nullspace joint targets as None as this is later processed as zero in the controller + self._nullspace_joint_pos_target = None + elif self.cfg.nullspace_joint_pos_target == "center": + # Get the center of the robot soft joint limits + self._nullspace_joint_pos_target = torch.mean( + self._asset.data.soft_joint_pos_limits[:, self._joint_ids, :], dim=-1 + ) + elif self.cfg.nullspace_joint_pos_target == "default": + # Get the default joint positions + self._nullspace_joint_pos_target = self._asset.data.default_joint_pos[:, self._joint_ids] + else: + raise ValueError("Invalid value for nullspace joint pos targets.") + + def _compute_dynamic_quantities(self): + """Computes the dynamic quantities for operational space control.""" + + self._mass_matrix[:] = self._asset.root_physx_view.get_generalized_mass_matrices()[:, self._joint_ids, :][ + :, :, self._joint_ids + ] + self._gravity[:] = self._asset.root_physx_view.get_gravity_compensation_forces()[:, self._joint_ids] + + def _compute_ee_jacobian(self): + """Computes the geometric Jacobian of the ee body frame in root frame. + + This function accounts for the target frame offset and applies the necessary transformations to obtain + the right Jacobian from the parent body Jacobian. + """ + # Get the Jacobian in root frame + self._jacobian_b[:] = self.jacobian_b + + # account for the offset + if self.cfg.body_offset is not None: + # Modify the jacobian to account for the offset + # -- translational part + # v_link = v_ee + w_ee x r_link_ee = v_J_ee * q + w_J_ee * q x r_link_ee + # = (v_J_ee + w_J_ee x r_link_ee ) * q + # = (v_J_ee - r_link_ee_[x] @ w_J_ee) * q + self._jacobian_b[:, 0:3, :] += torch.bmm( + -math_utils.skew_symmetric_matrix(self._offset_pos), self._jacobian_b[:, 3:, :] + ) # type: ignore + # -- rotational part + # w_link = R_link_ee @ w_ee + self._jacobian_b[:, 3:, :] = torch.bmm( + math_utils.matrix_from_quat(self._offset_rot), self._jacobian_b[:, 3:, :] + ) # type: ignore + + def _compute_ee_pose(self): + """Computes the pose of the ee frame in root frame.""" + # Obtain quantities from simulation + self._ee_pose_w[:, 0:3] = self._asset.data.body_pos_w[:, self._ee_body_idx] + self._ee_pose_w[:, 3:7] = self._asset.data.body_quat_w[:, self._ee_body_idx] + # Compute the pose of the ee body in the root frame + self._ee_pose_b_no_offset[:, 0:3], self._ee_pose_b_no_offset[:, 3:7] = math_utils.subtract_frame_transforms( + self._asset.data.root_pos_w, + self._asset.data.root_quat_w, + self._ee_pose_w[:, 0:3], + self._ee_pose_w[:, 3:7], + ) + # Account for the offset + if self.cfg.body_offset is not None: + self._ee_pose_b[:, 0:3], self._ee_pose_b[:, 3:7] = math_utils.combine_frame_transforms( + self._ee_pose_b_no_offset[:, 0:3], self._ee_pose_b_no_offset[:, 3:7], self._offset_pos, self._offset_rot + ) + else: + self._ee_pose_b[:] = self._ee_pose_b_no_offset + + def _compute_ee_velocity(self): + """Computes the velocity of the ee frame in root frame.""" + # Extract end-effector velocity in the world frame + self._ee_vel_w[:] = self._asset.data.body_vel_w[:, self._ee_body_idx, :] + # Compute the relative velocity in the world frame + relative_vel_w = self._ee_vel_w - self._asset.data.root_vel_w + + # Convert ee velocities from world to root frame + self._ee_vel_b[:, 0:3] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 0:3]) + self._ee_vel_b[:, 3:6] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 3:6]) + + # Account for the offset + if self.cfg.body_offset is not None: + # Compute offset vector in root frame + r_offset_b = math_utils.quat_apply(self._ee_pose_b_no_offset[:, 3:7], self._offset_pos) + # Adjust the linear velocity to account for the offset + self._ee_vel_b[:, :3] += torch.cross(self._ee_vel_b[:, 3:], r_offset_b, dim=-1) + # Angular velocity is not affected by the offset + + def _compute_ee_force(self): + """Computes the contact forces on the ee frame in root frame.""" + # Obtain contact forces only if the contact sensor is available + if self._contact_sensor is not None: + self._contact_sensor.update(self._sim_dt) + self._ee_force_w[:] = self._contact_sensor.data.net_forces_w[:, 0, :] # type: ignore + # Rotate forces and torques into root frame + self._ee_force_b[:] = math_utils.quat_apply_inverse(self._asset.data.root_quat_w, self._ee_force_w) + + def _compute_joint_states(self): + """Computes the joint states for operational space control.""" + # Extract joint positions and velocities + self._joint_pos[:] = self._asset.data.joint_pos[:, self._joint_ids] + self._joint_vel[:] = self._asset.data.joint_vel[:, self._joint_ids] + + def _compute_task_frame_pose(self): + """Computes the pose of the task frame in root frame.""" + # Update task frame pose if task frame rigidbody is provided + if self._task_frame_transformer is not None and self._task_frame_pose_b is not None: + self._task_frame_transformer.update(self._sim_dt) + # Calculate the pose of the task frame in the root frame + self._task_frame_pose_b[:, :3], self._task_frame_pose_b[:, 3:] = math_utils.subtract_frame_transforms( + self._asset.data.root_pos_w, + self._asset.data.root_quat_w, + self._task_frame_transformer.data.target_pos_w[:, 0, :], + self._task_frame_transformer.data.target_quat_w[:, 0, :], + ) + + def _preprocess_actions(self, actions: torch.Tensor): + """Pre-processes the raw actions for operational space control. + + Args: + actions (torch.Tensor): The raw actions for operational space control. It is a tensor of + shape (``num_envs``, ``action_dim``). + """ + # Store the raw actions. Please note that the actions contain task space targets + # (in the order of the target_types), and possibly the impedance parameters depending on impedance_mode. + self._raw_actions[:] = actions + # Initialize the processed actions with raw actions. + self._processed_actions[:] = self._raw_actions + # Go through the command types one by one, and apply the pre-processing if needed. + if self._pose_abs_idx is not None: + self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] *= self._position_scale + self._processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] *= self._orientation_scale + if self._pose_rel_idx is not None: + self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] *= self._position_scale + self._processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] *= self._orientation_scale + if self._wrench_abs_idx is not None: + self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] *= self._wrench_scale + if self._stiffness_idx is not None: + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6] *= self._stiffness_scale + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6] = torch.clamp( + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6], + min=self.cfg.controller_cfg.motion_stiffness_limits_task[0], + max=self.cfg.controller_cfg.motion_stiffness_limits_task[1], + ) + if self._damping_ratio_idx is not None: + self._processed_actions[:, self._damping_ratio_idx : self._damping_ratio_idx + 6] *= ( + self._damping_ratio_scale + ) + self._processed_actions[:, self._damping_ratio_idx : self._damping_ratio_idx + 6] = torch.clamp( + self._processed_actions[:, self._damping_ratio_idx : self._damping_ratio_idx + 6], + min=self.cfg.controller_cfg.motion_damping_ratio_limits_task[0], + max=self.cfg.controller_cfg.motion_damping_ratio_limits_task[1], + ) diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/__init__.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/__init__.py new file mode 100644 index 000000000..81ee798f2 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/__init__.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +gym.register( + id="Template-UR10-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.ur10_bimanual_env:UR10BimanualEnvCfg", + }, +) diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/universal_robots.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/universal_robots.py new file mode 100644 index 000000000..9acd49f1a --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/universal_robots.py @@ -0,0 +1,91 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +"""Configuration for the Universal Robots. + +The following configuration parameters are available: + +* :obj:`UR10_LEFT_CFG`: The UR10 arm without a gripper (left side). +* :obj:`UR10_RIGHT_CFG`: The UR10 arm without a gripper (right side). + +Reference: https://github.com/ros-industrial/universal_robot +""" + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR + +## +# Configuration +## + +UR10_LEFT_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/UniversalRobots/UR10/ur10_instanceable.usd", + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.28, 0.74, 0.827), + rot=(0.0, 0.0, 0.0, 1.0), + joint_pos={ + "shoulder_pan_joint": 1.5707, + "shoulder_lift_joint": -1.5707, + "elbow_joint": 1.5707, + "wrist_1_joint": -1.5707, + "wrist_2_joint": -1.5707, + "wrist_3_joint": 0.0, + }, + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ], + effort_limit_sim=150.0, + stiffness=800.0, + damping=40.0, + ), + }, +) + + +UR10_RIGHT_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/UniversalRobots/UR10/ur10_instanceable.usd", + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(1.00, 0.74, 0.827), + rot=(0.0, 0.0, 0.0, 1.0), + joint_pos={ + "shoulder_pan_joint": -1.5707, + "shoulder_lift_joint": -1.5707, + "elbow_joint": -1.5707, + "wrist_1_joint": -1.5707, + "wrist_2_joint": 1.5707, + "wrist_3_joint": 0.0, + }, + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=[ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ], + effort_limit_sim=150.0, + stiffness=800.0, + damping=40.0, + ), + }, +) diff --git a/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/ur10_bimanual_env.py b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/ur10_bimanual_env.py new file mode 100644 index 000000000..868ff89a0 --- /dev/null +++ b/examples/isaaclab_bimanual_robot/source/bimanual/tasks/manager_based/ur10/ur10_bimanual_env.py @@ -0,0 +1,132 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import os + +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.manipulation.reach.mdp as mdp + + +from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.keyboard import Se3KeyboardCfg +from isaaclab.devices.spacemouse import Se3SpaceMouseCfg +from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.sensors import CameraCfg +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +import isaaclab.sim as sim_utils + +## +# Pre-defined configs +## + +from bimanual.tasks.manager_based.common.base_env_cfg import BaseEnvCfg +from bimanual.tasks.manager_based.common.dummy_retargeter import DummyRetargeterCfg + +from .universal_robots import UR10_LEFT_CFG, UR10_RIGHT_CFG # isort: skip + + +## +# Environment configuration +## + + +@configclass +class UR10BimanualEnvCfg(BaseEnvCfg): + # Position of the XR anchor in the world frame + xr: XrCfg = XrCfg( + anchor_pos=(0.55, -0.3, -0.15), + anchor_rot=(1, 0, 0, 0), + ) + + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.scene.left_robot = UR10_LEFT_CFG.replace( + prim_path="{ENV_REGEX_NS}/left_robot", + ) + self.scene.right_robot = UR10_RIGHT_CFG.replace( + prim_path="{ENV_REGEX_NS}/right_robot", + ) + + # Replace joint-position arm action with differential IK (relative pose) + # NOTE: body_offset.pos is the TCP offset from ee_link along its z-axis (~130 mm for Robotiq 2F-85). + # Verify against the actual USD if needed. + self.actions.left_arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="left_robot", + joint_names=[ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ], + body_name="wrist_3_link", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + scale=1.0, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.13]), + ) + + self.actions.right_arm_action = DifferentialInverseKinematicsActionCfg( + asset_name="right_robot", + joint_names=[ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", + ], + body_name="wrist_3_link", + controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + scale=1.0, + body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.13]), + ) + + ## [Optional] Gripper control + # self.actions.left_gripper_action = mdp.BinaryJointVelocityActionCfg( + # asset_name="left_robot", + # joint_names=["finger_joint"], + # open_command_expr={"finger_joint": -0.5}, # rad/s, opening direction + # close_command_expr={"finger_joint": 0.5}, # rad/s, closing direction — tune this value + # ) + + # self.actions.right_gripper_action = mdp.BinaryJointVelocityActionCfg( + # asset_name="right_robot", + # joint_names=["finger_joint"], + # open_command_expr={"finger_joint": -0.5}, # rad/s, opening direction + # close_command_expr={"finger_joint": 0.5}, # rad/s, closing direction — tune this value + # ) + + + self.teleop_devices = DevicesCfg( + devices={ + "keyboard": Se3KeyboardCfg( + pos_sensitivity=0.02, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "spacemouse": Se3SpaceMouseCfg( + pos_sensitivity=0.05, + rot_sensitivity=0.05, + sim_device=self.sim.device, + ), + "motion_controllers": OpenXRDeviceCfg( + retargeters=[ + DummyRetargeterCfg( + enable_visualization=True, + sim_device=self.sim.device, + hand_joint_names=self.actions.left_arm_action.joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), + } + )