From c55a5c2783737d166155f9817de3bc69932e2d49 Mon Sep 17 00:00:00 2001 From: Hougant Chen Date: Wed, 22 Oct 2025 16:47:37 -0400 Subject: [PATCH 1/3] Add quest support for local loco-manipulation * Switch delta time to use wall clock time instead of physics step time. Update config values to compensate. * Add support for decoupling anchor rotation as a toggle when pressing right controller A button. * Fix reset not working Refactor motion controller specific code into OpenXRDeviceMotionController. If we are doing only "handtracking" as teleop_device, then we don't want all the headset anchoring code. Change the pre render callbacks to be a list so other systems can hook into it as well Rename classes from Controller to MotionController to limit confusion with Robotics controllers Move the code to alter the stage until after the telop device has been instantiated so we know what device we dealing with and if we need to change the ground plane material Format with ./isaaclab -f Update changelog, contributors and extension.toml Switch to rendering_dt instead of wall clock for determinism Address AI code review comments. Instead of having fixed heigh on by default, make it optional in the case of being able to go into crouch. We don't need to add pre render to ManagerBasedEnv since that can be done through XRCore. Moving new motion controller retargeters into new files Moving OpenXRDevice motion controller logic backup into OpenXRDevice, and moving interface enums into DeviceBase to remove retargeter dependancy on OpenXRDevice Adding retargeter requiremnts call Removing bespoke xr env adjustments Undo xr anchor height offset Cleaning up teleop script Code cleanup Update code comments Fix typo in RetargeterBase Properly set sim.device for retargeters Fixing greptile feedback Fix logic error if xr anchor rotation is disabled on AR startup Follow naming conventions for get_requirments --- CONTRIBUTORS.md | 1 + docs/source/api/lab/isaaclab.devices.rst | 13 + docs/source/how-to/cloudxr_teleoperation.rst | 8 +- .../teleoperation/teleop_se3_agent.py | 15 +- source/isaaclab/docs/CHANGELOG.rst | 14 + .../isaaclab/isaaclab/devices/device_base.py | 36 ++- .../isaaclab/devices/openxr/__init__.py | 2 +- .../isaaclab/devices/openxr/manus_vive.py | 15 +- .../isaaclab/devices/openxr/openxr_device.py | 258 ++++++++++++++++-- .../devices/openxr/retargeters/__init__.py | 8 + .../humanoid/fourier/gr1t2_retargeter.py | 10 +- .../unitree/g1_lower_body_standing.py | 5 + .../g1_motion_controller_locomotion.py | 85 ++++++ .../inspire/g1_upper_body_retargeter.py | 10 +- .../g1_upper_body_motion_ctrl_retargeter.py | 216 +++++++++++++++ .../trihand/g1_upper_body_retargeter.py | 12 +- .../manipulator/gripper_retargeter.py | 12 +- .../manipulator/se3_abs_retargeter.py | 14 +- .../manipulator/se3_rel_retargeter.py | 14 +- .../devices/openxr/xr_anchor_utils.py | 176 ++++++++++++ .../isaaclab/devices/openxr/xr_cfg.py | 68 +++++ .../isaaclab/devices/retargeter_base.py | 20 ++ .../isaaclab/devices/teleop_device_factory.py | 2 +- .../isaaclab/test/devices/test_oxr_device.py | 12 +- source/isaaclab_tasks/config/extension.toml | 2 +- source/isaaclab_tasks/docs/CHANGELOG.rst | 14 + .../pick_place/locomanipulation_g1_env_cfg.py | 27 +- .../config/franka/stack_ik_abs_env_cfg.py | 8 +- .../config/franka/stack_ik_rel_env_cfg.py | 8 +- .../franka/stack_ik_rel_env_cfg_skillgen.py | 8 +- .../config/galbot/stack_joint_pos_env_cfg.py | 11 +- .../config/galbot/stack_rmp_rel_env_cfg.py | 12 +- 32 files changed, 1004 insertions(+), 112 deletions(-) create mode 100644 source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_motion_controller_locomotion.py create mode 100644 source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_motion_ctrl_retargeter.py create mode 100644 source/isaaclab/isaaclab/devices/openxr/xr_anchor_utils.py diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index b906922b291..3e6006e17f5 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/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 6cc64b7cb8e..d297f8d9521 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,20 @@ Changelog --------- +0.48.5 (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.4 (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..8d1b8323d77 100644 --- a/source/isaaclab/test/devices/test_oxr_device.py +++ b/source/isaaclab/test/devices/test_oxr_device.py @@ -230,12 +230,14 @@ def test_get_raw_data(empty_env, mock_xrcore): 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 +248,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..11a01ff6d20 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,20 @@ 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, From e2ea1e38ea72fc165661393d6fdd90ce505d51e0 Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Tue, 18 Nov 2025 01:34:29 -0500 Subject: [PATCH 2/3] fixing unit tests and docs --- source/isaaclab/docs/CHANGELOG.rst | 1 + .../isaaclab/test/devices/test_oxr_device.py | 37 +++++++++++++++---- source/isaaclab_tasks/docs/CHANGELOG.rst | 3 +- 3 files changed, 32 insertions(+), 9 deletions(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index d297f8d9521..a640d238052 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -2,6 +2,7 @@ Changelog --------- 0.48.5 (2025-11-18) +~~~~~~~~~~~~~~~~~~~ Added ^^^^^ diff --git a/source/isaaclab/test/devices/test_oxr_device.py b/source/isaaclab/test/devices/test_oxr_device.py index 8d1b8323d77..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,8 +243,9 @@ 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() diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 11a01ff6d20..518c3a02541 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog --------- 0.11.9 (2025-11-10) -~~~~~~~~~~~~~~~~~~~~ +~~~~~~~~~~~~~~~~~~~ Added ^^^^^ @@ -15,6 +15,7 @@ Added * :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) ~~~~~~~~~~~~~~~~~~~ From 31025b2a9e6d12d8f5c96e2c2e68bfd2911e364d Mon Sep 17 00:00:00 2001 From: Kelly Guo Date: Mon, 24 Nov 2025 15:32:00 -0800 Subject: [PATCH 3/3] Bump version to 0.48.6 Signed-off-by: Kelly Guo --- source/isaaclab/config/extension.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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"