diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index aaef502a2e82..73893d8217a6 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -53,6 +53,7 @@ Guidelines for modifications: * Brian Bingham * Cameron Upright * Calvin Yu +* Cathy Y. Li * Cheng-Rong Lai * Chenyu Yang * Connor Smith diff --git a/docs/source/api/lab/isaaclab.devices.rst b/docs/source/api/lab/isaaclab.devices.rst index 3d0eb1da801d..1a2ed776d3b0 100644 --- a/docs/source/api/lab/isaaclab.devices.rst +++ b/docs/source/api/lab/isaaclab.devices.rst @@ -16,6 +16,7 @@ Se2SpaceMouse Se3SpaceMouse OpenXRDevice + ManusVive isaaclab.devices.openxr.retargeters.GripperRetargeter isaaclab.devices.openxr.retargeters.Se3AbsRetargeter isaaclab.devices.openxr.retargeters.Se3RelRetargeter @@ -86,6 +87,14 @@ OpenXR :inherited-members: :show-inheritance: +Manus + Vive +------------ + +.. autoclass:: ManusVive + :members: + :inherited-members: + :show-inheritance: + Retargeters ----------- diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst index c4523acbeb6a..0b7c8c9c017c 100644 --- a/docs/source/how-to/cloudxr_teleoperation.rst +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -390,6 +390,38 @@ Back on your Apple Vision Pro: and build teleoperation and imitation learning workflows with Isaac Lab. +.. _manus-vive-handtracking: + +Manus + Vive Hand Tracking +~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Manus gloves and HTC Vive trackers can provide hand tracking when optical hand tracking from a headset is occluded. +This setup expects Manus gloves with a Manus SDK license and Vive trackers attached to the gloves. +Requires Isaac Sim >=5.1. + +Run the teleoperation example with Manus + Vive tracking: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-PickPlace-GR1T2-Abs-v0 \ + --teleop_device manusvive \ + --xr \ + --enable_pinocchio + +Begin the session with your palms facing up. +This is necessary for calibrating Vive tracker poses using Apple Vision Pro wrist poses from a few initial frames, +as the Vive trackers attached to the back of the hands occlude the optical hand tracking. + +.. note:: + + To avoid resource contention and crashes, ensure Manus and Vive devices are connected to different USB controllers/buses. + Use ``lsusb -t`` to identify different buses and connect devices accordingly. + + Vive trackers are automatically calculated to map to the left and right wrist joints. + This auto-mapping calculation supports up to 2 Vive trackers; + if more than 2 Vive trackers are detected, it uses the first two trackers detected for calibration, which may not be correct. + .. _develop-xr-isaac-lab: Develop for XR in Isaac Lab diff --git a/scripts/environments/teleoperation/teleop_se3_agent.py b/scripts/environments/teleoperation/teleop_se3_agent.py index cb0f151380ba..021ee5ff80ff 100644 --- a/scripts/environments/teleoperation/teleop_se3_agent.py +++ b/scripts/environments/teleoperation/teleop_se3_agent.py @@ -19,8 +19,7 @@ "--teleop_device", type=str, default="keyboard", - choices=["keyboard", "spacemouse", "gamepad", "handtracking"], - help="Device for interacting with environment", + help="Device for interacting with environment. Examples: keyboard, spacemouse, gamepad, handtracking, manusvive", ) parser.add_argument("--task", type=str, default=None, help="Name of the task.") parser.add_argument("--sensitivity", type=float, default=1.0, help="Sensitivity factor.") diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 792634e10528..8b426e2d302b 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.45.12" +version = "0.45.13" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 1578f89be9d7..6789061e9143 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,15 @@ Changelog --------- +0.45.13 (2025-09-08) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.devices.openxr.manus_vive.ManusVive` to support teleoperation with Manus gloves and Vive trackers. + + 0.45.12 (2025-09-05) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/devices/__init__.py b/source/isaaclab/isaaclab/devices/__init__.py index 41dd348d53fd..718695e3503a 100644 --- a/source/isaaclab/isaaclab/devices/__init__.py +++ b/source/isaaclab/isaaclab/devices/__init__.py @@ -22,7 +22,7 @@ from .device_base import DeviceBase, DeviceCfg, DevicesCfg from .gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg from .keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg -from .openxr import OpenXRDevice, OpenXRDeviceCfg +from .openxr import ManusVive, ManusViveCfg, OpenXRDevice, OpenXRDeviceCfg from .retargeter_base import RetargeterBase, RetargeterCfg from .spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg from .teleop_device_factory import create_teleop_device diff --git a/source/isaaclab/isaaclab/devices/openxr/__init__.py b/source/isaaclab/isaaclab/devices/openxr/__init__.py index eaa2ccc42f04..e7bc0cfda038 100644 --- a/source/isaaclab/isaaclab/devices/openxr/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/__init__.py @@ -5,5 +5,6 @@ """Keyboard device for SE(2) and SE(3) control.""" +from .manus_vive import ManusVive, ManusViveCfg from .openxr_device import OpenXRDevice, OpenXRDeviceCfg from .xr_cfg import XrCfg, remove_camera_configs diff --git a/source/isaaclab/isaaclab/devices/openxr/manus_vive.py b/source/isaaclab/isaaclab/devices/openxr/manus_vive.py new file mode 100644 index 000000000000..4dda777843f2 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/manus_vive.py @@ -0,0 +1,248 @@ +# 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 + +""" +Manus and Vive for teleoperation and interaction. +""" + +import contextlib +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 + +from isaaclab.devices.openxr.common import HAND_JOINT_NAMES +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 +XRCore = None + +with contextlib.suppress(ModuleNotFoundError): + from omni.kit.xr.core import XRCore + +from isaacsim.core.prims import SingleXFormPrim + +from .manus_vive_utils import HAND_JOINT_MAP, ManusViveIntegration + + +@dataclass +class ManusViveCfg(DeviceCfg): + """Configuration for Manus and Vive.""" + + xr_cfg: XrCfg | None = None + + +class ManusVive(DeviceBase): + """Manus gloves and Vive trackers for teleoperation and interaction. + + This device tracks hand joints using Manus gloves and Vive trackers and makes them available as: + + 1. A dictionary of tracking data (when used without retargeters) + 2. Retargeted commands for robot control (when retargeters are provided) + + The user needs to install the Manus SDK and add `{path_to_manus_sdk}/manus_sdk/lib` to `LD_LIBRARY_PATH`. + Data are acquired by `ManusViveIntegration` from `isaaclab.devices.openxr.manus_vive_utils`, including + + * Vive tracker poses in scene frame, calibrated from AVP wrist poses. + * Hand joints calculated from Vive wrist joints and Manus hand joints (relative to wrist). + * Vive trackers are automatically mapped to the left and right wrist joints. + + Raw data format (_get_raw_data output): consistent with :class:`OpenXRDevice`. + Joint names are defined in `HAND_JOINT_MAP` from `isaaclab.devices.openxr.manus_vive_utils`. + + Teleop commands: consistent with :class:`OpenXRDevice`. + + The device tracks the left hand, right hand, head position, or any combination of these + based on the TrackingTarget enum values. When retargeters are provided, the raw tracking + 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): + """Initialize the Manus+Vive device. + + Args: + cfg: Configuration object for Manus+Vive settings. + retargeters: List of retargeter instances to use for transforming raw tracking data. + """ + super().__init__(retargeters) + # Enforce minimum Isaac Sim version (>= 5.1) + version_info = get_version() + major, minor = int(version_info[2]), int(version_info[3]) + if (major < 5) or (major == 5 and minor < 1): + raise RuntimeError(f"ManusVive requires Isaac Sim >= 5.1. Detected version {major}.{minor}. ") + self._xr_cfg = cfg.xr_cfg or XrCfg() + self._additional_callbacks = dict() + self._vc_subscription = ( + XRCore.get_singleton() + .get_message_bus() + .create_subscription_to_pop_by_type( + carb.events.type_from_string(self.TELEOP_COMMAND_EVENT_TYPE), self._on_teleop_command + ) + ) + self._manus_vive = ManusViveIntegration() + + # Initialize dictionaries instead of arrays + default_pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + 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() + + 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) + + def __del__(self): + """Clean up resources when the object is destroyed. + Properly unsubscribes from the XR message bus to prevent memory leaks + and resource issues when the device is no longer needed. + """ + if hasattr(self, "_vc_subscription") and self._vc_subscription is not None: + self._vc_subscription = None + + # No need to explicitly clean up OpenXR instance as it's managed by NVIDIA Isaac Sim + + def __str__(self) -> str: + """Provide details about the device configuration, tracking settings, + and available gesture commands. + + Returns: + Formatted string with device information. + """ + + msg = f"Manus+Vive 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" + + # Add retargeter information + if self._retargeters: + msg += "\tRetargeters:\n" + for i, retargeter in enumerate(self._retargeters): + msg += f"\t\t{i + 1}. {retargeter.__class__.__name__}\n" + else: + msg += "\tRetargeters: None (raw joint data output)\n" + + # Add available gesture commands + msg += "\t----------------------------------------------\n" + msg += "\tAvailable Gesture Commands:\n" + + # Check which callbacks are registered + start_avail = "START" in self._additional_callbacks + stop_avail = "STOP" in self._additional_callbacks + reset_avail = "RESET" in self._additional_callbacks + + msg += f"\t\tStart Teleoperation: {'✓' if start_avail else '✗'}\n" + msg += f"\t\tStop Teleoperation: {'✓' if stop_avail else '✗'}\n" + msg += f"\t\tReset Environment: {'✓' if reset_avail else '✗'}\n" + + # Add joint tracking information + msg += "\t----------------------------------------------\n" + msg += "\tTracked Joints: 26 XR hand joints including:\n" + msg += "\t\t- Wrist, palm\n" + msg += "\t\t- Thumb (tip, intermediate joints)\n" + msg += "\t\t- Fingers (tip, distal, intermediate, proximal)\n" + + return msg + + def reset(self): + """Reset cached joint and head poses.""" + default_pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32) + 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() + + def add_callback(self, key: str, func: Callable): + """Register a callback for a given key. + + Args: + key: The message key to bind ('START', 'STOP', 'RESET'). + func: The function to invoke when the message key is received. + """ + self._additional_callbacks[key] = func + + def _get_raw_data(self) -> dict: + """Get the latest tracking data from Manus and Vive. + + Returns: + Dictionary with TrackingTarget enum keys (HAND_LEFT, HAND_RIGHT, HEAD) containing: + - Left hand joint poses: Dictionary of 26 joints with position and orientation + - Right hand joint poses: Dictionary of 26 joints with position and orientation + - Head pose: Single 7-element array with position and orientation + + 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. + """ + hand_tracking_data = self._manus_vive.get_all_device_data()["manus_gloves"] + result = {"left": self._previous_joint_poses_left, "right": self._previous_joint_poses_right} + for joint, pose in hand_tracking_data.items(): + hand, index = joint.split("_") + 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(), + } + + def _calculate_headpose(self) -> np.ndarray: + """Calculate the head pose from OpenXR. + + Returns: + 7-element numpy.ndarray [x, y, z, qw, qx, qy, qz]. + """ + head_device = XRCore.get_singleton().get_input_device("/user/head") + if head_device: + hmd = head_device.get_virtual_world_pose("") + position = hmd.ExtractTranslation() + quat = hmd.ExtractRotationQuat() + quati = quat.GetImaginary() + quatw = quat.GetReal() + + # Store in w, x, y, z order to match our convention + self._previous_headpose = np.array([ + position[0], + position[1], + position[2], + quatw, + quati[0], + quati[1], + quati[2], + ]) + + return self._previous_headpose + + def _on_teleop_command(self, event: carb.events.IEvent): + """Handle teleoperation command events. + + Args: + event: The XR message-bus event containing a 'message' payload. + """ + msg = event.payload["message"] + + if "start" in msg: + if "START" in self._additional_callbacks: + self._additional_callbacks["START"]() + elif "stop" in msg: + if "STOP" in self._additional_callbacks: + self._additional_callbacks["STOP"]() + elif "reset" in msg: + if "RESET" in self._additional_callbacks: + self._additional_callbacks["RESET"]() diff --git a/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py b/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py new file mode 100644 index 000000000000..c58e32fa0d23 --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/manus_vive_utils.py @@ -0,0 +1,498 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import contextlib +import numpy as np +from time import time + +import carb +from isaacsim.core.utils.extensions import enable_extension + +# For testing purposes, we need to mock the XRCore +XRCore, XRPoseValidityFlags = None, None + +with contextlib.suppress(ModuleNotFoundError): + from omni.kit.xr.core import XRCore, XRPoseValidityFlags + +from pxr import Gf + +# Mapping from Manus joint index (0-24) to joint name. Palm (25) is calculated from middle metacarpal and proximal. +HAND_JOINT_MAP = { + # Palm + 25: "palm", + # Wrist + 0: "wrist", + # Thumb + 21: "thumb_metacarpal", + 22: "thumb_proximal", + 23: "thumb_distal", + 24: "thumb_tip", + # Index + 1: "index_metacarpal", + 2: "index_proximal", + 3: "index_intermediate", + 4: "index_distal", + 5: "index_tip", + # Middle + 6: "middle_metacarpal", + 7: "middle_proximal", + 8: "middle_intermediate", + 9: "middle_distal", + 10: "middle_tip", + # Ring + 11: "ring_metacarpal", + 12: "ring_proximal", + 13: "ring_intermediate", + 14: "ring_distal", + 15: "ring_tip", + # Little + 16: "little_metacarpal", + 17: "little_proximal", + 18: "little_intermediate", + 19: "little_distal", + 20: "little_tip", +} + + +class ManusViveIntegration: + def __init__(self): + enable_extension("isaacsim.xr.input_devices") + from isaacsim.xr.input_devices.impl.manus_vive_integration import get_manus_vive_integration + + _manus_vive_integration = get_manus_vive_integration() + self.manus = _manus_vive_integration.manus_tracker + self.vive_tracker = _manus_vive_integration.vive_tracker + self.device_status = _manus_vive_integration.device_status + self.default_pose = {"position": [0, 0, 0], "orientation": [1, 0, 0, 0]} + # 90-degree ccw rotation on Y-axis and 90-degree ccw rotation on Z-axis + self.rot_adjust = Gf.Matrix3d().SetRotate(Gf.Quatd(0.5, Gf.Vec3d(-0.5, 0.5, 0.5))) + self.scene_T_lighthouse_static = None + self._vive_left_id = None + self._vive_right_id = None + self._pairA_candidates = [] # Pair A: WM0->Left, WM1->Right + self._pairB_candidates = [] # Pair B: WM1->Left, WM0->Right + self._pairA_trans_errs = [] + self._pairA_rot_errs = [] + self._pairB_trans_errs = [] + self._pairB_rot_errs = [] + + def get_all_device_data(self) -> dict: + """Get all tracked device data in scene coordinates. + + Returns: + Manus glove joint data and Vive tracker data. + { + 'manus_gloves': { + '{left/right}_{joint_index}': { + 'position': [x, y, z], + 'orientation': [w, x, y, z] + }, + ... + }, + 'vive_trackers': { + '{vive_tracker_id}': { + 'position': [x, y, z], + 'orientation': [w, x, y, z] + }, + ... + } + } + """ + self.update_manus() + self.update_vive() + # Get raw data from trackers + manus_data = self.manus.get_data() + vive_data = self.vive_tracker.get_data() + vive_transformed = self._transform_vive_data(vive_data) + scene_T_wrist = self._get_scene_T_wrist_matrix(vive_transformed) + + return { + "manus_gloves": self._transform_manus_data(manus_data, scene_T_wrist), + "vive_trackers": vive_transformed, + } + + def get_device_status(self) -> dict: + """Get connection and data freshness status for Manus gloves and Vive trackers. + + Returns: + Dictionary containing connection flags and last-data timestamps. + Format: { + 'manus_gloves': {'connected': bool, 'last_data_time': float}, + 'vive_trackers': {'connected': bool, 'last_data_time': float}, + 'left_hand_connected': bool, + 'right_hand_connected': bool + } + """ + return self.device_status + + def update_manus(self): + """Update raw Manus glove data and status flags.""" + self.manus.update() + self.device_status["manus_gloves"]["last_data_time"] = time() + manus_data = self.manus.get_data() + self.device_status["left_hand_connected"] = "left_0" in manus_data + self.device_status["right_hand_connected"] = "right_0" in manus_data + + def update_vive(self): + """Update raw Vive tracker data, and initialize coordinate transformation if it is the first data update.""" + self.vive_tracker.update() + self.device_status["vive_trackers"]["last_data_time"] = time() + try: + # Initialize coordinate transformation from first Vive wrist position + if self.scene_T_lighthouse_static is None: + self._initialize_coordinate_transformation() + except Exception as e: + carb.log_error(f"Vive tracker update failed: {e}") + + def _initialize_coordinate_transformation(self): + """ + Initialize the scene to lighthouse coordinate transformation. + The coordinate transformation is used to transform the wrist pose from lighthouse coordinate system to isaac sim scene coordinate. + It is computed from multiple frames of AVP/OpenXR wrist pose and Vive wrist pose samples at the beginning of the session. + """ + min_frames = 6 + tolerance = 3.0 + vive_data = self.vive_tracker.get_data() + wm0_id, wm1_id = get_vive_wrist_ids(vive_data) + if wm0_id is None and wm1_id is None: + return + + try: + # Fetch OpenXR wrists + L, R, gloves = None, None, [] + if self.device_status["left_hand_connected"]: + gloves.append("left") + L = get_openxr_wrist_matrix("left") + if self.device_status["right_hand_connected"]: + gloves.append("right") + R = get_openxr_wrist_matrix("right") + + M0, M1, vives = None, None, [] + if wm0_id is not None: + vives.append(wm0_id) + M0 = pose_to_matrix(vive_data[wm0_id]) + if wm1_id is not None: + vives.append(wm1_id) + M1 = pose_to_matrix(vive_data[wm1_id]) + + TL0, TL1, TR0, TR1 = None, None, None, None + # Compute transforms for available pairs + if wm0_id is not None and L is not None: + TL0 = M0.GetInverse() * L + self._pairA_candidates.append(TL0) + if wm1_id is not None and L is not None: + TL1 = M1.GetInverse() * L + self._pairB_candidates.append(TL1) + if wm1_id is not None and R is not None: + TR1 = M1.GetInverse() * R + self._pairA_candidates.append(TR1) + if wm0_id is not None and R is not None: + TR0 = M0.GetInverse() * R + self._pairB_candidates.append(TR0) + + # Per-frame pairing error if both candidates present + if TL0 is not None and TR1 is not None and TL1 is not None and TR0 is not None: + eT, eR = compute_delta_errors(TL0, TR1) + self._pairA_trans_errs.append(eT) + self._pairA_rot_errs.append(eR) + eT, eR = compute_delta_errors(TL1, TR0) + self._pairB_trans_errs.append(eT) + self._pairB_rot_errs.append(eR) + + # Choose a mapping + choose_A = None + if len(self._pairA_candidates) == 0 and len(self._pairB_candidates) >= min_frames: + choose_A = False + elif len(self._pairB_candidates) == 0 and len(self._pairA_candidates) >= min_frames: + choose_A = True + elif len(self._pairA_trans_errs) >= min_frames and len(self._pairB_trans_errs) >= min_frames: + errA = get_pairing_error(self._pairA_trans_errs, self._pairA_rot_errs) + errB = get_pairing_error(self._pairB_trans_errs, self._pairB_rot_errs) + if errA < errB and errA < tolerance: + choose_A = True + elif errB < errA and errB < tolerance: + choose_A = False + if choose_A is None: + carb.log_info(f"error A: {errA}, error B: {errB}") + return + + if choose_A: + chosen_list = self._pairA_candidates + self._vive_left_id, self._vive_right_id = wm0_id, wm1_id + else: + chosen_list = self._pairB_candidates + self._vive_left_id, self._vive_right_id = wm1_id, wm0_id + + if len(chosen_list) >= min_frames: + cluster = select_mode_cluster(chosen_list) + carb.log_info(f"Wrist calibration: formed size {len(cluster)} cluster from {len(chosen_list)} samples") + if len(cluster) >= min_frames // 2: + averaged = average_transforms(cluster) + self.scene_T_lighthouse_static = averaged + carb.log_info(f"Resolved mapping: {self._vive_left_id}->Left, {self._vive_right_id}->Right") + + except Exception as e: + carb.log_error(f"Failed to initialize coordinate transformation: {e}") + + def _transform_vive_data(self, device_data: dict) -> dict: + """Transform Vive tracker poses to scene coordinates. + + Args: + device_data: raw vive tracker poses, with device id as keys. + + Returns: + Vive tracker poses in scene coordinates, with device id as keys. + """ + transformed_data = {} + for joint_name, joint_data in device_data.items(): + transformed_pose = self.default_pose + if self.scene_T_lighthouse_static is not None: + transformed_matrix = pose_to_matrix(joint_data) * self.scene_T_lighthouse_static + transformed_pose = matrix_to_pose(transformed_matrix) + transformed_data[joint_name] = transformed_pose + return transformed_data + + def _get_scene_T_wrist_matrix(self, vive_data: dict) -> dict: + """Compute scene-frame wrist transforms for left and right hands. + + Args: + vive_data: Vive tracker poses expressed in scene coordinates. + + Returns: + Dictionary with 'left' and 'right' keys mapping to 4x4 transforms. + """ + scene_T_wrist = {"left": Gf.Matrix4d().SetIdentity(), "right": Gf.Matrix4d().SetIdentity()} + # 10 cm offset on Y-axis for change in vive tracker position after flipping the palm + Rcorr = Gf.Matrix4d(self.rot_adjust, Gf.Vec3d(0, -0.1, 0)) + if self._vive_left_id is not None: + scene_T_wrist["left"] = Rcorr * pose_to_matrix(vive_data[self._vive_left_id]) + if self._vive_right_id is not None: + scene_T_wrist["right"] = Rcorr * pose_to_matrix(vive_data[self._vive_right_id]) + return scene_T_wrist + + def _transform_manus_data(self, manus_data: dict, scene_T_wrist: dict) -> dict: + """Transform Manus glove joints from wrist-relative to scene coordinates. + + Args: + manus_data: Raw Manus joint pose dictionary, wrist-relative. + scene_T_wrist: Dictionary of scene transforms for left and right wrists. + + Returns: + Dictionary of Manus joint poses in scene coordinates. + """ + Rcorr = Gf.Matrix4d(self.rot_adjust, Gf.Vec3d(0, 0, 0)).GetInverse() + transformed_data = {} + for joint_name, joint_data in manus_data.items(): + hand, _ = joint_name.split("_") + joint_mat = Rcorr * pose_to_matrix(joint_data) * scene_T_wrist[hand] + transformed_data[joint_name] = matrix_to_pose(joint_mat) + # Calculate palm with middle metacarpal and proximal + transformed_data["left_25"] = self._get_palm(transformed_data, "left") + transformed_data["right_25"] = self._get_palm(transformed_data, "right") + return transformed_data + + def _get_palm(self, transformed_data: dict, hand: str) -> dict: + """Compute palm pose from middle metacarpal and proximal joints. + + Args: + transformed_data: Manus joint poses in scene coordinates. + hand: The hand side, either 'left' or 'right'. + + Returns: + Pose dictionary with 'position' and 'orientation'. + """ + if f"{hand}_6" not in transformed_data or f"{hand}_7" not in transformed_data: + carb.log_error(f"Joint data not found for {hand}") + return self.default_pose + metacarpal = transformed_data[f"{hand}_6"] + proximal = transformed_data[f"{hand}_7"] + pos = (np.array(metacarpal["position"]) + np.array(proximal["position"])) / 2.0 + return {"position": [pos[0], pos[1], pos[2]], "orientation": metacarpal["orientation"]} + + +def compute_delta_errors(a: Gf.Matrix4d, b: Gf.Matrix4d) -> tuple[float, float]: + """Compute translation and rotation error between two transforms. + + Args: + a: The first transform. + b: The second transform. + + Returns: + Tuple containing (translation_error_m, rotation_error_deg). + """ + try: + delta = a * b.GetInverse() + t = delta.ExtractTranslation() + trans_err = float(np.linalg.norm([t[0], t[1], t[2]])) + q = delta.ExtractRotation().GetQuat() + w = float(max(min(q.GetReal(), 1.0), -1.0)) + ang = 2.0 * float(np.arccos(w)) + ang_deg = float(np.degrees(ang)) + if ang_deg > 180.0: + ang_deg = 360.0 - ang_deg + return trans_err, ang_deg + except Exception: + return float("inf"), float("inf") + + +def average_transforms(mats: list[Gf.Matrix4d]) -> Gf.Matrix4d: + """Average rigid transforms across translations and quaternions. + + Args: + mats: The list of 4x4 transforms to average. + + Returns: + Averaged 4x4 transform, or None if the list is empty. + """ + if not mats: + return None + ref_quat = mats[0].ExtractRotation().GetQuat() + ref = np.array([ref_quat.GetReal(), *ref_quat.GetImaginary()]) + acc_q = np.zeros(4, dtype=np.float64) + acc_t = np.zeros(3, dtype=np.float64) + for m in mats: + t = m.ExtractTranslation() + acc_t += np.array([t[0], t[1], t[2]], dtype=np.float64) + q = m.ExtractRotation().GetQuat() + qi = np.array([q.GetReal(), *q.GetImaginary()], dtype=np.float64) + if np.dot(qi, ref) < 0.0: + qi = -qi + acc_q += qi + mean_t = acc_t / float(len(mats)) + norm = np.linalg.norm(acc_q) + if norm <= 1e-12: + quat_avg = Gf.Quatd(1.0, Gf.Vec3d(0.0, 0.0, 0.0)) + else: + qn = acc_q / norm + quat_avg = Gf.Quatd(float(qn[0]), Gf.Vec3d(float(qn[1]), float(qn[2]), float(qn[3]))) + rot3 = Gf.Matrix3d().SetRotate(quat_avg) + trans = Gf.Vec3d(float(mean_t[0]), float(mean_t[1]), float(mean_t[2])) + return Gf.Matrix4d(rot3, trans) + + +def select_mode_cluster( + mats: list[Gf.Matrix4d], trans_thresh_m: float = 0.03, rot_thresh_deg: float = 10.0 +) -> list[Gf.Matrix4d]: + """Select the largest cluster of transforms under proximity thresholds. + + Args: + mats: The list of 4x4 transforms to cluster. + trans_thresh_m: The translation threshold in meters. + rot_thresh_deg: The rotation threshold in degrees. + + Returns: + The largest cluster (mode) of transforms. + """ + if not mats: + return [] + best_cluster: list[Gf.Matrix4d] = [] + for center in mats: + cluster: list[Gf.Matrix4d] = [] + for m in mats: + trans_err, rot_err = compute_delta_errors(m, center) + if trans_err <= trans_thresh_m and rot_err <= rot_thresh_deg: + cluster.append(m) + if len(cluster) > len(best_cluster): + best_cluster = cluster + return best_cluster + + +def get_openxr_wrist_matrix(hand: str) -> Gf.Matrix4d: + """Get the OpenXR wrist matrix if valid. + + Args: + hand: The hand side ('left' or 'right'). + + Returns: + 4x4 transform for the wrist if valid, otherwise None. + """ + hand = hand.lower() + try: + hand_device = XRCore.get_singleton().get_input_device(f"/user/hand/{hand}") + if hand_device is None: + return None + joints = hand_device.get_all_virtual_world_poses() + if "wrist" not in joints: + return None + joint = joints["wrist"] + required = XRPoseValidityFlags.POSITION_VALID | XRPoseValidityFlags.ORIENTATION_VALID + if (joint.validity_flags & required) != required: + return None + return joint.pose_matrix + except Exception as e: + carb.log_warn(f"OpenXR {hand} wrist fetch failed: {e}") + return None + + +def get_vive_wrist_ids(vive_data: dict) -> tuple[str, str]: + """Get the Vive wrist tracker IDs if available. + + Args: + vive_data: The raw Vive data dictionary. + + Returns: + (wm0_id, wm1_id) if available, otherwise None values. + """ + wm_ids = [k for k in vive_data.keys() if len(k) >= 2 and k[:2] == "WM"] + wm_ids.sort() + if len(wm_ids) >= 2: # Assumes the first two vive trackers are the wrist trackers + return wm_ids[0], wm_ids[1] + if len(wm_ids) == 1: + return wm_ids[0], None + return None, None + + +def pose_to_matrix(pose: dict) -> Gf.Matrix4d: + """Convert a pose dictionary to a 4x4 transform matrix. + + Args: + pose: The pose with 'position' and 'orientation' fields. + + Returns: + A 4x4 transform representing the pose. + """ + pos, ori = pose["position"], pose["orientation"] + quat = Gf.Quatd(ori[0], Gf.Vec3d(ori[1], ori[2], ori[3])) + rot = Gf.Matrix3d().SetRotate(quat) + trans = Gf.Vec3d(pos[0], pos[1], pos[2]) + return Gf.Matrix4d(rot, trans) + + +def matrix_to_pose(matrix: Gf.Matrix4d) -> dict: + """Convert a 4x4 transform matrix to a pose dictionary. + + Args: + matrix: The 4x4 transform matrix to convert. + + Returns: + Pose dictionary with 'position' and 'orientation'. + """ + pos = matrix.ExtractTranslation() + rot = matrix.ExtractRotation() + quat = rot.GetQuat() + return { + "position": [pos[0], pos[1], pos[2]], + "orientation": [quat.GetReal(), quat.GetImaginary()[0], quat.GetImaginary()[1], quat.GetImaginary()[2]], + } + + +def get_pairing_error(trans_errs: list, rot_errs: list) -> float: + """Compute a scalar pairing error from translation and rotation errors. + + Args: + trans_errs: The list of translation errors across samples. + rot_errs: The list of rotation errors across samples. + + Returns: + The weighted sum of medians of translation and rotation errors. + """ + + def _median(values: list) -> float: + try: + return float(np.median(np.asarray(values, dtype=np.float64))) + except Exception: + return float("inf") + + return _median(trans_errs) + 0.01 * _median(rot_errs) diff --git a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py index 34cd4bb2cfe6..4e5e08249803 100644 --- a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py +++ b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py @@ -40,10 +40,12 @@ class OpenXRDevice(DeviceBase): """An OpenXR-powered device for teleoperation and interaction. This device tracks hand joints using OpenXR and makes them available as: + 1. A dictionary of tracking data (when used without retargeters) 2. Retargeted commands for robot control (when retargeters are provided) Raw data format (_get_raw_data output): + * A dictionary with keys matching TrackingTarget enum values (HAND_LEFT, HAND_RIGHT, HEAD) * Each hand tracking entry contains a dictionary of joint poses * Each joint pose is a 7D vector (x, y, z, qw, qx, qy, qz) in meters and quaternion units @@ -52,6 +54,7 @@ class OpenXRDevice(DeviceBase): Teleop commands: The device responds to several teleop commands that can be subscribed to via add_callback(): + * "START": Resume hand tracking data flow * "STOP": Pause hand tracking data flow * "RESET": Reset the tracking and signal simulation reset diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py index 89787b86674f..f2a7eed32c6d 100644 --- a/source/isaaclab/isaaclab/devices/teleop_device_factory.py +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -29,7 +29,7 @@ with contextlib.suppress(ModuleNotFoundError): # May fail if xr is not in use - from isaaclab.devices.openxr import OpenXRDevice, OpenXRDeviceCfg + from isaaclab.devices.openxr import ManusVive, ManusViveCfg, OpenXRDevice, OpenXRDeviceCfg # Map device types to their constructor and expected config type DEVICE_MAP: dict[type[DeviceCfg], type[DeviceBase]] = { @@ -40,6 +40,7 @@ Se2GamepadCfg: Se2Gamepad, Se2SpaceMouseCfg: Se2SpaceMouse, OpenXRDeviceCfg: OpenXRDevice, + ManusViveCfg: ManusVive, } diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py index 4d0871fcb8a0..6192f3e58836 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/pickplace_gr1t2_env_cfg.py @@ -14,7 +14,7 @@ from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.controllers.pink_ik import NullSpacePostureTask, PinkIKControllerCfg from isaaclab.devices.device_base import DevicesCfg -from isaaclab.devices.openxr import OpenXRDeviceCfg, XrCfg +from isaaclab.devices.openxr import ManusViveCfg, OpenXRDeviceCfg, XrCfg from isaaclab.devices.openxr.retargeters.humanoid.fourier.gr1t2_retargeter import GR1T2RetargeterCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg @@ -437,5 +437,17 @@ def __post_init__(self): sim_device=self.sim.device, xr_cfg=self.xr, ), + "manusvive": ManusViveCfg( + retargeters=[ + GR1T2RetargeterCfg( + enable_visualization=True, + num_open_xr_hand_joints=2 * 26, + sim_device=self.sim.device, + hand_joint_names=self.actions.pink_ik_cfg.hand_joint_names, + ), + ], + sim_device=self.sim.device, + xr_cfg=self.xr, + ), } )