Unverified Commit 72515ebe authored by rwiltz's avatar rwiltz Committed by GitHub

Refactors retargeters and adds Quest retargeters for G1 tasks (#3950)

# Description
This MR does the following:

- Introduces Quest retargeters for G1 env loco-manipulation tasks. This
enables lower body control via the quest controller joysticks, and upper
body control via controller tracking.
- Refactors the retargeters to *not* depend on OpenXRDevice, instead
move enums into DeviceBase and allow retargeters to be used across
devices.
- Adds XrAnchor "pinning" to a specific robot prim so that the XR view
follows the robot in the scene.

Fixes # (issue)

## Type of change

- New feature (non-breaking change which adds functionality)
- Breaking change (existing functionality will not work without user
modification)
- Documentation update

## Screenshots

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

## Checklist

- [x] I have read and understood the [contribution
guidelines](https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html)
- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

---------
Signed-off-by: 's avatarKelly Guo <kellyg@nvidia.com>
Co-authored-by: 's avatarHougant Chen <hougantc@nvidia.com>
Co-authored-by: 's avatarKelly Guo <kellyg@nvidia.com>
parent 10cf4c15
......@@ -73,6 +73,7 @@ Guidelines for modifications:
* HoJin Jeon
* Hongwei Xiong
* Hongyu Li
* Hougant Chen
* Huihua Zhao
* Iretiayo Akinola
* Jack Zeng
......
......@@ -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:
......@@ -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
# ...
......
......@@ -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 collections.abc import Callable
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 @@ logger = logging.getLogger(__name__)
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 main() -> 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 main() -> None:
if should_reset_recording_instance:
env.reset()
teleop_interface.reset()
should_reset_recording_instance = False
print("Environment reset complete")
except Exception as e:
......
[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"
......
Changelog
---------
0.48.6 (2025-11-18)
~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added OpenXR motion controller support for the G1 robot locomanipulation environment
``Isaac-PickPlace-Locomanipulation-G1-Abs-v0``. This enables teleoperation using XR motion controllers
in addition to hand tracking.
* Added :class:`OpenXRDeviceMotionController` for motion controller-based teleoperation with headset anchoring control.
* Added motion controller-specific retargeters:
* :class:`G1TriHandControllerUpperBodyRetargeterCfg` for upper body and hand control using motion controllers.
* :class:`G1LowerBodyStandingControllerRetargeterCfg` for lower body control using motion controllers.
0.48.5 (2025-11-14)
~~~~~~~~~~~~~~~~~~~
......
......@@ -9,6 +9,7 @@ import torch
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 @@ class DeviceBase(ABC):
"""
# 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 @@ class DeviceBase(ABC):
# 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
......@@ -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
......@@ -13,7 +13,6 @@ 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
......@@ -22,7 +21,6 @@ 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
......@@ -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 @@ class ManusVive(DeviceBase):
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:
......
......@@ -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,
......
......@@ -12,7 +12,7 @@ from dataclasses import dataclass
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 @@ class GR1T2Retargeter(RetargeterBase):
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 @@ class GR1T2Retargeter(RetargeterBase):
"""
# 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 @@ class GR1T2Retargeter(RetargeterBase):
# 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.
......
......@@ -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):
......
# 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
......@@ -12,7 +12,7 @@ from dataclasses import dataclass
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 @@ class UnitreeG1Retargeter(RetargeterBase):
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 @@ class UnitreeG1Retargeter(RetargeterBase):
"""
# 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 @@ class UnitreeG1Retargeter(RetargeterBase):
# 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.
......
# 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
......@@ -12,7 +12,7 @@ from dataclasses import dataclass
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 @@ class G1TriHandUpperBodyRetargeter(RetargeterBase):
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 @@ class G1TriHandUpperBodyRetargeter(RetargeterBase):
"""
# 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 @@ class G1TriHandUpperBodyRetargeter(RetargeterBase):
# 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 @@ class G1TriHandUpperBodyRetargeter(RetargeterBase):
@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
......
......@@ -9,7 +9,7 @@ import torch
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 @@ class GripperRetargeter(RetargeterBase):
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 @@ class GripperRetargeter(RetargeterBase):
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 @@ class GripperRetargeter(RetargeterBase):
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
......@@ -9,7 +9,7 @@ import torch
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 @@ class Se3AbsRetargeter(RetargeterBase):
"""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 @@ class Se3AbsRetargeter(RetargeterBase):
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 @@ class Se3AbsRetargeter(RetargeterBase):
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
......@@ -9,7 +9,7 @@ import torch
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 @@ class Se3RelRetargeter(RetargeterBase):
"""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 @@ class Se3RelRetargeter(RetargeterBase):
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 @@ class Se3RelRetargeter(RetargeterBase):
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
# 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}")
......@@ -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
......
......@@ -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 @@ class RetargeterBase(ABC):
"""
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 @@ class RetargeterBase(ABC):
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,
]
......@@ -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
......@@ -19,6 +19,7 @@ simulation_app = app_launcher.app
import importlib
import numpy as np
import torch
import carb
import omni.usd
......@@ -27,11 +28,30 @@ from isaacsim.core.prims import XFormPrim
from isaaclab.devices import OpenXRDevice, OpenXRDeviceCfg
from isaaclab.devices.openxr import XrCfg
from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg
from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg
from isaaclab.scene import InteractiveSceneCfg
from isaaclab.utils import configclass
class NoOpRetargeter(RetargeterBase):
"""A no-op retargeter that requests hand and head tracking but returns empty tensor."""
def __init__(self, cfg: RetargeterCfg):
super().__init__(cfg)
def get_requirements(self) -> list[RetargeterBase.Requirement]:
"""Request hand and head tracking to trigger data collection."""
return [
RetargeterBase.Requirement.HAND_TRACKING,
RetargeterBase.Requirement.HEAD_TRACKING,
]
def retarget(self, data):
"""Return empty tensor."""
return torch.tensor([], device=self._sim_device)
@configclass
class EmptyManagerCfg:
"""Empty manager."""
......@@ -159,7 +179,7 @@ def test_xr_anchor(empty_env, mock_xrcore):
device = OpenXRDevice(OpenXRDeviceCfg(xr_cfg=env_cfg.xr))
# Check that the xr anchor prim is created with the correct pose
xr_anchor_prim = XFormPrim("/XRAnchor")
xr_anchor_prim = XFormPrim("/World/XRAnchor")
assert xr_anchor_prim.is_valid()
position, orientation = xr_anchor_prim.get_world_poses()
......@@ -168,7 +188,7 @@ def test_xr_anchor(empty_env, mock_xrcore):
# Check that xr anchor mode and custom anchor are set correctly
assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor"
assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/XRAnchor"
assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor"
device.reset()
......@@ -181,7 +201,7 @@ def test_xr_anchor_default(empty_env, mock_xrcore):
device = OpenXRDevice(OpenXRDeviceCfg())
# Check that the xr anchor prim is created with the correct default pose
xr_anchor_prim = XFormPrim("/XRAnchor")
xr_anchor_prim = XFormPrim("/World/XRAnchor")
assert xr_anchor_prim.is_valid()
position, orientation = xr_anchor_prim.get_world_poses()
......@@ -190,7 +210,7 @@ def test_xr_anchor_default(empty_env, mock_xrcore):
# Check that xr anchor mode and custom anchor are set correctly
assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor"
assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/XRAnchor"
assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor"
device.reset()
......@@ -204,7 +224,7 @@ def test_xr_anchor_multiple_devices(empty_env, mock_xrcore):
device_2 = OpenXRDevice(OpenXRDeviceCfg())
# Check that the xr anchor prim is created with the correct default pose
xr_anchor_prim = XFormPrim("/XRAnchor")
xr_anchor_prim = XFormPrim("/World/XRAnchor")
assert xr_anchor_prim.is_valid()
position, orientation = xr_anchor_prim.get_world_poses()
......@@ -213,7 +233,7 @@ def test_xr_anchor_multiple_devices(empty_env, mock_xrcore):
# Check that xr anchor mode and custom anchor are set correctly
assert carb.settings.get_settings().get("/persistent/xr/profile/ar/anchorMode") == "custom anchor"
assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/XRAnchor"
assert carb.settings.get_settings().get("/xrstage/profile/ar/customAnchor") == "/World/XRAnchor"
device_1.reset()
device_2.reset()
......@@ -223,19 +243,22 @@ def test_xr_anchor_multiple_devices(empty_env, mock_xrcore):
def test_get_raw_data(empty_env, mock_xrcore):
"""Test the _get_raw_data method returns correctly formatted tracking data."""
env, _ = empty_env
# Create a proper config object with default values
device = OpenXRDevice(OpenXRDeviceCfg())
# Create a proper config object with default values and a no-op retargeter to trigger data collection
retargeter = NoOpRetargeter(RetargeterCfg())
device = OpenXRDevice(OpenXRDeviceCfg(), retargeters=[retargeter])
# Get raw tracking data
raw_data = device._get_raw_data()
# Check that the data structure is as expected
assert OpenXRDevice.TrackingTarget.HAND_LEFT in raw_data
assert OpenXRDevice.TrackingTarget.HAND_RIGHT in raw_data
assert OpenXRDevice.TrackingTarget.HEAD in raw_data
from isaaclab.devices.device_base import DeviceBase
assert DeviceBase.TrackingTarget.HAND_LEFT in raw_data
assert DeviceBase.TrackingTarget.HAND_RIGHT in raw_data
assert DeviceBase.TrackingTarget.HEAD in raw_data
# Check left hand joints
left_hand = raw_data[OpenXRDevice.TrackingTarget.HAND_LEFT]
left_hand = raw_data[DeviceBase.TrackingTarget.HAND_LEFT]
assert "palm" in left_hand
assert "wrist" in left_hand
......@@ -246,7 +269,7 @@ def test_get_raw_data(empty_env, mock_xrcore):
np.testing.assert_almost_equal(palm_pose[3:], [0.9, 0.1, 0.2, 0.3]) # Orientation
# Check head pose
head_pose = raw_data[OpenXRDevice.TrackingTarget.HEAD]
head_pose = raw_data[DeviceBase.TrackingTarget.HEAD]
assert len(head_pose) == 7
np.testing.assert_almost_equal(head_pose[:3], [0.1, 0.2, 0.3]) # Position
np.testing.assert_almost_equal(head_pose[3:], [0.9, 0.1, 0.2, 0.3]) # Orientation
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.11.8"
version = "0.11.9"
# Description
title = "Isaac Lab Environments"
......
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)
~~~~~~~~~~~~~~~~~~~
......
......@@ -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.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg
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 @@ class LocomanipulationG1EnvCfg(ManagerBasedRLEnvCfg):
# 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 @@ class LocomanipulationG1EnvCfg(ManagerBasedRLEnvCfg):
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,
),
}
)
......@@ -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 @@ class FrankaCubeStackEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg):
"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,
......
......@@ -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 @@ class FrankaCubeStackEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg):
"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 @@ class FrankaCubeStackEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCfg):
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,
......
......@@ -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 @@ class FrankaCubeStackSkillgenEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCf
"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 @@ class FrankaCubeStackSkillgenEnvCfg(stack_joint_pos_env_cfg.FrankaCubeStackEnvCf
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,
......
......@@ -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 @@ class GalbotLeftArmCubeStackEnvCfg(StackEnvCfg):
"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 @@ class GalbotRightArmCubeStackEnvCfg(GalbotLeftArmCubeStackEnvCfg):
"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,
......
......@@ -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 @@ class RmpFlowGalbotLeftArmCubeStackEnvCfg(stack_joint_pos_env_cfg.GalbotLeftArmC
"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 @@ class RmpFlowGalbotLeftArmCubeStackEnvCfg(stack_joint_pos_env_cfg.GalbotLeftArmC
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 @@ class RmpFlowGalbotRightArmCubeStackEnvCfg(stack_joint_pos_env_cfg.GalbotRightAr
"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 @@ class RmpFlowGalbotRightArmCubeStackEnvCfg(stack_joint_pos_env_cfg.GalbotRightAr
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,
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment