diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst index 6411296116c..b21ed817211 100644 --- a/docs/source/how-to/cloudxr_teleoperation.rst +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -935,18 +935,15 @@ The retargeting system is designed to be extensible. You can create custom retar # Return control commands in appropriate format return torch.tensor([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]) # Example output -3. Register your retargeter with the factory by adding it to the ``RETARGETER_MAP``: +3. Register your retargeter by setting ``retargeter_type`` on the config class: .. code-block:: python # Import your retargeter at the top of your module from my_package.retargeters import MyCustomRetargeter, MyCustomRetargeterCfg - # Add your retargeter to the factory - from isaaclab.devices.teleop_device_factory import RETARGETER_MAP - - # Register your retargeter type with its constructor - RETARGETER_MAP[MyCustomRetargeterCfg] = MyCustomRetargeter + # Link the config to the implementation for factory construction + MyCustomRetargeterCfg.retargeter_type = MyCustomRetargeter 4. Now you can use your custom retargeter in teleop device configurations: diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index aa294f1dbf8..5903a09cef4 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.47.4" +version = "0.47.5" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 2828326ff4b..b6da52b64ad 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,15 @@ Changelog --------- +0.47.5 (2025-10-30) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Moved retargeter and device declaration out of factory and into the devices/retargeters themselves. + + 0.47.4 (2025-10-30) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/devices/device_base.py b/source/isaaclab/isaaclab/devices/device_base.py index b7955468cc1..da33d9a64fd 100644 --- a/source/isaaclab/isaaclab/devices/device_base.py +++ b/source/isaaclab/isaaclab/devices/device_base.py @@ -18,8 +18,14 @@ class DeviceCfg: """Configuration for teleoperation devices.""" + # Whether teleoperation should start active by default + teleoperation_active_default: bool = True + # Torch device string to place output tensors on sim_device: str = "cpu" + # Retargeters that transform device data into robot commands retargeters: list[RetargeterCfg] = field(default_factory=list) + # Concrete device class to construct for this config. Set by each device module. + device_type: type["DeviceBase"] | None = None @dataclass diff --git a/source/isaaclab/isaaclab/devices/gamepad/se2_gamepad.py b/source/isaaclab/isaaclab/devices/gamepad/se2_gamepad.py index dacf1cdb497..33ede14aebd 100644 --- a/source/isaaclab/isaaclab/devices/gamepad/se2_gamepad.py +++ b/source/isaaclab/isaaclab/devices/gamepad/se2_gamepad.py @@ -5,6 +5,8 @@ """Gamepad controller for SE(2) control.""" +from __future__ import annotations + import numpy as np import torch import weakref @@ -18,16 +20,6 @@ from ..device_base import DeviceBase, DeviceCfg -@dataclass -class Se2GamepadCfg(DeviceCfg): - """Configuration for SE2 gamepad devices.""" - - v_x_sensitivity: float = 1.0 - v_y_sensitivity: float = 1.0 - omega_z_sensitivity: float = 1.0 - dead_zone: float = 0.01 - - class Se2Gamepad(DeviceBase): r"""A gamepad controller for sending SE(2) commands as velocity commands. @@ -209,3 +201,14 @@ def _resolve_command_buffer(self, raw_command: np.ndarray) -> np.ndarray: command[command_sign] *= -1 return command + + +@dataclass +class Se2GamepadCfg(DeviceCfg): + """Configuration for SE2 gamepad devices.""" + + v_x_sensitivity: float = 1.0 + v_y_sensitivity: float = 1.0 + omega_z_sensitivity: float = 1.0 + dead_zone: float = 0.01 + device_type: type[DeviceBase] = Se2Gamepad diff --git a/source/isaaclab/isaaclab/devices/gamepad/se3_gamepad.py b/source/isaaclab/isaaclab/devices/gamepad/se3_gamepad.py index f30dc8357e0..3e8276fc1cd 100644 --- a/source/isaaclab/isaaclab/devices/gamepad/se3_gamepad.py +++ b/source/isaaclab/isaaclab/devices/gamepad/se3_gamepad.py @@ -5,6 +5,8 @@ """Gamepad controller for SE(3) control.""" +from __future__ import annotations + import numpy as np import torch import weakref @@ -18,17 +20,6 @@ from ..device_base import DeviceBase, DeviceCfg -@dataclass -class Se3GamepadCfg(DeviceCfg): - """Configuration for SE3 gamepad devices.""" - - gripper_term: bool = True - dead_zone: float = 0.01 # For gamepad devices - pos_sensitivity: float = 1.0 - rot_sensitivity: float = 1.6 - retargeters: None = None - - class Se3Gamepad(DeviceBase): """A gamepad controller for sending SE(3) commands as delta poses and binary command (open/close). @@ -264,3 +255,14 @@ def _resolve_command_buffer(self, raw_command: np.ndarray) -> np.ndarray: delta_command[delta_command_sign] *= -1 return delta_command + + +@dataclass +class Se3GamepadCfg(DeviceCfg): + """Configuration for SE3 gamepad devices.""" + + gripper_term: bool = True + dead_zone: float = 0.01 # For gamepad devices + pos_sensitivity: float = 1.0 + rot_sensitivity: float = 1.6 + device_type: type[DeviceBase] = Se3Gamepad diff --git a/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py b/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py index 45edf1145b7..a86fbe1bf25 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py +++ b/source/isaaclab/isaaclab/devices/keyboard/se2_keyboard.py @@ -5,6 +5,8 @@ """Keyboard controller for SE(2) control.""" +from __future__ import annotations + import numpy as np import torch import weakref @@ -17,15 +19,6 @@ from ..device_base import DeviceBase, DeviceCfg -@dataclass -class Se2KeyboardCfg(DeviceCfg): - """Configuration for SE2 keyboard devices.""" - - v_x_sensitivity: float = 0.8 - v_y_sensitivity: float = 0.4 - omega_z_sensitivity: float = 1.0 - - class Se2Keyboard(DeviceBase): r"""A keyboard controller for sending SE(2) commands as velocity commands. @@ -178,3 +171,13 @@ def _create_key_bindings(self): "NUMPAD_9": np.asarray([0.0, 0.0, -1.0]) * self.omega_z_sensitivity, "X": np.asarray([0.0, 0.0, -1.0]) * self.omega_z_sensitivity, } + + +@dataclass +class Se2KeyboardCfg(DeviceCfg): + """Configuration for SE2 keyboard devices.""" + + v_x_sensitivity: float = 0.8 + v_y_sensitivity: float = 0.4 + omega_z_sensitivity: float = 1.0 + device_type: type[DeviceBase] = Se2Keyboard diff --git a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py index 94b654e8b17..7448a90b427 100644 --- a/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py +++ b/source/isaaclab/isaaclab/devices/keyboard/se3_keyboard.py @@ -5,6 +5,8 @@ """Keyboard controller for SE(3) control.""" +from __future__ import annotations + import numpy as np import torch import weakref @@ -18,16 +20,6 @@ from ..device_base import DeviceBase, DeviceCfg -@dataclass -class Se3KeyboardCfg(DeviceCfg): - """Configuration for SE3 keyboard devices.""" - - gripper_term: bool = True - pos_sensitivity: float = 0.4 - rot_sensitivity: float = 0.8 - retargeters: None = None - - class Se3Keyboard(DeviceBase): """A keyboard controller for sending SE(3) commands as delta poses and binary command (open/close). @@ -206,3 +198,14 @@ def _create_key_bindings(self): "C": np.asarray([0.0, 0.0, 1.0]) * self.rot_sensitivity, "V": np.asarray([0.0, 0.0, -1.0]) * self.rot_sensitivity, } + + +@dataclass +class Se3KeyboardCfg(DeviceCfg): + """Configuration for SE3 keyboard devices.""" + + gripper_term: bool = True + pos_sensitivity: float = 0.4 + rot_sensitivity: float = 0.8 + retargeters: None = None + device_type: type[DeviceBase] = Se3Keyboard diff --git a/source/isaaclab/isaaclab/devices/openxr/manus_vive.py b/source/isaaclab/isaaclab/devices/openxr/manus_vive.py index 4dda777843f..8891c1aab5c 100644 --- a/source/isaaclab/isaaclab/devices/openxr/manus_vive.py +++ b/source/isaaclab/isaaclab/devices/openxr/manus_vive.py @@ -7,6 +7,8 @@ Manus and Vive for teleoperation and interaction. """ +from __future__ import annotations + import contextlib import numpy as np from collections.abc import Callable @@ -34,13 +36,6 @@ 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. @@ -246,3 +241,11 @@ def _on_teleop_command(self, event: carb.events.IEvent): elif "reset" in msg: if "RESET" in self._additional_callbacks: self._additional_callbacks["RESET"]() + + +@dataclass +class ManusViveCfg(DeviceCfg): + """Configuration for Manus and Vive.""" + + xr_cfg: XrCfg | None = None + device_type: type[DeviceBase] = ManusVive diff --git a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py index 4e5e0824980..011a6af3ddb 100644 --- a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py +++ b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py @@ -5,6 +5,8 @@ """OpenXR-powered device for teleoperation and interaction.""" +from __future__ import annotations + import contextlib import numpy as np from collections.abc import Callable @@ -26,14 +28,8 @@ with contextlib.suppress(ModuleNotFoundError): from omni.kit.xr.core import XRCore, XRPoseValidityFlags -from isaacsim.core.prims import SingleXFormPrim - - -@dataclass -class OpenXRDeviceCfg(DeviceCfg): - """Configuration for OpenXR devices.""" - xr_cfg: XrCfg | None = None +from isaacsim.core.prims import SingleXFormPrim class OpenXRDevice(DeviceBase): @@ -303,3 +299,11 @@ def _on_teleop_command(self, event: carb.events.IEvent): elif "reset" in msg: if "RESET" in self._additional_callbacks: self._additional_callbacks["RESET"]() + + +@dataclass +class OpenXRDeviceCfg(DeviceCfg): + """Configuration for OpenXR devices.""" + + xr_cfg: XrCfg | None = None + device_type: type[DeviceBase] = OpenXRDevice diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/dex/dex_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/dex/dex_retargeter.py deleted file mode 100644 index 2092728b231..00000000000 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/dex/dex_retargeter.py +++ /dev/null @@ -1,39 +0,0 @@ -# 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 numpy as np -from typing import Any - -from isaaclab.devices.retargeter_base import RetargeterBase - - -class DexRetargeter(RetargeterBase): - """Retargets OpenXR hand joint data to DEX robot joint commands. - - This class implements the RetargeterBase interface to convert hand tracking data - into a format suitable for controlling DEX robot hands. - """ - - def __init__(self): - """Initialize the DEX retargeter.""" - super().__init__() - # TODO: Add any initialization parameters and state variables needed - pass - - def retarget(self, joint_data: dict[str, np.ndarray]) -> Any: - """Convert OpenXR hand joint poses to DEX robot commands. - - Args: - joint_data: Dictionary mapping OpenXR joint names to their pose data. - Each pose is a numpy array of shape (7,) containing - [x, y, z, qx, qy, qz, qw] for absolute mode or - [x, y, z, roll, pitch, yaw] for relative mode. - - Returns: - Retargeted data in the format expected by DEX robot control interface. - TODO: Specify the exact return type and format - """ - # TODO: Implement the retargeting logic - raise NotImplementedError("DexRetargeter.retarget() not implemented") diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py index 4548c0f99cb..6f307bc0fa4 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/fourier/gr1t2_retargeter.py @@ -3,6 +3,8 @@ # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import contextlib import numpy as np import torch @@ -19,15 +21,6 @@ from .gr1_t2_dex_retargeting_utils import GR1TR2DexRetargeting -@dataclass -class GR1T2RetargeterCfg(RetargeterCfg): - """Configuration for the GR1T2 retargeter.""" - - enable_visualization: bool = False - num_open_xr_hand_joints: int = 100 - hand_joint_names: list[str] | None = None # List of robot hand joint names - - class GR1T2Retargeter(RetargeterBase): """Retargets OpenXR hand tracking data to GR1T2 hand end-effector commands. @@ -156,3 +149,13 @@ def _retarget_abs(self, wrist: np.ndarray) -> np.ndarray: usd_right_roll_link_in_world_quat = PoseUtils.quat_from_matrix(usd_right_roll_link_in_world_mat) return np.concatenate([usd_right_roll_link_in_world_pos, usd_right_roll_link_in_world_quat]) + + +@dataclass +class GR1T2RetargeterCfg(RetargeterCfg): + """Configuration for the GR1T2 retargeter.""" + + enable_visualization: bool = False + num_open_xr_hand_joints: int = 100 + hand_joint_names: list[str] | None = None # List of robot hand joint names + retargeter_type: type[RetargeterBase] = GR1T2Retargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py index 9cf6ba09c42..7ad2a62bfe7 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/g1_lower_body_standing.py @@ -3,20 +3,14 @@ # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import torch from dataclasses import dataclass from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg -@dataclass -class G1LowerBodyStandingRetargeterCfg(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.""" - - class G1LowerBodyStandingRetargeter(RetargeterBase): """Provides lower body standing commands for the G1 robot.""" @@ -26,3 +20,12 @@ def __init__(self, cfg: G1LowerBodyStandingRetargeterCfg): 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) + + +@dataclass +class G1LowerBodyStandingRetargeterCfg(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.""" + retargeter_type: type[RetargeterBase] = G1LowerBodyStandingRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py index 98855cc352e..39342bc522e 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/inspire/g1_upper_body_retargeter.py @@ -3,6 +3,8 @@ # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import contextlib import numpy as np import torch @@ -19,15 +21,6 @@ from .g1_dex_retargeting_utils import UnitreeG1DexRetargeting -@dataclass -class UnitreeG1RetargeterCfg(RetargeterCfg): - """Configuration for the UnitreeG1 retargeter.""" - - enable_visualization: bool = False - num_open_xr_hand_joints: int = 100 - hand_joint_names: list[str] | None = None # List of robot hand joint names - - class UnitreeG1Retargeter(RetargeterBase): """Retargets OpenXR hand tracking data to GR1T2 hand end-effector commands. @@ -152,3 +145,13 @@ def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: quat = PoseUtils.quat_from_matrix(rot_mat) return np.concatenate([pos.numpy(), quat.numpy()]) + + +@dataclass +class UnitreeG1RetargeterCfg(RetargeterCfg): + """Configuration for the UnitreeG1 retargeter.""" + + enable_visualization: bool = False + num_open_xr_hand_joints: int = 100 + hand_joint_names: list[str] | None = None # List of robot hand joint names + retargeter_type: type[RetargeterBase] = UnitreeG1Retargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py index 41f7f49fd9f..0e3dd6058ff 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/humanoid/unitree/trihand/g1_upper_body_retargeter.py @@ -21,15 +21,6 @@ from .g1_dex_retargeting_utils import G1TriHandDexRetargeting -@dataclass -class G1TriHandUpperBodyRetargeterCfg(RetargeterCfg): - """Configuration for the G1UpperBody retargeter.""" - - enable_visualization: bool = False - num_open_xr_hand_joints: int = 100 - hand_joint_names: list[str] | None = None # List of robot hand joint names - - class G1TriHandUpperBodyRetargeter(RetargeterBase): """Retargets OpenXR data to G1 upper body commands. @@ -164,3 +155,13 @@ def _retarget_abs(self, wrist: np.ndarray, is_left: bool) -> np.ndarray: quat = PoseUtils.quat_from_matrix(rot_mat) return np.concatenate([pos.numpy(), quat.numpy()]) + + +@dataclass +class G1TriHandUpperBodyRetargeterCfg(RetargeterCfg): + """Configuration for the G1UpperBody retargeter.""" + + enable_visualization: bool = False + num_open_xr_hand_joints: int = 100 + hand_joint_names: list[str] | None = None # List of robot hand joint names + retargeter_type: type[RetargeterBase] = G1TriHandUpperBodyRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py index 2174e148d44..a606e5ff002 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/gripper_retargeter.py @@ -2,6 +2,8 @@ # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import numpy as np import torch from dataclasses import dataclass @@ -11,13 +13,6 @@ from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg -@dataclass -class GripperRetargeterCfg(RetargeterCfg): - """Configuration for gripper retargeter.""" - - bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT - - class GripperRetargeter(RetargeterBase): """Retargeter specifically for gripper control based on hand tracking data. @@ -90,3 +85,11 @@ def _calculate_gripper_command(self, thumb_pos: np.ndarray, index_pos: np.ndarra self._previous_gripper_command = True return self._previous_gripper_command + + +@dataclass +class GripperRetargeterCfg(RetargeterCfg): + """Configuration for gripper retargeter.""" + + bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT + retargeter_type: type[RetargeterBase] = GripperRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py index 789ff3c44f6..72a2a82fcb2 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_abs_retargeter.py @@ -2,6 +2,8 @@ # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import numpy as np import torch from dataclasses import dataclass @@ -13,17 +15,6 @@ from isaaclab.markers.config import FRAME_MARKER_CFG -@dataclass -class Se3AbsRetargeterCfg(RetargeterCfg): - """Configuration for absolute position retargeter.""" - - zero_out_xy_rotation: bool = True - use_wrist_rotation: bool = False - use_wrist_position: bool = True - enable_visualization: bool = False - bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT - - class Se3AbsRetargeter(RetargeterBase): """Retargets OpenXR hand tracking data to end-effector commands using absolute positioning. @@ -164,3 +155,15 @@ def _update_visualization(self): quat = Rotation.from_matrix(self._visualization_rot).as_quat() rot = np.array([np.array([quat[3], quat[0], quat[1], quat[2]])]) self._goal_marker.visualize(translations=trans, orientations=rot) + + +@dataclass +class Se3AbsRetargeterCfg(RetargeterCfg): + """Configuration for absolute position retargeter.""" + + zero_out_xy_rotation: bool = True + use_wrist_rotation: bool = False + use_wrist_position: bool = True + enable_visualization: bool = False + bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT + retargeter_type: type[RetargeterBase] = Se3AbsRetargeter diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py index 1a3d80ec249..e5d68ed39cd 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/se3_rel_retargeter.py @@ -2,6 +2,8 @@ # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import numpy as np import torch from dataclasses import dataclass @@ -13,21 +15,6 @@ from isaaclab.markers.config import FRAME_MARKER_CFG -@dataclass -class Se3RelRetargeterCfg(RetargeterCfg): - """Configuration for relative position retargeter.""" - - zero_out_xy_rotation: bool = True - use_wrist_rotation: bool = False - use_wrist_position: bool = True - delta_pos_scale_factor: float = 10.0 - delta_rot_scale_factor: float = 10.0 - alpha_pos: float = 0.5 - alpha_rot: float = 0.5 - enable_visualization: bool = False - bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT - - class Se3RelRetargeter(RetargeterBase): """Retargets OpenXR hand tracking data to end-effector commands using relative positioning. @@ -206,3 +193,19 @@ def _update_visualization(self): quat = Rotation.from_matrix(self._visualization_rot).as_quat() rot = np.array([np.array([quat[3], quat[0], quat[1], quat[2]])]) self._goal_marker.visualize(translations=trans, orientations=rot) + + +@dataclass +class Se3RelRetargeterCfg(RetargeterCfg): + """Configuration for relative position retargeter.""" + + zero_out_xy_rotation: bool = True + use_wrist_rotation: bool = False + use_wrist_position: bool = True + delta_pos_scale_factor: float = 10.0 + delta_rot_scale_factor: float = 10.0 + alpha_pos: float = 0.5 + alpha_rot: float = 0.5 + enable_visualization: bool = False + bound_hand: OpenXRDevice.TrackingTarget = OpenXRDevice.TrackingTarget.HAND_RIGHT + retargeter_type: type[RetargeterBase] = Se3RelRetargeter diff --git a/source/isaaclab/isaaclab/devices/retargeter_base.py b/source/isaaclab/isaaclab/devices/retargeter_base.py index 6193966d713..c4abcf8bf9c 100644 --- a/source/isaaclab/isaaclab/devices/retargeter_base.py +++ b/source/isaaclab/isaaclab/devices/retargeter_base.py @@ -13,6 +13,8 @@ class RetargeterCfg: """Base configuration for hand tracking retargeters.""" sim_device: str = "cpu" + # Concrete retargeter class to construct for this config. Set by each retargeter module. + retargeter_type: type["RetargeterBase"] | None = None class RetargeterBase(ABC): diff --git a/source/isaaclab/isaaclab/devices/spacemouse/se2_spacemouse.py b/source/isaaclab/isaaclab/devices/spacemouse/se2_spacemouse.py index 190ddc19ebb..e9bd116cb6d 100644 --- a/source/isaaclab/isaaclab/devices/spacemouse/se2_spacemouse.py +++ b/source/isaaclab/isaaclab/devices/spacemouse/se2_spacemouse.py @@ -5,6 +5,8 @@ """Spacemouse controller for SE(2) control.""" +from __future__ import annotations + import hid import numpy as np import threading @@ -19,16 +21,6 @@ from .utils import convert_buffer -@dataclass -class Se2SpaceMouseCfg(DeviceCfg): - """Configuration for SE2 space mouse devices.""" - - v_x_sensitivity: float = 0.8 - v_y_sensitivity: float = 0.4 - omega_z_sensitivity: float = 1.0 - sim_device: str = "cpu" - - class Se2SpaceMouse(DeviceBase): r"""A space-mouse controller for sending SE(2) commands as delta poses. @@ -168,3 +160,13 @@ def _run_device(self): # additional callbacks if "R" in self._additional_callbacks: self._additional_callbacks["R"] + + +@dataclass +class Se2SpaceMouseCfg(DeviceCfg): + """Configuration for SE2 space mouse devices.""" + + v_x_sensitivity: float = 0.8 + v_y_sensitivity: float = 0.4 + omega_z_sensitivity: float = 1.0 + device_type: type[DeviceBase] = Se2SpaceMouse diff --git a/source/isaaclab/isaaclab/devices/spacemouse/se3_spacemouse.py b/source/isaaclab/isaaclab/devices/spacemouse/se3_spacemouse.py index feb366ee4cd..d1845547ce1 100644 --- a/source/isaaclab/isaaclab/devices/spacemouse/se3_spacemouse.py +++ b/source/isaaclab/isaaclab/devices/spacemouse/se3_spacemouse.py @@ -5,6 +5,8 @@ """Spacemouse controller for SE(3) control.""" +from __future__ import annotations + import hid import numpy as np import threading @@ -18,16 +20,6 @@ from .utils import convert_buffer -@dataclass -class Se3SpaceMouseCfg(DeviceCfg): - """Configuration for SE3 space mouse devices.""" - - gripper_term: bool = True - pos_sensitivity: float = 0.4 - rot_sensitivity: float = 0.8 - retargeters: None = None - - class Se3SpaceMouse(DeviceBase): """A space-mouse controller for sending SE(3) commands as delta poses. @@ -210,3 +202,14 @@ def _run_device(self): self._additional_callbacks["R"]() if data[1] == 3: self._read_rotation = not self._read_rotation + + +@dataclass +class Se3SpaceMouseCfg(DeviceCfg): + """Configuration for SE3 space mouse devices.""" + + gripper_term: bool = True + pos_sensitivity: float = 0.4 + rot_sensitivity: float = 0.8 + retargeters: None = None + device_type: type[DeviceBase] = Se3SpaceMouse diff --git a/source/isaaclab/isaaclab/devices/teleop_device_factory.py b/source/isaaclab/isaaclab/devices/teleop_device_factory.py index a02029645b6..93a15d6c059 100644 --- a/source/isaaclab/isaaclab/devices/teleop_device_factory.py +++ b/source/isaaclab/isaaclab/devices/teleop_device_factory.py @@ -4,62 +4,14 @@ # SPDX-License-Identifier: BSD-3-Clause """Factory to create teleoperation devices from configuration.""" - -import contextlib import inspect from collections.abc import Callable +from typing import cast import omni.log from isaaclab.devices import DeviceBase, DeviceCfg -from isaaclab.devices.gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg -from isaaclab.devices.keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg -from isaaclab.devices.openxr.retargeters import ( - G1LowerBodyStandingRetargeter, - G1LowerBodyStandingRetargeterCfg, - G1TriHandUpperBodyRetargeter, - G1TriHandUpperBodyRetargeterCfg, - GR1T2Retargeter, - GR1T2RetargeterCfg, - GripperRetargeter, - GripperRetargeterCfg, - Se3AbsRetargeter, - Se3AbsRetargeterCfg, - Se3RelRetargeter, - Se3RelRetargeterCfg, - UnitreeG1Retargeter, - UnitreeG1RetargeterCfg, -) -from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg -from isaaclab.devices.spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg - -with contextlib.suppress(ModuleNotFoundError): - # May fail if xr is not in use - 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]] = { - Se3KeyboardCfg: Se3Keyboard, - Se3SpaceMouseCfg: Se3SpaceMouse, - Se3GamepadCfg: Se3Gamepad, - Se2KeyboardCfg: Se2Keyboard, - Se2GamepadCfg: Se2Gamepad, - Se2SpaceMouseCfg: Se2SpaceMouse, - OpenXRDeviceCfg: OpenXRDevice, - ManusViveCfg: ManusVive, -} - - -# Map configuration types to their corresponding retargeter classes -RETARGETER_MAP: dict[type[RetargeterCfg], type[RetargeterBase]] = { - Se3AbsRetargeterCfg: Se3AbsRetargeter, - Se3RelRetargeterCfg: Se3RelRetargeter, - GripperRetargeterCfg: GripperRetargeter, - GR1T2RetargeterCfg: GR1T2Retargeter, - G1TriHandUpperBodyRetargeterCfg: G1TriHandUpperBodyRetargeter, - G1LowerBodyStandingRetargeterCfg: G1LowerBodyStandingRetargeter, - UnitreeG1RetargeterCfg: UnitreeG1Retargeter, -} +from isaaclab.devices.retargeter_base import RetargeterBase def create_teleop_device( @@ -86,35 +38,44 @@ def create_teleop_device( device_cfg = devices_cfg[device_name] callbacks = callbacks or {} - # Check if device config type is supported - cfg_type = type(device_cfg) - if cfg_type not in DEVICE_MAP: - raise ValueError(f"Unsupported device configuration type: {cfg_type.__name__}") - - # Get the constructor for this config type - constructor = DEVICE_MAP[cfg_type] + # Determine constructor from the configuration itself + device_constructor = getattr(device_cfg, "device_type", None) + if device_constructor is None: + raise ValueError( + f"Device configuration '{device_name}' does not declare device_type. " + "Set cfg.device_type to the concrete DeviceBase subclass." + ) + if not issubclass(device_constructor, DeviceBase): + raise TypeError(f"device_type for '{device_name}' must be a subclass of DeviceBase; got {device_constructor}") # Try to create retargeters if they are configured retargeters = [] if hasattr(device_cfg, "retargeters") and device_cfg.retargeters is not None: try: - # Create retargeters based on configuration + # Create retargeters based on configuration using per-config retargeter_type for retargeter_cfg in device_cfg.retargeters: - cfg_type = type(retargeter_cfg) - if cfg_type in RETARGETER_MAP: - retargeters.append(RETARGETER_MAP[cfg_type](retargeter_cfg)) - else: - raise ValueError(f"Unknown retargeter configuration type: {cfg_type.__name__}") + retargeter_constructor = getattr(retargeter_cfg, "retargeter_type", None) + if retargeter_constructor is None: + raise ValueError( + f"Retargeter configuration {type(retargeter_cfg).__name__} does not declare retargeter_type. " + "Set cfg.retargeter_type to the concrete RetargeterBase subclass." + ) + if not issubclass(retargeter_constructor, RetargeterBase): + raise TypeError( + f"retargeter_type for {type(retargeter_cfg).__name__} must be a subclass of RetargeterBase; got" + f" {retargeter_constructor}" + ) + retargeters.append(retargeter_constructor(retargeter_cfg)) except NameError as e: raise ValueError(f"Failed to create retargeters: {e}") - # Check if the constructor accepts retargeters parameter - constructor_params = inspect.signature(constructor).parameters - if "retargeters" in constructor_params and retargeters: - device = constructor(cfg=device_cfg, retargeters=retargeters) - else: - device = constructor(cfg=device_cfg) + # Build constructor kwargs based on signature + constructor_params = inspect.signature(device_constructor).parameters + params: dict = {"cfg": device_cfg} + if "retargeters" in constructor_params: + params["retargeters"] = retargeters + device = cast(DeviceBase, device_constructor(**params)) # Register callbacks for key, callback in callbacks.items(): diff --git a/source/isaaclab/test/devices/test_device_constructors.py b/source/isaaclab/test/devices/test_device_constructors.py index ffe44740d01..145cf136c85 100644 --- a/source/isaaclab/test/devices/test_device_constructors.py +++ b/source/isaaclab/test/devices/test_device_constructors.py @@ -14,11 +14,13 @@ import importlib import torch +from typing import cast import pytest # Import device classes to test from isaaclab.devices import ( + DeviceCfg, OpenXRDevice, OpenXRDeviceCfg, Se2Gamepad, @@ -66,6 +68,11 @@ def mock_environment(mocker): carb_mock.input.KeyboardEventType.KEY_PRESS = 1 carb_mock.input.KeyboardEventType.KEY_RELEASE = 2 + # Mock carb events used by OpenXRDevice + events_mock = mocker.MagicMock() + events_mock.type_from_string.return_value = 0 + carb_mock.events = events_mock + # Mock the SpaceMouse hid_mock.enumerate.return_value = [{"product_string": "SpaceMouse Compact", "vendor_id": 123, "product_id": 456}] hid_mock.device.return_value = device_mock @@ -290,6 +297,7 @@ def test_openxr_constructors(mock_environment, mocker): "isaacsim.core.prims": mocker.MagicMock(), }, ) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) mocker.patch.object(device_mod, "XRCore", mock_environment["omni"].kit.xr.core.XRCore) mocker.patch.object(device_mod, "XRPoseValidityFlags", mock_environment["omni"].kit.xr.core.XRPoseValidityFlags) mock_single_xform = mocker.patch.object(device_mod, "SingleXFormPrim") @@ -332,7 +340,7 @@ def test_create_teleop_device_basic(mock_environment, mocker): keyboard_cfg = Se3KeyboardCfg(pos_sensitivity=0.8, rot_sensitivity=1.2) # Create devices configuration dictionary - devices_cfg = {"test_keyboard": keyboard_cfg} + devices_cfg: dict[str, DeviceCfg] = {"test_keyboard": keyboard_cfg} # Mock Se3Keyboard class device_mod = importlib.import_module("isaaclab.devices.keyboard.se3_keyboard") @@ -356,7 +364,7 @@ def test_create_teleop_device_with_callbacks(mock_environment, mocker): openxr_cfg = OpenXRDeviceCfg(xr_cfg=xr_cfg) # Create devices configuration dictionary - devices_cfg = {"test_xr": openxr_cfg} + devices_cfg: dict[str, DeviceCfg] = {"test_xr": openxr_cfg} # Create mock callbacks button_a_callback = mocker.MagicMock() @@ -373,6 +381,7 @@ def test_create_teleop_device_with_callbacks(mock_environment, mocker): "isaacsim.core.prims": mocker.MagicMock(), }, ) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) mocker.patch.object(device_mod, "XRCore", mock_environment["omni"].kit.xr.core.XRCore) mocker.patch.object(device_mod, "XRPoseValidityFlags", mock_environment["omni"].kit.xr.core.XRPoseValidityFlags) mock_single_xform = mocker.patch.object(device_mod, "SingleXFormPrim") @@ -387,10 +396,8 @@ def test_create_teleop_device_with_callbacks(mock_environment, mocker): # Verify the device was created correctly assert isinstance(device, OpenXRDevice) - # Verify callbacks were registered - device.add_callback("button_a", button_a_callback) - device.add_callback("button_b", button_b_callback) - assert len(device._additional_callbacks) == 2 + # Verify callbacks were registered by the factory + assert set(device._additional_callbacks.keys()) == {"button_a", "button_b"} def test_create_teleop_device_with_retargeters(mock_environment, mocker): @@ -404,7 +411,7 @@ def test_create_teleop_device_with_retargeters(mock_environment, mocker): device_cfg = OpenXRDeviceCfg(xr_cfg=xr_cfg, retargeters=[retargeter_cfg1, retargeter_cfg2]) # Create devices configuration dictionary - devices_cfg = {"test_xr": device_cfg} + devices_cfg: dict[str, DeviceCfg] = {"test_xr": device_cfg} # Mock OpenXRDevice class and dependencies device_mod = importlib.import_module("isaaclab.devices.openxr.openxr_device") @@ -416,6 +423,7 @@ def test_create_teleop_device_with_retargeters(mock_environment, mocker): "isaacsim.core.prims": mocker.MagicMock(), }, ) + mocker.patch.object(device_mod, "carb", mock_environment["carb"]) mocker.patch.object(device_mod, "XRCore", mock_environment["omni"].kit.xr.core.XRCore) mocker.patch.object(device_mod, "XRPoseValidityFlags", mock_environment["omni"].kit.xr.core.XRPoseValidityFlags) mock_single_xform = mocker.patch.object(device_mod, "SingleXFormPrim") @@ -424,11 +432,6 @@ def test_create_teleop_device_with_retargeters(mock_environment, mocker): mock_instance = mock_single_xform.return_value mock_instance.prim_path = "/XRAnchor" - # Mock retargeter classes - retargeter_mod = importlib.import_module("isaaclab.devices.openxr.retargeters") - mocker.patch.object(retargeter_mod, "Se3AbsRetargeter") - mocker.patch.object(retargeter_mod, "GripperRetargeter") - # Create the device using the factory device = create_teleop_device("test_xr", devices_cfg) @@ -439,7 +442,7 @@ def test_create_teleop_device_with_retargeters(mock_environment, mocker): def test_create_teleop_device_device_not_found(): """Test error when device name is not found in configuration.""" # Create devices configuration dictionary - devices_cfg = {"keyboard": Se3KeyboardCfg()} + devices_cfg: dict[str, DeviceCfg] = {"keyboard": Se3KeyboardCfg()} # Try to create a non-existent device with pytest.raises(ValueError, match="Device 'gamepad' not found"): @@ -454,8 +457,8 @@ class UnsupportedCfg: pass # Create devices configuration dictionary with unsupported config - devices_cfg = {"unsupported": UnsupportedCfg()} + devices_cfg: dict[str, DeviceCfg] = cast(dict[str, DeviceCfg], {"unsupported": UnsupportedCfg()}) # Try to create a device with unsupported configuration - with pytest.raises(ValueError, match="Unsupported device configuration type"): + with pytest.raises(ValueError, match="does not declare device_type"): create_teleop_device("unsupported", devices_cfg)