diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 2ccf5c751fe..9a7f43604fa 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -73,6 +73,7 @@ Guidelines for modifications: * HoJin Jeon * Hongwei Xiong * Hongyu Li +* Hougant Chen * Huihua Zhao * Iretiayo Akinola * Jack Zeng diff --git a/docs/source/api/lab/isaaclab.devices.rst b/docs/source/api/lab/isaaclab.devices.rst index 588b8db8381..5f04c4733cf 100644 --- a/docs/source/api/lab/isaaclab.devices.rst +++ b/docs/source/api/lab/isaaclab.devices.rst @@ -48,11 +48,13 @@ Game Pad :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: Se3Gamepad :members: :inherited-members: :show-inheritance: + :noindex: Keyboard -------- @@ -61,11 +63,13 @@ Keyboard :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: Se3Keyboard :members: :inherited-members: :show-inheritance: + :noindex: Space Mouse ----------- @@ -74,11 +78,13 @@ Space Mouse :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: Se3SpaceMouse :members: :inherited-members: :show-inheritance: + :noindex: Haply ----- @@ -87,6 +93,7 @@ Haply :members: :inherited-members: :show-inheritance: + :noindex: OpenXR ------ @@ -95,6 +102,7 @@ OpenXR :members: :inherited-members: :show-inheritance: + :noindex: Manus + Vive ------------ @@ -103,6 +111,7 @@ Manus + Vive :members: :inherited-members: :show-inheritance: + :noindex: Retargeters ----------- @@ -111,18 +120,22 @@ Retargeters :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: isaaclab.devices.openxr.retargeters.Se3AbsRetargeter :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: isaaclab.devices.openxr.retargeters.Se3RelRetargeter :members: :inherited-members: :show-inheritance: + :noindex: .. autoclass:: isaaclab.devices.openxr.retargeters.GR1T2Retargeter :members: :inherited-members: :show-inheritance: + :noindex: diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst index b21ed817211..d05e1656f40 100644 --- a/docs/source/how-to/cloudxr_teleoperation.rst +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -721,11 +721,11 @@ Here's an example of setting up hand tracking: # Create retargeters position_retargeter = Se3AbsRetargeter( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, zero_out_xy_rotation=True, use_wrist_position=False # Use pinch position (thumb-index midpoint) instead of wrist ) - gripper_retargeter = GripperRetargeter(bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT) + gripper_retargeter = GripperRetargeter(bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT) # Create OpenXR device with hand tracking and both retargeters device = OpenXRDevice( @@ -919,7 +919,7 @@ The retargeting system is designed to be extensible. You can create custom retar Any: The transformed control commands for the robot. """ # Access hand tracking data using TrackingTarget enum - right_hand_data = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + right_hand_data = data[DeviceBase.TrackingTarget.HAND_RIGHT] # Extract specific joint positions and orientations wrist_pose = right_hand_data.get("wrist") @@ -927,7 +927,7 @@ The retargeting system is designed to be extensible. You can create custom retar index_tip_pose = right_hand_data.get("index_tip") # Access head tracking data - head_pose = data[OpenXRDevice.TrackingTarget.HEAD] + head_pose = data[DeviceBase.TrackingTarget.HEAD] # Process the tracking data and apply your custom logic # ... diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index 85f1cbdf2f8..9b28ad24108 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -3,7 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Script to run a keyboard teleoperation with Isaac Lab manipulation environments.""" +"""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.""" @@ -13,7 +17,7 @@ from isaaclab.app import AppLauncher # add argparse arguments -parser = argparse.ArgumentParser(description="Keyboard teleoperation for Isaac Lab environments.") +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", @@ -78,7 +82,7 @@ def main() -> None: """ - Run keyboard teleoperation with Isaac Lab manipulation environment. + 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. @@ -98,8 +102,6 @@ def main() -> None: env_cfg.terminations.object_reached_goal = DoneTerm(func=mdp.object_reached_goal) if args_cli.xr: - # External cameras are not supported with XR teleop - # Check for any camera configs and disable them env_cfg = remove_camera_configs(env_cfg) env_cfg.sim.render.antialiasing_mode = "DLSS" @@ -204,7 +206,7 @@ def stop_teleoperation() -> None: ) else: logger.error(f"Unsupported teleop device: {args_cli.teleop_device}") - logger.error("Supported devices: keyboard, spacemouse, gamepad, handtracking") + logger.error("Configure the teleop device in the environment config.") env.close() simulation_app.close() return @@ -254,6 +256,7 @@ def stop_teleoperation() -> None: if should_reset_recording_instance: env.reset() + teleop_interface.reset() should_reset_recording_instance = False print("Environment reset complete") except Exception as e: diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index e1f7205392a..48a598cd23f 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.48.5" +version = "0.48.6" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 6a5b57f89c2..2e5a44188ce 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,21 @@ Changelog --------- +0.48.6 (2025-11-18) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added OpenXR motion controller support for the G1 robot locomanipulation environment + ``Isaac-PickPlace-Locomanipulation-G1-Abs-v0``. This enables teleoperation using XR motion controllers + in addition to hand tracking. +* Added :class:`OpenXRDeviceMotionController` for motion controller-based teleoperation with headset anchoring control. +* Added motion controller-specific retargeters: + * :class:`G1TriHandControllerUpperBodyRetargeterCfg` for upper body and hand control using motion controllers. + * :class:`G1LowerBodyStandingControllerRetargeterCfg` for lower body control using motion controllers. + + 0.48.5 (2025-11-14) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/devices/device_base.py b/source/isaaclab/isaaclab/devices/device_base.py index 3a851aa6d9d..70e0e391a9a 100644 --- a/source/isaaclab/isaaclab/devices/device_base.py +++ b/source/isaaclab/isaaclab/devices/device_base.py @@ -9,6 +9,7 @@ from abc import ABC, abstractmethod from collections.abc import Callable from dataclasses import dataclass, field +from enum import Enum from typing import Any from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg @@ -58,9 +59,13 @@ def __init__(self, retargeters: list[RetargeterBase] | None = None): """ # Initialize empty list if None is provided self._retargeters = retargeters or [] + # Aggregate required features across all retargeters + self._required_features = set() + for retargeter in self._retargeters: + self._required_features.update(retargeter.get_requirements()) def __str__(self) -> str: - """Returns: A string containing the information of joystick.""" + """Returns: A string identifier for the device.""" return f"{self.__class__.__name__}" """ @@ -123,3 +128,32 @@ def advance(self) -> torch.Tensor: # With multiple retargeters, return a tuple of outputs # Concatenate retargeted outputs into a single tensor return torch.cat([retargeter.retarget(raw_data) for retargeter in self._retargeters], dim=-1) + + # ----------------------------- + # Shared data layout helpers (for retargeters across devices) + # ----------------------------- + class TrackingTarget(Enum): + """Standard tracking targets shared across devices.""" + + HAND_LEFT = 0 + HAND_RIGHT = 1 + HEAD = 2 + CONTROLLER_LEFT = 3 + CONTROLLER_RIGHT = 4 + + class MotionControllerDataRowIndex(Enum): + """Rows in the motion-controller 2x7 array.""" + + POSE = 0 + INPUTS = 1 + + class MotionControllerInputIndex(Enum): + """Indices in the motion-controller input row.""" + + THUMBSTICK_X = 0 + THUMBSTICK_Y = 1 + TRIGGER = 2 + SQUEEZE = 3 + BUTTON_0 = 4 + BUTTON_1 = 5 + PADDING = 6 diff --git a/source/isaaclab/isaaclab/devices/openxr/__init__.py b/source/isaaclab/isaaclab/devices/openxr/__init__.py index e7bc0cfda03..0c187106c7a 100644 --- a/source/isaaclab/isaaclab/devices/openxr/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/__init__.py @@ -7,4 +7,4 @@ from .manus_vive import ManusVive, ManusViveCfg from .openxr_device import OpenXRDevice, OpenXRDeviceCfg -from .xr_cfg import XrCfg, remove_camera_configs +from .xr_cfg import XrAnchorRotationMode, XrCfg, remove_camera_configs diff --git a/source/isaaclab/isaaclab/devices/openxr/manus_vive.py b/source/isaaclab/isaaclab/devices/openxr/manus_vive.py index 0886ccc5a68..ff696bb0d2f 100644 --- a/source/isaaclab/isaaclab/devices/openxr/manus_vive.py +++ b/source/isaaclab/isaaclab/devices/openxr/manus_vive.py @@ -13,7 +13,6 @@ import numpy as np from collections.abc import Callable from dataclasses import dataclass -from enum import Enum import carb from isaacsim.core.version import get_version @@ -22,7 +21,6 @@ from isaaclab.devices.retargeter_base import RetargeterBase from ..device_base import DeviceBase, DeviceCfg -from .openxr_device import OpenXRDevice from .xr_cfg import XrCfg # For testing purposes, we need to mock the XRCore @@ -61,13 +59,6 @@ class ManusVive(DeviceBase): data is transformed into robot control commands suitable for teleoperation. """ - class TrackingTarget(Enum): - """Enum class specifying what to track with Manus+Vive. Consistent with :class:`OpenXRDevice.TrackingTarget`.""" - - HAND_LEFT = 0 - HAND_RIGHT = 1 - HEAD = 2 - TELEOP_COMMAND_EVENT_TYPE = "teleop_command" def __init__(self, cfg: ManusViveCfg, retargeters: list[RetargeterBase] | None = None): @@ -192,9 +183,9 @@ def _get_raw_data(self) -> dict: joint_name = HAND_JOINT_MAP[int(index)] result[hand][joint_name] = np.array(pose["position"] + pose["orientation"], dtype=np.float32) return { - OpenXRDevice.TrackingTarget.HAND_LEFT: result["left"], - OpenXRDevice.TrackingTarget.HAND_RIGHT: result["right"], - OpenXRDevice.TrackingTarget.HEAD: self._calculate_headpose(), + DeviceBase.TrackingTarget.HAND_LEFT: result["left"], + DeviceBase.TrackingTarget.HAND_RIGHT: result["right"], + DeviceBase.TrackingTarget.HEAD: self._calculate_headpose(), } def _calculate_headpose(self) -> np.ndarray: diff --git a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py index bb6f40c4e91..e929d102a5e 100644 --- a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py +++ b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py @@ -4,30 +4,34 @@ # SPDX-License-Identifier: BSD-3-Clause """OpenXR-powered device for teleoperation and interaction.""" - from __future__ import annotations import contextlib +import logging import numpy as np from collections.abc import Callable from dataclasses import dataclass -from enum import Enum from typing import Any import carb +# import logger +logger = logging.getLogger(__name__) + from isaaclab.devices.openxr.common import HAND_JOINT_NAMES from isaaclab.devices.retargeter_base import RetargeterBase from ..device_base import DeviceBase, DeviceCfg +from .xr_anchor_utils import XrAnchorSynchronizer from .xr_cfg import XrCfg # For testing purposes, we need to mock the XRCore, XRPoseValidityFlags classes XRCore = None XRPoseValidityFlags = None +XRCoreEventType = None with contextlib.suppress(ModuleNotFoundError): - from omni.kit.xr.core import XRCore, XRPoseValidityFlags + from omni.kit.xr.core import XRCore, XRPoseValidityFlags, XRCoreEventType from isaacsim.core.prims import SingleXFormPrim @@ -60,19 +64,6 @@ class OpenXRDevice(DeviceBase): data is transformed into robot control commands suitable for teleoperation. """ - class TrackingTarget(Enum): - """Enum class specifying what to track with OpenXR. - - Attributes: - HAND_LEFT: Track the left hand (index 0 in _get_raw_data output) - HAND_RIGHT: Track the right hand (index 1 in _get_raw_data output) - HEAD: Track the head/headset position (index 2 in _get_raw_data output) - """ - - HAND_LEFT = 0 - HAND_RIGHT = 1 - HEAD = 2 - TELEOP_COMMAND_EVENT_TYPE = "teleop_command" def __init__( @@ -89,12 +80,13 @@ def __init__( super().__init__(retargeters) self._xr_cfg = cfg.xr_cfg or XrCfg() self._additional_callbacks = dict() + self._xr_core = XRCore.get_singleton() if XRCore is not None else None self._vc_subscription = ( - XRCore.get_singleton() - .get_message_bus() - .create_subscription_to_pop_by_type( + self._xr_core.get_message_bus().create_subscription_to_pop_by_type( carb.events.type_from_string(self.TELEOP_COMMAND_EVENT_TYPE), self._on_teleop_command ) + if self._xr_core is not None + else None ) # Initialize dictionaries instead of arrays @@ -103,10 +95,57 @@ def __init__( self._previous_joint_poses_right = {name: default_pose.copy() for name in HAND_JOINT_NAMES} self._previous_headpose = default_pose.copy() - xr_anchor = SingleXFormPrim("/XRAnchor", position=self._xr_cfg.anchor_pos, orientation=self._xr_cfg.anchor_rot) - carb.settings.get_settings().set_float("/persistent/xr/profile/ar/render/nearPlane", self._xr_cfg.near_plane) - carb.settings.get_settings().set_string("/persistent/xr/profile/ar/anchorMode", "custom anchor") - carb.settings.get_settings().set_string("/xrstage/profile/ar/customAnchor", xr_anchor.prim_path) + if self._xr_cfg.anchor_prim_path is not None: + anchor_path = self._xr_cfg.anchor_prim_path + if anchor_path.endswith("/"): + anchor_path = anchor_path[:-1] + self._xr_anchor_headset_path = f"{anchor_path}/XRAnchor" + else: + self._xr_anchor_headset_path = "/World/XRAnchor" + + _ = SingleXFormPrim( + self._xr_anchor_headset_path, position=self._xr_cfg.anchor_pos, orientation=self._xr_cfg.anchor_rot + ) + + if hasattr(carb, "settings"): + carb.settings.get_settings().set_float( + "/persistent/xr/profile/ar/render/nearPlane", self._xr_cfg.near_plane + ) + carb.settings.get_settings().set_string("/persistent/xr/profile/ar/anchorMode", "custom anchor") + carb.settings.get_settings().set_string("/xrstage/profile/ar/customAnchor", self._xr_anchor_headset_path) + + # Button binding support + self.__button_subscriptions: dict[str, dict] = {} + + # Optional anchor synchronizer + self._anchor_sync: XrAnchorSynchronizer | None = None + if self._xr_core is not None and self._xr_cfg.anchor_prim_path is not None: + try: + self._anchor_sync = XrAnchorSynchronizer( + xr_core=self._xr_core, + xr_cfg=self._xr_cfg, + xr_anchor_headset_path=self._xr_anchor_headset_path, + ) + # Subscribe to pre_sync_update to keep anchor in sync + if XRCoreEventType is not None: + self._xr_pre_sync_update_subscription = ( + self._xr_core.get_message_bus().create_subscription_to_pop_by_type( + XRCoreEventType.pre_sync_update, + lambda _: self._anchor_sync.sync_headset_to_anchor(), + name="isaaclab_xr_pre_sync_update", + ) + ) + except Exception as e: + logger.warning(f"XR: Failed to initialize anchor synchronizer: {e}") + + # Default convenience binding: toggle anchor rotation with right controller 'a' button + with contextlib.suppress(Exception): + self._bind_button_press( + "/user/hand/right", + "a", + "isaaclab_right_a", + lambda ev: self._toggle_anchor_rotation(), + ) def __del__(self): """Clean up resources when the object is destroyed. @@ -116,6 +155,11 @@ def __del__(self): """ if hasattr(self, "_vc_subscription") and self._vc_subscription is not None: self._vc_subscription = None + if hasattr(self, "_xr_pre_sync_update_subscription") and self._xr_pre_sync_update_subscription is not None: + self._xr_pre_sync_update_subscription = None + # clear button subscriptions + if hasattr(self, "__button_subscriptions"): + self._unbind_all_buttons() # No need to explicitly clean up OpenXR instance as it's managed by NVIDIA Isaac Sim @@ -132,6 +176,10 @@ def __str__(self) -> str: msg = f"OpenXR Hand Tracking Device: {self.__class__.__name__}\n" msg += f"\tAnchor Position: {self._xr_cfg.anchor_pos}\n" msg += f"\tAnchor Rotation: {self._xr_cfg.anchor_rot}\n" + if self._xr_cfg.anchor_prim_path is not None: + msg += f"\tAnchor Prim Path: {self._xr_cfg.anchor_prim_path} (Dynamic Anchoring)\n" + else: + msg += "\tAnchor Mode: Static (Root Level)\n" # Add retargeter information if self._retargeters: @@ -172,6 +220,8 @@ def reset(self): self._previous_joint_poses_left = {name: default_pose.copy() for name in HAND_JOINT_NAMES} self._previous_joint_poses_right = {name: default_pose.copy() for name in HAND_JOINT_NAMES} self._previous_headpose = default_pose.copy() + if hasattr(self, "_anchor_sync") and self._anchor_sync is not None: + self._anchor_sync.reset() def add_callback(self, key: str, func: Callable): """Add additional functions to bind to client messages. @@ -195,17 +245,37 @@ def _get_raw_data(self) -> Any: Each pose is represented as a 7-element array: [x, y, z, qw, qx, qy, qz] where the first 3 elements are position and the last 4 are quaternion orientation. """ - return { - self.TrackingTarget.HAND_LEFT: self._calculate_joint_poses( + data = {} + + if RetargeterBase.Requirement.HAND_TRACKING in self._required_features: + data[DeviceBase.TrackingTarget.HAND_LEFT] = self._calculate_joint_poses( XRCore.get_singleton().get_input_device("/user/hand/left"), self._previous_joint_poses_left, - ), - self.TrackingTarget.HAND_RIGHT: self._calculate_joint_poses( + ) + data[DeviceBase.TrackingTarget.HAND_RIGHT] = self._calculate_joint_poses( XRCore.get_singleton().get_input_device("/user/hand/right"), self._previous_joint_poses_right, - ), - self.TrackingTarget.HEAD: self._calculate_headpose(), - } + ) + + if RetargeterBase.Requirement.HEAD_TRACKING in self._required_features: + data[DeviceBase.TrackingTarget.HEAD] = self._calculate_headpose() + + if RetargeterBase.Requirement.MOTION_CONTROLLER in self._required_features: + # Optionally include motion controller pose+inputs if devices are available + try: + left_dev = XRCore.get_singleton().get_input_device("/user/hand/left") + right_dev = XRCore.get_singleton().get_input_device("/user/hand/right") + left_ctrl = self._query_controller(left_dev) if left_dev is not None else np.array([]) + right_ctrl = self._query_controller(right_dev) if right_dev is not None else np.array([]) + if left_ctrl.size: + data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] = left_ctrl + if right_ctrl.size: + data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] = right_ctrl + except Exception: + # Ignore controller data if XRCore/controller APIs are unavailable + pass + + return data """ Internal helpers. @@ -287,6 +357,133 @@ def _calculate_headpose(self) -> np.ndarray: return self._previous_headpose + # ----------------------------- + # Controller button binding utilities + # ----------------------------- + def _bind_button_press( + self, + device_path: str, + button_name: str, + event_name: str, + on_button_press: Callable[[carb.events.IEvent], None], + ) -> None: + if self._xr_core is None: + logger.warning("XR core not available; skipping button binding") + return + + sub_key = f"{device_path}/{button_name}" + self.__button_subscriptions[sub_key] = {} + + def try_emit_button_events(): + if self.__button_subscriptions[sub_key].get("generator"): + return + device = self._xr_core.get_input_device(device_path) + if not device: + return + names = {str(n) for n in (device.get_input_names() or ())} + if button_name not in names: + return + gen = device.bind_event_generator(button_name, event_name, ("press",)) + if gen is not None: + logger.info(f"XR: Bound event generator for {sub_key}, {event_name}") + self.__button_subscriptions[sub_key]["generator"] = gen + + def on_inputs_change(_ev: carb.events.IEvent) -> None: + try_emit_button_events() + + def on_disable(_ev: carb.events.IEvent) -> None: + self.__button_subscriptions[sub_key]["generator"] = None + + message_bus = self._xr_core.get_message_bus() + event_suffix = device_path.strip("/").replace("/", "_") + self.__button_subscriptions[sub_key]["press_sub"] = message_bus.create_subscription_to_pop_by_type( + carb.events.type_from_string(f"{event_name}.press"), on_button_press + ) + self.__button_subscriptions[sub_key]["inputs_change_sub"] = message_bus.create_subscription_to_pop_by_type( + carb.events.type_from_string(f"xr_input.{event_suffix}.inputs_change"), on_inputs_change + ) + self.__button_subscriptions[sub_key]["disable_sub"] = message_bus.create_subscription_to_pop_by_type( + carb.events.type_from_string(f"xr_input.{event_suffix}.disable"), on_disable + ) + try_emit_button_events() + + def _unbind_all_buttons(self) -> None: + for sub_key, subs in self.__button_subscriptions.items(): + if "generator" in subs: + subs["generator"] = None + for key in ["press_sub", "inputs_change_sub", "disable_sub"]: + if key in subs: + subs[key] = None + self.__button_subscriptions.clear() + logger.info("XR: Unbound all button event handlers") + + def _toggle_anchor_rotation(self): + if self._anchor_sync is not None: + self._anchor_sync.toggle_anchor_rotation() + + def _query_controller(self, input_device) -> np.ndarray: + """Query motion controller pose and inputs as a 2x7 array. + + Row 0 (POSE): [x, y, z, w, x, y, z] + Row 1 (INPUTS): [thumbstick_x, thumbstick_y, trigger, squeeze, button_0, button_1, padding] + """ + if input_device is None: + return np.array([]) + + pose = input_device.get_virtual_world_pose() + position = pose.ExtractTranslation() + quat = pose.ExtractRotationQuat() + + thumbstick_x = 0.0 + thumbstick_y = 0.0 + trigger = 0.0 + squeeze = 0.0 + button_0 = 0.0 + button_1 = 0.0 + + if input_device.has_input_gesture("thumbstick", "x"): + thumbstick_x = float(input_device.get_input_gesture_value("thumbstick", "x")) + if input_device.has_input_gesture("thumbstick", "y"): + thumbstick_y = float(input_device.get_input_gesture_value("thumbstick", "y")) + if input_device.has_input_gesture("trigger", "value"): + trigger = float(input_device.get_input_gesture_value("trigger", "value")) + if input_device.has_input_gesture("squeeze", "value"): + squeeze = float(input_device.get_input_gesture_value("squeeze", "value")) + + # Determine which button pair exists on this device + if input_device.has_input_gesture("x", "click") or input_device.has_input_gesture("y", "click"): + if input_device.has_input_gesture("x", "click"): + button_0 = float(input_device.get_input_gesture_value("x", "click")) + if input_device.has_input_gesture("y", "click"): + button_1 = float(input_device.get_input_gesture_value("y", "click")) + else: + if input_device.has_input_gesture("a", "click"): + button_0 = float(input_device.get_input_gesture_value("a", "click")) + if input_device.has_input_gesture("b", "click"): + button_1 = float(input_device.get_input_gesture_value("b", "click")) + + pose_row = [ + position[0], + position[1], + position[2], + quat.GetReal(), + quat.GetImaginary()[0], + quat.GetImaginary()[1], + quat.GetImaginary()[2], + ] + + input_row = [ + thumbstick_x, + thumbstick_y, + trigger, + squeeze, + button_0, + button_1, + 0.0, + ] + + return np.array([pose_row, input_row], dtype=np.float32) + def _on_teleop_command(self, event: carb.events.IEvent): msg = event.payload["message"] @@ -299,6 +496,7 @@ def _on_teleop_command(self, event: carb.events.IEvent): elif "reset" in msg: if "RESET" in self._additional_callbacks: self._additional_callbacks["RESET"]() + self.reset() @dataclass diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py index f2972ec6580..d4e12bd40ed 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py @@ -6,7 +6,15 @@ from .humanoid.fourier.gr1t2_retargeter import GR1T2Retargeter, GR1T2RetargeterCfg from .humanoid.unitree.g1_lower_body_standing import G1LowerBodyStandingRetargeter, G1LowerBodyStandingRetargeterCfg +from .humanoid.unitree.g1_motion_controller_locomotion import ( + G1LowerBodyStandingMotionControllerRetargeter, + G1LowerBodyStandingMotionControllerRetargeterCfg, +) from .humanoid.unitree.inspire.g1_upper_body_retargeter import UnitreeG1Retargeter, UnitreeG1RetargeterCfg +from .humanoid.unitree.trihand.g1_upper_body_motion_ctrl_retargeter import ( + G1TriHandUpperBodyMotionControllerRetargeter, + G1TriHandUpperBodyMotionControllerRetargeterCfg, +) from .humanoid.unitree.trihand.g1_upper_body_retargeter import ( G1TriHandUpperBodyRetargeter, G1TriHandUpperBodyRetargeterCfg, diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py index 6f307bc0fa4..a352580c2e9 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py @@ -12,7 +12,7 @@ import isaaclab.sim as sim_utils import isaaclab.utils.math as PoseUtils -from isaaclab.devices import OpenXRDevice +from isaaclab.devices.device_base import DeviceBase from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg @@ -41,6 +41,7 @@ def __init__( hand_joint_names: List of robot hand joint names """ + super().__init__(cfg) self._hand_joint_names = cfg.hand_joint_names self._hands_controller = GR1TR2DexRetargeting(self._hand_joint_names) @@ -74,8 +75,8 @@ def retarget(self, data: dict) -> torch.Tensor: """ # Access the left and right hand data using the enum key - left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT] - right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + left_hand_poses = data[DeviceBase.TrackingTarget.HAND_LEFT] + right_hand_poses = data[DeviceBase.TrackingTarget.HAND_RIGHT] left_wrist = left_hand_poses.get("wrist") right_wrist = right_hand_poses.get("wrist") @@ -110,6 +111,9 @@ def retarget(self, data: dict) -> torch.Tensor: # Combine all tensors into a single tensor return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + def _retarget_abs(self, wrist: np.ndarray) -> np.ndarray: """Handle absolute pose retargeting. diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py index 7ad2a62bfe7..3c7f4309b05 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py @@ -16,11 +16,16 @@ class G1LowerBodyStandingRetargeter(RetargeterBase): def __init__(self, cfg: G1LowerBodyStandingRetargeterCfg): """Initialize the retargeter.""" + super().__init__(cfg) self.cfg = cfg def retarget(self, data: dict) -> torch.Tensor: return torch.tensor([0.0, 0.0, 0.0, self.cfg.hip_height], device=self.cfg.sim_device) + def get_requirements(self) -> list[RetargeterBase.Requirement]: + # This retargeter does not consume any device data + return [] + @dataclass class G1LowerBodyStandingRetargeterCfg(RetargeterCfg): diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_motion_controller_locomotion.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_motion_controller_locomotion.py new file mode 100644 index 00000000000..a3dd950e882 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_motion_controller_locomotion.py @@ -0,0 +1,85 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import torch +from dataclasses import dataclass + +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.sim import SimulationContext + + +class G1LowerBodyStandingMotionControllerRetargeter(RetargeterBase): + """Provides lower body standing commands for the G1 robot.""" + + def __init__(self, cfg: G1LowerBodyStandingMotionControllerRetargeterCfg): + """Initialize the retargeter.""" + super().__init__(cfg) + self.cfg = cfg + self._hip_height = cfg.hip_height + + def retarget(self, data: dict) -> torch.Tensor: + left_thumbstick_x = 0.0 + left_thumbstick_y = 0.0 + right_thumbstick_x = 0.0 + right_thumbstick_y = 0.0 + + # Get controller data using enums + if ( + DeviceBase.TrackingTarget.CONTROLLER_LEFT in data + and data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] is not None + ): + left_controller_data = data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] + if len(left_controller_data) > DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + left_inputs = left_controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + if len(left_inputs) > DeviceBase.MotionControllerInputIndex.THUMBSTICK_Y.value: + left_thumbstick_x = left_inputs[DeviceBase.MotionControllerInputIndex.THUMBSTICK_X.value] + left_thumbstick_y = left_inputs[DeviceBase.MotionControllerInputIndex.THUMBSTICK_Y.value] + + if ( + DeviceBase.TrackingTarget.CONTROLLER_RIGHT in data + and data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] is not None + ): + right_controller_data = data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] + if len(right_controller_data) > DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + right_inputs = right_controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + if len(right_inputs) > DeviceBase.MotionControllerInputIndex.THUMBSTICK_Y.value: + right_thumbstick_x = right_inputs[DeviceBase.MotionControllerInputIndex.THUMBSTICK_X.value] + right_thumbstick_y = right_inputs[DeviceBase.MotionControllerInputIndex.THUMBSTICK_Y.value] + + # Thumbstick values are in the range of [-1, 1], so we need to scale them to the range of [-movement_scale, movement_scale] + left_thumbstick_x = left_thumbstick_x * self.cfg.movement_scale + left_thumbstick_y = left_thumbstick_y * self.cfg.movement_scale + + # Use rendering time step for deterministic hip height adjustment regardless of wall clock time. + dt = SimulationContext.instance().get_rendering_dt() + self._hip_height -= right_thumbstick_y * dt * self.cfg.rotation_scale + self._hip_height = max(0.4, min(1.0, self._hip_height)) + + return torch.tensor( + [-left_thumbstick_y, -left_thumbstick_x, -right_thumbstick_x, self._hip_height], + device=self.cfg.sim_device, + dtype=torch.float32, + ) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + +@dataclass +class G1LowerBodyStandingMotionControllerRetargeterCfg(RetargeterCfg): + """Configuration for the G1 lower body standing retargeter.""" + + hip_height: float = 0.72 + """Height of the G1 robot hip in meters. The value is a fixed height suitable for G1 to do tabletop manipulation.""" + + movement_scale: float = 0.5 + """Scale the movement of the robot to the range of [-movement_scale, movement_scale].""" + + rotation_scale: float = 0.35 + """Scale the rotation of the robot to the range of [-rotation_scale, rotation_scale].""" + retargeter_type: type[RetargeterBase] = G1LowerBodyStandingMotionControllerRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py index 39342bc522e..74fa7518e90 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py @@ -12,7 +12,7 @@ import isaaclab.sim as sim_utils import isaaclab.utils.math as PoseUtils -from isaaclab.devices import OpenXRDevice +from isaaclab.devices.device_base import DeviceBase from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg @@ -41,6 +41,7 @@ def __init__( hand_joint_names: List of robot hand joint names """ + super().__init__(cfg) self._hand_joint_names = cfg.hand_joint_names self._hands_controller = UnitreeG1DexRetargeting(self._hand_joint_names) @@ -74,8 +75,8 @@ def retarget(self, data: dict) -> torch.Tensor: """ # Access the left and right hand data using the enum key - left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT] - right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + left_hand_poses = data[DeviceBase.TrackingTarget.HAND_LEFT] + right_hand_poses = data[DeviceBase.TrackingTarget.HAND_RIGHT] left_wrist = left_hand_poses.get("wrist") right_wrist = right_hand_poses.get("wrist") @@ -114,6 +115,9 @@ def retarget(self, data: dict) -> torch.Tensor: # Combine all tensors into a single tensor return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: """Handle absolute pose retargeting. diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_retargeter.py new file mode 100644 index 00000000000..49481da6d6f --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_retargeter.py @@ -0,0 +1,216 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import numpy as np +import torch +from dataclasses import dataclass + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as PoseUtils +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg +from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg + + +class G1TriHandUpperBodyMotionControllerRetargeter(RetargeterBase): + """Simple retargeter that maps motion controller inputs to G1 hand joints. + + Mapping: + - A button (digital 0/1) → Thumb joints + - Trigger (analog 0-1) → Index finger joints + - Squeeze (analog 0-1) → Middle finger joints + """ + + def __init__(self, cfg: G1TriHandUpperBodyMotionControllerRetargeterCfg): + """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. + + Args: + data: Dictionary with MotionControllerTrackingTarget.LEFT/RIGHT keys + Each value is a 2D array: [pose(7), inputs(7)] + + Returns: + Tensor: [left_wrist(7), right_wrist(7), hand_joints(14)] + hand_joints order: [left_proximal(3), right_proximal(3), left_distal(2), left_thumb_middle(1), right_distal(2), right_thumb_middle(1), left_thumb_tip(1), right_thumb_tip(1)] + """ + + # Get controller data + left_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_LEFT, np.array([])) + right_controller_data = data.get(DeviceBase.TrackingTarget.CONTROLLER_RIGHT, np.array([])) + + # Default wrist poses (position + quaternion) + default_wrist = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]) + + # Extract poses from controller data + left_wrist = self._extract_wrist_pose(left_controller_data, default_wrist) + right_wrist = self._extract_wrist_pose(right_controller_data, default_wrist) + + # Map controller inputs to hand joints + left_hand_joints = self._map_to_hand_joints(left_controller_data, is_left=True) + right_hand_joints = self._map_to_hand_joints(right_controller_data, is_left=False) + + # Negate left hand joints for proper mirroring + left_hand_joints = -left_hand_joints + + # Combine joints in the expected order: [left_proximal(3), right_proximal(3), left_distal(2), left_thumb_middle(1), right_distal(2), right_thumb_middle(1), left_thumb_tip(1), right_thumb_tip(1)] + all_hand_joints = np.array([ + left_hand_joints[3], # left_index_proximal + left_hand_joints[5], # left_middle_proximal + left_hand_joints[0], # left_thumb_base + right_hand_joints[3], # right_index_proximal + right_hand_joints[5], # right_middle_proximal + right_hand_joints[0], # right_thumb_base + left_hand_joints[4], # left_index_distal + left_hand_joints[6], # left_middle_distal + left_hand_joints[1], # left_thumb_middle + right_hand_joints[4], # right_index_distal + right_hand_joints[6], # right_middle_distal + right_hand_joints[1], # right_thumb_middle + left_hand_joints[2], # left_thumb_tip + right_hand_joints[2], # right_thumb_tip + ]) + + # Convert to tensors + left_wrist_tensor = torch.tensor( + self._retarget_abs(left_wrist, is_left=True), dtype=torch.float32, device=self._sim_device + ) + right_wrist_tensor = torch.tensor( + self._retarget_abs(right_wrist, is_left=False), dtype=torch.float32, device=self._sim_device + ) + hand_joints_tensor = torch.tensor(all_hand_joints, dtype=torch.float32, device=self._sim_device) + + return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + def _extract_wrist_pose(self, controller_data: np.ndarray, default_pose: np.ndarray) -> np.ndarray: + """Extract wrist pose from controller data. + + Args: + controller_data: 2D array [pose(7), inputs(7)] + default_pose: Default pose to use if no data + + Returns: + Wrist pose array [x, y, z, w, x, y, z] + """ + if len(controller_data) > DeviceBase.MotionControllerDataRowIndex.POSE.value: + return controller_data[DeviceBase.MotionControllerDataRowIndex.POSE.value] + return default_pose + + def _map_to_hand_joints(self, controller_data: np.ndarray, is_left: bool) -> np.ndarray: + """Map controller inputs to hand joint angles. + + Args: + controller_data: 2D array [pose(7), inputs(7)] + is_left: True for left hand, False for right hand + + Returns: + Hand joint angles (7 joints per hand) in radians + """ + + # Initialize all joints to zero + hand_joints = np.zeros(7) + + if len(controller_data) <= DeviceBase.MotionControllerDataRowIndex.INPUTS.value: + return hand_joints + + # Extract inputs from second row + inputs = controller_data[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + + if len(inputs) < len(DeviceBase.MotionControllerInputIndex): + return hand_joints + + # Extract specific inputs using enum + trigger = inputs[DeviceBase.MotionControllerInputIndex.TRIGGER.value] # 0.0 to 1.0 (analog) + squeeze = inputs[DeviceBase.MotionControllerInputIndex.SQUEEZE.value] # 0.0 to 1.0 (analog) + + # Grasping logic: If trigger is pressed, we grasp with index and thumb. If squeeze is pressed, we grasp with middle and thumb. + # If both are pressed, we grasp with index, middle, and thumb. + # The thumb rotates towards the direction of the pressing finger. If both are pressed, the thumb stays in the middle. + + thumb_button = max(trigger, squeeze) + + # Map to G1 hand joints (in radians) + # Thumb joints (3 joints) - controlled by A button (digital) + thumb_angle = -thumb_button # Max 1 radian ≈ 57° + + # Thumb rotation: If trigger is pressed, we rotate the thumb toward the index finger. If squeeze is pressed, we rotate the thumb toward the middle finger. + # If both are pressed, the thumb stays between the index and middle fingers. + # Trigger pushes toward +0.5, squeeze pushes toward -0.5 + # trigger=1,squeeze=0 → 0.5; trigger=0,squeeze=1 → -0.5; both=1 → 0 + thumb_rotation = 0.5 * trigger - 0.5 * squeeze + + if not is_left: + thumb_rotation = -thumb_rotation + + # These values were found empirically to get a good gripper pose. + + hand_joints[0] = thumb_rotation # thumb_0_joint (base) + hand_joints[1] = thumb_angle * 0.4 # thumb_1_joint (middle) + hand_joints[2] = thumb_angle * 0.7 # thumb_2_joint (tip) + + # Index finger joints (2 joints) - controlled by trigger (analog) + index_angle = trigger * 1.0 # Max 1.0 radians ≈ 57° + hand_joints[3] = index_angle # index_0_joint (proximal) + hand_joints[4] = index_angle # index_1_joint (distal) + + # Middle finger joints (2 joints) - controlled by squeeze (analog) + middle_angle = squeeze * 1.0 # Max 1.0 radians ≈ 57° + hand_joints[5] = middle_angle # middle_0_joint (proximal) + hand_joints[6] = middle_angle # middle_1_joint (distal) + + return hand_joints + + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: + """Handle absolute pose retargeting for controller wrists.""" + wrist_pos = torch.tensor(wrist[:3], dtype=torch.float32) + wrist_quat = torch.tensor(wrist[3:], dtype=torch.float32) + + # Combined -75° (rather than -90° for wrist comfort) Y rotation + 90° Z rotation + # This is equivalent to (0, -75, 90) in euler angles + combined_quat = torch.tensor([0.5358, -0.4619, 0.5358, 0.4619], dtype=torch.float32) + + openxr_pose = PoseUtils.make_pose(wrist_pos, PoseUtils.matrix_from_quat(wrist_quat)) + transform_pose = PoseUtils.make_pose(torch.zeros(3), PoseUtils.matrix_from_quat(combined_quat)) + + result_pose = PoseUtils.pose_in_A_to_pose_in_B(transform_pose, openxr_pose) + pos, rot_mat = PoseUtils.unmake_pose(result_pose) + quat = PoseUtils.quat_from_matrix(rot_mat) + + return np.concatenate([pos.numpy(), quat.numpy()]) + + +@dataclass +class G1TriHandUpperBodyMotionControllerRetargeterCfg(RetargeterCfg): + """Configuration for the G1 Controller Upper Body retargeter.""" + + enable_visualization: bool = False + hand_joint_names: list[str] | None = None # List of robot hand joint names + retargeter_type: type[RetargeterBase] = G1TriHandUpperBodyMotionControllerRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py index 0e3dd6058ff..8e432aa59fe 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py @@ -12,7 +12,7 @@ import isaaclab.sim as sim_utils import isaaclab.utils.math as PoseUtils -from isaaclab.devices import OpenXRDevice +from isaaclab.devices.device_base import DeviceBase from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg @@ -38,6 +38,7 @@ def __init__( Args: cfg: Configuration for the retargeter. """ + super().__init__(cfg) # Store device name for runtime retrieval self._sim_device = cfg.sim_device @@ -78,8 +79,8 @@ def retarget(self, data: dict) -> torch.Tensor: """ # Access the left and right hand data using the enum key - left_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_LEFT] - right_hand_poses = data[OpenXRDevice.TrackingTarget.HAND_RIGHT] + left_hand_poses = data[DeviceBase.TrackingTarget.HAND_LEFT] + right_hand_poses = data[DeviceBase.TrackingTarget.HAND_RIGHT] left_wrist = left_hand_poses.get("wrist") right_wrist = right_hand_poses.get("wrist") @@ -127,6 +128,9 @@ def retarget(self, data: dict) -> torch.Tensor: # Combine all tensors into a single tensor return torch.cat([left_wrist_tensor, right_wrist_tensor, hand_joints_tensor]) + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: """Handle absolute pose retargeting. @@ -159,7 +163,7 @@ def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: @dataclass class G1TriHandUpperBodyRetargeterCfg(RetargeterCfg): - """Configuration for the G1UpperBody retargeter.""" + """Configuration for the G1 Controller Upper Body retargeter.""" enable_visualization: bool = False num_open_xr_hand_joints: int = 100 diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py index a606e5ff002..547df1dc8ee 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py @@ -9,7 +9,7 @@ from dataclasses import dataclass from typing import Final -from isaaclab.devices import OpenXRDevice +from isaaclab.devices.device_base import DeviceBase from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg @@ -36,10 +36,9 @@ def __init__( super().__init__(cfg) """Initialize the gripper retargeter.""" # Store the hand to track - if cfg.bound_hand not in [OpenXRDevice.TrackingTarget.HAND_LEFT, OpenXRDevice.TrackingTarget.HAND_RIGHT]: + if cfg.bound_hand not in [DeviceBase.TrackingTarget.HAND_LEFT, DeviceBase.TrackingTarget.HAND_RIGHT]: raise ValueError( - "bound_hand must be either OpenXRDevice.TrackingTarget.HAND_LEFT or" - " OpenXRDevice.TrackingTarget.HAND_RIGHT" + "bound_hand must be either DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.TrackingTarget.HAND_RIGHT" ) self.bound_hand = cfg.bound_hand # Initialize gripper state @@ -66,6 +65,9 @@ def retarget(self, data: dict) -> torch.Tensor: return torch.tensor([gripper_value], dtype=torch.float32, device=self._sim_device) + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + def _calculate_gripper_command(self, thumb_pos: np.ndarray, index_pos: np.ndarray) -> bool: """Calculate gripper command from finger positions with hysteresis. @@ -91,5 +93,5 @@ def _calculate_gripper_command(self, thumb_pos: np.ndarray, index_pos: np.ndarra class GripperRetargeterCfg(RetargeterCfg): """Configuration for gripper retargeter.""" - bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT + bound_hand: DeviceBase.TrackingTarget = DeviceBase.TrackingTarget.HAND_RIGHT retargeter_type: type[RetargeterBase] = GripperRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py index 72a2a82fcb2..fd82ab07556 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py @@ -9,7 +9,7 @@ from dataclasses import dataclass from scipy.spatial.transform import Rotation, Slerp -from isaaclab.devices import OpenXRDevice +from isaaclab.devices.device_base import DeviceBase from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.markers import VisualizationMarkers from isaaclab.markers.config import FRAME_MARKER_CFG @@ -35,7 +35,7 @@ def __init__( """Initialize the retargeter. Args: - bound_hand: The hand to track (OpenXRDevice.TrackingTarget.HAND_LEFT or OpenXRDevice.TrackingTarget.HAND_RIGHT) + bound_hand: The hand to track (DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.TrackingTarget.HAND_RIGHT) zero_out_xy_rotation: If True, zero out rotation around x and y axes use_wrist_rotation: If True, use wrist rotation instead of finger average use_wrist_position: If True, use wrist position instead of pinch position @@ -43,10 +43,9 @@ def __init__( device: The device to place the returned tensor on ('cpu' or 'cuda') """ super().__init__(cfg) - if cfg.bound_hand not in [OpenXRDevice.TrackingTarget.HAND_LEFT, OpenXRDevice.TrackingTarget.HAND_RIGHT]: + if cfg.bound_hand not in [DeviceBase.TrackingTarget.HAND_LEFT, DeviceBase.TrackingTarget.HAND_RIGHT]: raise ValueError( - "bound_hand must be either OpenXRDevice.TrackingTarget.HAND_LEFT or" - " OpenXRDevice.TrackingTarget.HAND_RIGHT" + "bound_hand must be either DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.TrackingTarget.HAND_RIGHT" ) self.bound_hand = cfg.bound_hand @@ -88,6 +87,9 @@ def retarget(self, data: dict) -> torch.Tensor: return ee_command + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + def _retarget_abs(self, thumb_tip: np.ndarray, index_tip: np.ndarray, wrist: np.ndarray) -> np.ndarray: """Handle absolute pose retargeting. @@ -165,5 +167,5 @@ class Se3AbsRetargeterCfg(RetargeterCfg): use_wrist_rotation: bool = False use_wrist_position: bool = True enable_visualization: bool = False - bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT + bound_hand: DeviceBase.TrackingTarget = DeviceBase.TrackingTarget.HAND_RIGHT retargeter_type: type[RetargeterBase] = Se3AbsRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py index e5d68ed39cd..5c70e9ea61a 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py @@ -9,7 +9,7 @@ from dataclasses import dataclass from scipy.spatial.transform import Rotation -from isaaclab.devices import OpenXRDevice +from isaaclab.devices.device_base import DeviceBase from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.markers import VisualizationMarkers from isaaclab.markers.config import FRAME_MARKER_CFG @@ -36,7 +36,7 @@ def __init__( """Initialize the relative motion retargeter. Args: - bound_hand: The hand to track (OpenXRDevice.TrackingTarget.HAND_LEFT or OpenXRDevice.TrackingTarget.HAND_RIGHT) + bound_hand: The hand to track (DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.TrackingTarget.HAND_RIGHT) zero_out_xy_rotation: If True, ignore rotations around x and y axes, allowing only z-axis rotation use_wrist_rotation: If True, use wrist rotation for control instead of averaging finger orientations use_wrist_position: If True, use wrist position instead of pinch position (midpoint between fingers) @@ -48,10 +48,9 @@ def __init__( device: The device to place the returned tensor on ('cpu' or 'cuda') """ # Store the hand to track - if cfg.bound_hand not in [OpenXRDevice.TrackingTarget.HAND_LEFT, OpenXRDevice.TrackingTarget.HAND_RIGHT]: + if cfg.bound_hand not in [DeviceBase.TrackingTarget.HAND_LEFT, DeviceBase.TrackingTarget.HAND_RIGHT]: raise ValueError( - "bound_hand must be either OpenXRDevice.TrackingTarget.HAND_LEFT or" - " OpenXRDevice.TrackingTarget.HAND_RIGHT" + "bound_hand must be either DeviceBase.TrackingTarget.HAND_LEFT or DeviceBase.TrackingTarget.HAND_RIGHT" ) super().__init__(cfg) self.bound_hand = cfg.bound_hand @@ -117,6 +116,9 @@ def retarget(self, data: dict) -> torch.Tensor: return ee_command + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.HAND_TRACKING] + def _calculate_delta_pose(self, joint_pose: np.ndarray, previous_joint_pose: np.ndarray) -> np.ndarray: """Calculate delta pose from previous joint pose. @@ -207,5 +209,5 @@ class Se3RelRetargeterCfg(RetargeterCfg): alpha_pos: float = 0.5 alpha_rot: float = 0.5 enable_visualization: bool = False - bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT + bound_hand: DeviceBase.TrackingTarget = DeviceBase.TrackingTarget.HAND_RIGHT retargeter_type: type[RetargeterBase] = Se3RelRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/xr_anchor_utils.py b/source/isaaclab/isaaclab/devices/openxr/xr_anchor_utils.py new file mode 100644 index 00000000000..bb2d1e1a185 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/xr_anchor_utils.py @@ -0,0 +1,176 @@ +# 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 + +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# SPDX-License-Identifier: BSD-3-Clause +"""Utilities for synchronizing XR anchor pose with a reference prim and XR config.""" + +from __future__ import annotations + +import contextlib +import logging +import math +import numpy as np +from typing import Any + +# import logger +logger = logging.getLogger(__name__) + +import isaaclab.sim as sim_utils +from isaaclab.sim import SimulationContext + +from .xr_cfg import XrAnchorRotationMode + +with contextlib.suppress(ModuleNotFoundError): + import usdrt + from pxr import Gf as pxrGf + from usdrt import Rt + + +class XrAnchorSynchronizer: + """Keeps the XR anchor prim aligned with a reference prim according to XR config.""" + + def __init__(self, xr_core: Any, xr_cfg: Any, xr_anchor_headset_path: str): + self._xr_core = xr_core + self._xr_cfg = xr_cfg + self._xr_anchor_headset_path = xr_anchor_headset_path + + self.__anchor_prim_initial_quat = None + self.__anchor_prim_initial_height = None + self.__smoothed_anchor_quat = None + self.__last_anchor_quat = None + self.__anchor_rotation_enabled = True + + # Resolve USD layer identifier of the anchor for updates + try: + from isaacsim.core.utils.stage import get_current_stage + + stage = get_current_stage() + xr_anchor_headset_prim = stage.GetPrimAtPath(self._xr_anchor_headset_path) + prim_stack = xr_anchor_headset_prim.GetPrimStack() if xr_anchor_headset_prim is not None else None + self.__anchor_headset_layer_identifier = prim_stack[0].layer.identifier if prim_stack else None + except Exception: + self.__anchor_headset_layer_identifier = None + + def reset(self): + self.__anchor_prim_initial_quat = None + self.__anchor_prim_initial_height = None + self.__smoothed_anchor_quat = None + self.__last_anchor_quat = None + self.__anchor_rotation_enabled = True + self.sync_headset_to_anchor() + + def toggle_anchor_rotation(self): + self.__anchor_rotation_enabled = not self.__anchor_rotation_enabled + logger.info(f"XR: Toggling anchor rotation: {self.__anchor_rotation_enabled}") + + def sync_headset_to_anchor(self): + """Sync XR anchor pose in USD from reference prim (in Fabric/usdrt).""" + try: + if self._xr_cfg.anchor_prim_path is None: + return + + stage_id = sim_utils.get_current_stage_id() + rt_stage = usdrt.Usd.Stage.Attach(stage_id) + if rt_stage is None: + return + + rt_prim = rt_stage.GetPrimAtPath(self._xr_cfg.anchor_prim_path) + if rt_prim is None: + return + + rt_xformable = Rt.Xformable(rt_prim) + if rt_xformable is None: + return + + world_matrix_attr = rt_xformable.GetFabricHierarchyWorldMatrixAttr() + if world_matrix_attr is None: + return + + rt_matrix = world_matrix_attr.Get() + rt_pos = rt_matrix.ExtractTranslation() + + if self.__anchor_prim_initial_quat is None: + self.__anchor_prim_initial_quat = rt_matrix.ExtractRotationQuat() + + if getattr(self._xr_cfg, "fixed_anchor_height", False): + if self.__anchor_prim_initial_height is None: + self.__anchor_prim_initial_height = rt_pos[2] + rt_pos[2] = self.__anchor_prim_initial_height + + pxr_anchor_pos = pxrGf.Vec3d(*rt_pos) + pxrGf.Vec3d(*self._xr_cfg.anchor_pos) + + w, x, y, z = self._xr_cfg.anchor_rot + pxr_cfg_quat = pxrGf.Quatd(w, pxrGf.Vec3d(x, y, z)) + + pxr_anchor_quat = pxr_cfg_quat + + if self._xr_cfg.anchor_rotation_mode in ( + XrAnchorRotationMode.FOLLOW_PRIM, + XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED, + ): + rt_prim_quat = rt_matrix.ExtractRotationQuat() + rt_delta_quat = rt_prim_quat * self.__anchor_prim_initial_quat.GetInverse() + pxr_delta_quat = pxrGf.Quatd(rt_delta_quat.GetReal(), pxrGf.Vec3d(*rt_delta_quat.GetImaginary())) + + # yaw-only about Z (right-handed, Z-up) + wq = pxr_delta_quat.GetReal() + ix, iy, iz = pxr_delta_quat.GetImaginary() + yaw = math.atan2(2.0 * (wq * iz + ix * iy), 1.0 - 2.0 * (iy * iy + iz * iz)) + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + pxr_delta_yaw_only_quat = pxrGf.Quatd(cy, pxrGf.Vec3d(0.0, 0.0, sy)) + pxr_anchor_quat = pxr_delta_yaw_only_quat * pxr_cfg_quat + + if self._xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED: + if self.__smoothed_anchor_quat is None: + self.__smoothed_anchor_quat = pxr_anchor_quat + else: + dt = SimulationContext.instance().get_rendering_dt() + alpha = 1.0 - math.exp(-dt / max(self._xr_cfg.anchor_rotation_smoothing_time, 1e-6)) + alpha = min(1.0, max(0.05, alpha)) + self.__smoothed_anchor_quat = pxrGf.Slerp(alpha, self.__smoothed_anchor_quat, pxr_anchor_quat) + pxr_anchor_quat = self.__smoothed_anchor_quat + + elif self._xr_cfg.anchor_rotation_mode == XrAnchorRotationMode.CUSTOM: + if self._xr_cfg.anchor_rotation_custom_func is not None: + rt_prim_quat = rt_matrix.ExtractRotationQuat() + anchor_prim_pose = np.array( + [ + rt_pos[0], + rt_pos[1], + rt_pos[2], + rt_prim_quat.GetReal(), + rt_prim_quat.GetImaginary()[0], + rt_prim_quat.GetImaginary()[1], + rt_prim_quat.GetImaginary()[2], + ], + dtype=np.float64, + ) + # Previous headpose must be provided by caller; fall back to zeros. + prev_head = getattr(self, "_previous_headpose", np.zeros(7, dtype=np.float64)) + np_array_quat = self._xr_cfg.anchor_rotation_custom_func(prev_head, anchor_prim_pose) + w, x, y, z = np_array_quat + pxr_anchor_quat = pxrGf.Quatd(w, pxrGf.Vec3d(x, y, z)) + + pxr_mat = pxrGf.Matrix4d() + pxr_mat.SetTranslateOnly(pxr_anchor_pos) + + if self.__anchor_rotation_enabled: + pxr_mat.SetRotateOnly(pxr_anchor_quat) + self.__last_anchor_quat = pxr_anchor_quat + else: + + if self.__last_anchor_quat is None: + self.__last_anchor_quat = pxr_anchor_quat + + pxr_mat.SetRotateOnly(self.__last_anchor_quat) + self.__smoothed_anchor_quat = self.__last_anchor_quat + + self._xr_core.set_world_transform_matrix( + self._xr_anchor_headset_path, pxr_mat, self.__anchor_headset_layer_identifier + ) + except Exception as e: + logger.warning(f"XR: Anchor sync failed: {e}") diff --git a/source/isaaclab/isaaclab/devices/openxr/xr_cfg.py b/source/isaaclab/isaaclab/devices/openxr/xr_cfg.py index 7da044f0211..ec35fc0219a 100644 --- a/source/isaaclab/isaaclab/devices/openxr/xr_cfg.py +++ b/source/isaaclab/isaaclab/devices/openxr/xr_cfg.py @@ -8,9 +8,29 @@ from __future__ import annotations +import enum +import numpy as np +from collections.abc import Callable + from isaaclab.utils import configclass +class XrAnchorRotationMode(enum.Enum): + """Enumeration for XR anchor rotation modes.""" + + FIXED = "fixed" + """Fixed rotation mode: sets rotation once and doesn't change it.""" + + FOLLOW_PRIM = "follow_prim" + """Follow prim rotation mode: rotation follows prim's rotation.""" + + FOLLOW_PRIM_SMOOTHED = "follow_prim_smoothed" + """Follow prim rotation mode with smooth interpolation: rotation smoothly follows prim's rotation using slerp.""" + + CUSTOM = "custom_rotation" + """Custom rotation mode: user provided function to calculate the rotation.""" + + @configclass class XrCfg: """Configuration for viewing and interacting with the environment through an XR device.""" @@ -30,12 +50,60 @@ class XrCfg: This quantity is only effective if :attr:`xr_anchor_pos` is set. """ + anchor_prim_path: str | None = None + """Specifies the prim path to attach the XR anchor to for dynamic positioning. + + When set, the XR anchor will be attached to the specified prim (e.g., robot root prim), + allowing the XR camera to move with the prim. This is particularly useful for locomotion + robot teleoperation where the robot moves and the XR camera should follow it. + + If None, the anchor will use the static :attr:`anchor_pos` and :attr:`anchor_rot` values. + """ + + anchor_rotation_mode: XrAnchorRotationMode = XrAnchorRotationMode.FIXED + """Specifies how the XR anchor rotation should behave when attached to a prim. + + The available modes are: + - :attr:`XrAnchorRotationMode.FIXED`: Sets rotation once to anchor_rot value + - :attr:`XrAnchorRotationMode.FOLLOW_PRIM`: Rotation follows prim's rotation + - :attr:`XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED`: Rotation smoothly follows prim's rotation using slerp + - :attr:`XrAnchorRotationMode.CUSTOM`: user provided function to calculate the rotation + """ + + anchor_rotation_smoothing_time: float = 1.0 + """Wall-clock time constant (seconds) for rotation smoothing in FOLLOW_PRIM_SMOOTHED mode. + + This time constant is applied using wall-clock delta time between frames (not physics dt). + Smaller values (e.g., 0.1) result in faster/snappier response but less smoothing. + Larger values (e.g., 0.75–2.0) result in slower/smoother response but more lag. + Typical useful range: 0.3 – 1.5 seconds depending on runtime frame-rate and comfort. + """ + + anchor_rotation_custom_func: Callable[[np.ndarray, np.ndarray], np.ndarray] = lambda headpose, primpose: np.array( + [1, 0, 0, 0], dtype=np.float64 + ) + """Specifies the function to calculate the rotation of the XR anchor when anchor_rotation_mode is CUSTOM. + + Args: + headpose: Previous head pose as numpy array [x, y, z, w, x, y, z] (position + quaternion) + pose: Anchor prim pose as numpy array [x, y, z, w, x, y, z] (position + quaternion) + + Returns: + np.ndarray: Quaternion as numpy array [w, x, y, z] + """ + near_plane: float = 0.15 """Specifies the near plane distance for the XR device. This value determines the closest distance at which objects will be rendered in the XR device. """ + fixed_anchor_height: bool = True + """Specifies if the anchor height should be fixed. + + If True, the anchor height will be fixed to the initial height of the anchor prim. + """ + from typing import Any diff --git a/source/isaaclab/isaaclab/devices/retargeter_base.py b/source/isaaclab/isaaclab/devices/retargeter_base.py index c4abcf8bf9c..c3d2e51f6b8 100644 --- a/source/isaaclab/isaaclab/devices/retargeter_base.py +++ b/source/isaaclab/isaaclab/devices/retargeter_base.py @@ -5,6 +5,7 @@ from abc import ABC, abstractmethod from dataclasses import dataclass +from enum import Enum from typing import Any @@ -36,6 +37,13 @@ def __init__(self, cfg: RetargeterCfg): """ self._sim_device = cfg.sim_device + class Requirement(Enum): + """Features a retargeter may require from a device's raw data feed.""" + + HAND_TRACKING = "hand_tracking" + HEAD_TRACKING = "head_tracking" + MOTION_CONTROLLER = "motion_controller" + @abstractmethod def retarget(self, data: Any) -> Any: """Retarget input data to desired output format. @@ -47,3 +55,15 @@ def retarget(self, data: Any) -> Any: Retargeted data in implementation-specific format """ pass + + def get_requirements(self) -> list["RetargeterBase.Requirement"]: + """Return the list of required data features for this retargeter. + + Defaults to requesting all available features for backward compatibility. + Implementations should override to narrow to only what they need. + """ + return [ + RetargeterBase.Requirement.HAND_TRACKING, + RetargeterBase.Requirement.HEAD_TRACKING, + RetargeterBase.Requirement.MOTION_CONTROLLER, + ] diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py index bc8455f2aee..12b322f0275 100644 --- a/source/isaaclab/isaaclab/devices/teleop_device_factory.py +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -83,5 +83,5 @@ def create_teleop_device( for key, callback in callbacks.items(): device.add_callback(key, callback) - logger.info(f"Created teleoperation device: {device_name}") + logging.info(f"Created teleoperation device: {device_name}") return device diff --git a/source/isaaclab/test/devices/test_oxr_device.py b/source/isaaclab/test/devices/test_oxr_device.py index 6bf805d3272..7512333d80c 100644 --- a/source/isaaclab/test/devices/test_oxr_device.py +++ b/source/isaaclab/test/devices/test_oxr_device.py @@ -19,6 +19,7 @@ import importlib import numpy as np +import torch import carb import omni.usd @@ -27,11 +28,30 @@ from isaaclab.devices import OpenXRDevice, OpenXRDeviceCfg from isaaclab.devices.openxr import XrCfg +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg from isaaclab.scene import InteractiveSceneCfg from isaaclab.utils import configclass +class NoOpRetargeter(RetargeterBase): + """A no-op retargeter that requests hand and head tracking but returns empty tensor.""" + + def __init__(self, cfg: RetargeterCfg): + super().__init__(cfg) + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + """Request hand and head tracking to trigger data collection.""" + return [ + RetargeterBase.Requirement.HAND_TRACKING, + RetargeterBase.Requirement.HEAD_TRACKING, + ] + + def retarget(self, data): + """Return empty tensor.""" + return torch.tensor([], device=self._sim_device) + + @configclass class EmptyManagerCfg: """Empty manager.""" @@ -159,7 +179,7 @@ def test_xr_anchor(empty_env, mock_xrcore): device = OpenXRDevice(OpenXRDeviceCfg(xr_cfg=env_cfg.xr)) # Check that the xr anchor prim is created with the correct pose - xr_anchor_prim = XFormPrim("/XRAnchor") + xr_anchor_prim = XFormPrim("/World/XRAnchor") assert xr_anchor_prim.is_valid() position, orientation = xr_anchor_prim.get_world_poses() @@ -168,7 +188,7 @@ def test_xr_anchor(empty_env, mock_xrcore): # Check that xr anchor mode and custom anchor are set correctly assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor" - assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/XRAnchor" + assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor" device.reset() @@ -181,7 +201,7 @@ def test_xr_anchor_default(empty_env, mock_xrcore): device = OpenXRDevice(OpenXRDeviceCfg()) # Check that the xr anchor prim is created with the correct default pose - xr_anchor_prim = XFormPrim("/XRAnchor") + xr_anchor_prim = XFormPrim("/World/XRAnchor") assert xr_anchor_prim.is_valid() position, orientation = xr_anchor_prim.get_world_poses() @@ -190,7 +210,7 @@ def test_xr_anchor_default(empty_env, mock_xrcore): # Check that xr anchor mode and custom anchor are set correctly assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor" - assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/XRAnchor" + assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor" device.reset() @@ -204,7 +224,7 @@ def test_xr_anchor_multiple_devices(empty_env, mock_xrcore): device_2 = OpenXRDevice(OpenXRDeviceCfg()) # Check that the xr anchor prim is created with the correct default pose - xr_anchor_prim = XFormPrim("/XRAnchor") + xr_anchor_prim = XFormPrim("/World/XRAnchor") assert xr_anchor_prim.is_valid() position, orientation = xr_anchor_prim.get_world_poses() @@ -213,7 +233,7 @@ def test_xr_anchor_multiple_devices(empty_env, mock_xrcore): # Check that xr anchor mode and custom anchor are set correctly assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor" - assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/XRAnchor" + assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor" device_1.reset() device_2.reset() @@ -223,19 +243,22 @@ def test_xr_anchor_multiple_devices(empty_env, mock_xrcore): def test_get_raw_data(empty_env, mock_xrcore): """Test the _get_raw_data method returns correctly formatted tracking data.""" env, _ = empty_env - # Create a proper config object with default values - device = OpenXRDevice(OpenXRDeviceCfg()) + # Create a proper config object with default values and a no-op retargeter to trigger data collection + retargeter = NoOpRetargeter(RetargeterCfg()) + device = OpenXRDevice(OpenXRDeviceCfg(), retargeters=[retargeter]) # Get raw tracking data raw_data = device._get_raw_data() # Check that the data structure is as expected - assert OpenXRDevice.TrackingTarget.HAND_LEFT in raw_data - assert OpenXRDevice.TrackingTarget.HAND_RIGHT in raw_data - assert OpenXRDevice.TrackingTarget.HEAD in raw_data + from isaaclab.devices.device_base import DeviceBase + + assert DeviceBase.TrackingTarget.HAND_LEFT in raw_data + assert DeviceBase.TrackingTarget.HAND_RIGHT in raw_data + assert DeviceBase.TrackingTarget.HEAD in raw_data # Check left hand joints - left_hand = raw_data[OpenXRDevice.TrackingTarget.HAND_LEFT] + left_hand = raw_data[DeviceBase.TrackingTarget.HAND_LEFT] assert "palm" in left_hand assert "wrist" in left_hand @@ -246,7 +269,7 @@ def test_get_raw_data(empty_env, mock_xrcore): np.testing.assert_almost_equal(palm_pose[3:], [0.9, 0.1, 0.2, 0.3]) # Orientation # Check head pose - head_pose = raw_data[OpenXRDevice.TrackingTarget.HEAD] + head_pose = raw_data[DeviceBase.TrackingTarget.HEAD] assert len(head_pose) == 7 np.testing.assert_almost_equal(head_pose[:3], [0.1, 0.2, 0.3]) # Position np.testing.assert_almost_equal(head_pose[3:], [0.9, 0.1, 0.2, 0.3]) # Orientation diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index bc01e841fdb..ef079ce9112 100644 --- a/source/isaaclab_tasks/config/extension.toml +++ b/source/isaaclab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.11.8" +version = "0.11.9" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 264ead17a66..518c3a02541 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,21 @@ Changelog --------- +0.11.9 (2025-11-10) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added OpenXR motion controller support for the G1 robot locomanipulation environment + ``Isaac-PickPlace-Locomanipulation-G1-Abs-v0``. This enables teleoperation using XR motion controllers + in addition to hand tracking. +* Added :class:`OpenXRDeviceMotionController` for motion controller-based teleoperation with headset anchoring control. +* Added motion controller-specific retargeters: + * :class:`G1TriHandControllerUpperBodyRetargeterCfg` for upper body and hand control using motion controllers. + * :class:`G1LowerBodyStandingControllerRetargeterCfg` for lower body control using motion controllers. + + 0.11.8 (2025-11-06) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py index bf09c7f0426..e460c12d662 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/locomanipulation_g1_env_cfg.py @@ -3,7 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause - from isaaclab_assets.robots.unitree import G1_29DOF_CFG import isaaclab.envs.mdp as base_mdp @@ -12,9 +11,16 @@ from isaaclab.devices.device_base import DevicesCfg from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg from isaaclab.devices.openxr.retargeters.humanoid.unitree.g1_lower_body_standing import G1LowerBodyStandingRetargeterCfg +from isaaclab.devices.openxr.retargeters.humanoid.unitree.g1_motion_controller_locomotion import ( + G1LowerBodyStandingMotionControllerRetargeterCfg, +) +from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_motion_ctrl_retargeter import ( + G1TriHandUpperBodyMotionControllerRetargeterCfg, +) from isaaclab.devices.openxr.retargeters.humanoid.unitree.trihand.g1_upper_body_retargeter import ( G1TriHandUpperBodyRetargeterCfg, ) +from isaaclab.devices.openxr.xr_cfg import XrAnchorRotationMode from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm @@ -207,6 +213,11 @@ def __post_init__(self): # Retrieve local paths for the URDF and mesh files. Will be cached for call after the first time. self.actions.upper_body_ik.controller.urdf_path = retrieve_file_path(urdf_omniverse_path) + self.xr.anchor_prim_path = "/World/envs/env_0/Robot/pelvis" + self.xr.fixed_anchor_height = True + # Ensure XR anchor rotation follows the robot pelvis (yaw only), with smoothing for comfort + self.xr.anchor_rotation_mode = XrAnchorRotationMode.FOLLOW_PRIM_SMOOTHED + self.teleop_devices = DevicesCfg( devices={ "handtracking": OpenXRDeviceCfg( @@ -225,5 +236,19 @@ def __post_init__(self): sim_device=self.sim.device, xr_cfg=self.xr, ), + "motion_controllers": OpenXRDeviceCfg( + retargeters=[ + G1TriHandUpperBodyMotionControllerRetargeterCfg( + enable_visualization=True, + sim_device=self.sim.device, + hand_joint_names=self.actions.upper_body_ik.hand_joint_names, + ), + G1LowerBodyStandingMotionControllerRetargeterCfg( + sim_device=self.sim.device, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), } ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py index 17dbe0ce2a7..add822599ad 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_abs_env_cfg.py @@ -4,8 +4,8 @@ # SPDX-License-Identifier: BSD-3-Clause from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg -from isaaclab.devices.device_base import DevicesCfg -from isaaclab.devices.openxr.openxr_device import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg from isaaclab.devices.openxr.retargeters.manipulator.se3_abs_retargeter import Se3AbsRetargeterCfg from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg @@ -42,14 +42,14 @@ def __post_init__(self): "handtracking": OpenXRDeviceCfg( retargeters=[ Se3AbsRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, zero_out_xy_rotation=True, use_wrist_rotation=False, use_wrist_position=True, sim_device=self.sim.device, ), GripperRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device ), ], sim_device=self.sim.device, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py index f173ee644ce..a543d7fe124 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg.py @@ -4,9 +4,9 @@ # SPDX-License-Identifier: BSD-3-Clause from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg -from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg -from isaaclab.devices.openxr.openxr_device import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg from isaaclab.devices.openxr.retargeters.manipulator.se3_rel_retargeter import Se3RelRetargeterCfg from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg @@ -45,7 +45,7 @@ def __post_init__(self): "handtracking": OpenXRDeviceCfg( retargeters=[ Se3RelRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, zero_out_xy_rotation=True, use_wrist_rotation=False, use_wrist_position=True, @@ -54,7 +54,7 @@ def __post_init__(self): sim_device=self.sim.device, ), GripperRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device ), ], sim_device=self.sim.device, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg_skillgen.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg_skillgen.py index b95640be8a7..10da599d3d9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg_skillgen.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/franka/stack_ik_rel_env_cfg_skillgen.py @@ -4,9 +4,9 @@ # SPDX-License-Identifier: BSD-3-Clause from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg -from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg -from isaaclab.devices.openxr.openxr_device import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg from isaaclab.devices.openxr.retargeters.manipulator.gripper_retargeter import GripperRetargeterCfg from isaaclab.devices.openxr.retargeters.manipulator.se3_rel_retargeter import Se3RelRetargeterCfg from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg @@ -129,7 +129,7 @@ def __post_init__(self): "handtracking": OpenXRDeviceCfg( retargeters=[ Se3RelRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, zero_out_xy_rotation=True, use_wrist_rotation=False, use_wrist_position=True, @@ -138,7 +138,7 @@ def __post_init__(self): sim_device=self.sim.device, ), GripperRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device ), ], sim_device=self.sim.device, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py index cdf9baeb4e5..ff8df74a196 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_joint_pos_env_cfg.py @@ -6,7 +6,8 @@ from isaaclab.assets import RigidObjectCfg, SurfaceGripperCfg from isaaclab.devices import DevicesCfg -from isaaclab.devices.openxr import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg from isaaclab.devices.openxr.retargeters import GripperRetargeterCfg, Se3AbsRetargeterCfg from isaaclab.envs.mdp.actions.actions_cfg import SurfaceGripperBinaryActionCfg from isaaclab.managers import EventTermCfg as EventTerm @@ -252,14 +253,14 @@ def __post_init__(self): "handtracking": OpenXRDeviceCfg( retargeters=[ Se3AbsRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_LEFT, + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, zero_out_xy_rotation=True, use_wrist_rotation=False, use_wrist_position=True, sim_device=self.sim.device, ), GripperRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_LEFT, sim_device=self.sim.device + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, sim_device=self.sim.device ), ], sim_device=self.sim.device, @@ -310,14 +311,14 @@ def __post_init__(self): "handtracking": OpenXRDeviceCfg( retargeters=[ Se3AbsRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, zero_out_xy_rotation=True, use_wrist_rotation=False, use_wrist_position=True, sim_device=self.sim.device, ), GripperRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device ), ], sim_device=self.sim.device, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py index 1ee49d9db18..a1c61cb87fb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/stack/config/galbot/stack_rmp_rel_env_cfg.py @@ -7,9 +7,9 @@ import os import isaaclab.sim as sim_utils -from isaaclab.devices.device_base import DevicesCfg +from isaaclab.devices.device_base import DeviceBase, DevicesCfg from isaaclab.devices.keyboard import Se3KeyboardCfg -from isaaclab.devices.openxr import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.openxr.openxr_device import OpenXRDeviceCfg from isaaclab.devices.openxr.retargeters import GripperRetargeterCfg, Se3RelRetargeterCfg from isaaclab.devices.spacemouse import Se3SpaceMouseCfg from isaaclab.envs.mdp.actions.rmpflow_actions_cfg import RMPFlowActionCfg @@ -80,7 +80,7 @@ def __post_init__(self): "handtracking": OpenXRDeviceCfg( retargeters=[ Se3RelRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_LEFT, + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, zero_out_xy_rotation=True, use_wrist_rotation=False, use_wrist_position=True, @@ -89,7 +89,7 @@ def __post_init__(self): sim_device=self.sim.device, ), GripperRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_LEFT, sim_device=self.sim.device + bound_hand=DeviceBase.TrackingTarget.HAND_LEFT, sim_device=self.sim.device ), ], sim_device=self.sim.device, @@ -150,7 +150,7 @@ def __post_init__(self): "handtracking": OpenXRDeviceCfg( retargeters=[ Se3RelRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, zero_out_xy_rotation=True, use_wrist_rotation=False, use_wrist_position=True, @@ -159,7 +159,7 @@ def __post_init__(self): sim_device=self.sim.device, ), GripperRetargeterCfg( - bound_hand=OpenXRDevice.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device + bound_hand=DeviceBase.TrackingTarget.HAND_RIGHT, sim_device=self.sim.device ), ], sim_device=self.sim.device,