Commit 28c0d665 authored by rwiltz's avatar rwiltz Committed by Kelly Guo

Adds handtracking joints and retargetting pipeline (#280)

- Add an input device that returns a dict of joint_str : pose for hand
tracking joints
- Adds retargeting boilerplate code
- Adds retargeting arg to record demos

<!-- As you go through the list, delete the ones that are not
applicable. -->

- Bug fix (non-breaking change which fixes an issue)
- New feature (non-breaking change which adds functionality)
- Breaking change (fix or feature that would cause existing
functionality to not work as expected)
- This change requires a documentation update

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

<!--
Example:

| Before | After |
| ------ | ----- |
| _gif/png before_ | _gif/png after_ |

To upload images to a PR -- simply drag and drop an image while in edit
mode and it should upload the image directly. You can then paste that
source into the above before/after sections.
-->

- [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

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->
parent 05669ea4
......@@ -88,6 +88,7 @@ Guidelines for modifications:
* Peter Du
* Qian Wan
* Qinxi Yu
* Rafael Wiltz
* René Zurbrügg
* Ritvik Singh
* Rosario Scalise
......
......@@ -23,7 +23,7 @@ AppLauncher.add_app_launcher_args(parser)
args_cli = parser.parse_args()
app_launcher_args = vars(args_cli)
if args_cli.teleop_device.lower() == "handtracking":
if "handtracking" in args_cli.teleop_device.lower():
app_launcher_args["xr"] = True
# launch omniverse app
app_launcher = AppLauncher(app_launcher_args)
......@@ -37,7 +37,8 @@ import torch
import omni.log
from isaaclab.devices import Se3Gamepad, Se3HandTracking, Se3Keyboard, Se3SpaceMouse
from isaaclab.devices import OpenXRDevice, Se3Gamepad, Se3Keyboard, Se3SpaceMouse
from isaaclab.devices.openxr.retargeters.manipulator import GripperRetargeter, Se3AbsRetargeter, Se3RelRetargeter
from isaaclab.managers import TerminationTermCfg as DoneTerm
import isaaclab_tasks # noqa: F401
......@@ -79,6 +80,53 @@ def main():
f"The environment '{args_cli.task}' does not support gripper control. The device command will be ignored."
)
# Flags for controlling teleoperation flow
should_reset_recording_instance = False
teleoperation_active = True
# Callback handlers
def reset_recording_instance():
"""Reset the environment to its initial state.
This callback is triggered when the user presses the reset key (typically 'R').
It's useful when:
- The robot gets into an undesirable configuration
- The user wants to start over with the task
- Objects in the scene need to be reset to their initial positions
The environment will be reset on the next simulation step.
"""
nonlocal should_reset_recording_instance
should_reset_recording_instance = True
def start_teleoperation():
"""Activate teleoperation control of the robot.
This callback enables active control of the robot through the input device.
It's typically triggered by a specific gesture or button press and is used when:
- Beginning a new teleoperation session
- Resuming control after temporarily pausing
- Switching from observation mode to control mode
While active, all commands from the device will be applied to the robot.
"""
nonlocal teleoperation_active
teleoperation_active = True
def stop_teleoperation():
"""Deactivate teleoperation control of the robot.
This callback temporarily suspends control of the robot through the input device.
It's typically triggered by a specific gesture or button press and is used when:
- Taking a break from controlling the robot
- Repositioning the input device without moving the robot
- Pausing to observe the scene without interference
While inactive, the simulation continues to render but device commands are ignored.
"""
nonlocal teleoperation_active
teleoperation_active = False
# create controller
if args_cli.teleop_device.lower() == "keyboard":
teleop_interface = Se3Keyboard(
......@@ -92,24 +140,34 @@ def main():
teleop_interface = Se3Gamepad(
pos_sensitivity=0.1 * args_cli.sensitivity, rot_sensitivity=0.1 * args_cli.sensitivity
)
elif args_cli.teleop_device.lower() == "handtracking":
from isaacsim.xr.openxr import OpenXRSpec
elif "handtracking" in args_cli.teleop_device.lower():
# Create EE retargeter with desired configuration
if "_abs" in args_cli.teleop_device.lower():
retargeter_device = Se3AbsRetargeter(zero_out_xy_rotation=True)
else:
retargeter_device = Se3RelRetargeter(zero_out_xy_rotation=True)
grip_retargeter = GripperRetargeter()
# Create hand tracking device with retargeter (in a list)
teleop_interface = OpenXRDevice(
env_cfg.xr,
hand=OpenXRDevice.Hand.RIGHT,
retargeters=[retargeter_device, grip_retargeter],
)
teleop_interface.add_callback("RESET", reset_recording_instance)
teleop_interface.add_callback("START", start_teleoperation)
teleop_interface.add_callback("STOP", stop_teleoperation)
teleop_interface = Se3HandTracking(env_cfg.xr, OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False, True)
teleop_interface.add_callback("RESET", env.reset)
# Hand tracking needs explicit start gesture to activate
teleoperation_active = False
else:
raise ValueError(
f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse', 'gamepad',"
" 'handtracking'."
" 'handtracking', 'handtracking_abs'."
)
# add teleoperation key for env reset
should_reset_recording_instance = False
def reset_recording_instance():
nonlocal should_reset_recording_instance
should_reset_recording_instance = True
# add teleoperation key for env reset (for all devices)
teleop_interface.add_callback("R", reset_recording_instance)
print(teleop_interface)
......@@ -121,15 +179,20 @@ def main():
while simulation_app.is_running():
# run everything in inference mode
with torch.inference_mode():
# get keyboard command
# get device command
delta_pose, gripper_command = teleop_interface.advance()
delta_pose = delta_pose.astype("float32")
# convert to torch
delta_pose = torch.tensor(delta_pose, device=env.device).repeat(env.num_envs, 1)
# pre-process actions
actions = pre_process_actions(delta_pose, gripper_command)
# apply actions
env.step(actions)
# Only apply teleop commands when active
if teleoperation_active:
delta_pose = delta_pose.astype("float32")
# convert to torch
delta_pose = torch.tensor(delta_pose, device=env.device).repeat(env.num_envs, 1)
# pre-process actions
actions = pre_process_actions(delta_pose, gripper_command)
# apply actions
env.step(actions)
else:
env.sim.render()
if should_reset_recording_instance:
env.reset()
......
......@@ -46,13 +46,14 @@ parser.add_argument(
default=10,
help="Number of continuous steps with task success for concluding a demo as successful. Default is 10.",
)
# append AppLauncher cli args
AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()
app_launcher_args = vars(args_cli)
if args_cli.teleop_device.lower() == "handtracking":
if "handtracking" in args_cli.teleop_device.lower():
app_launcher_args["xr"] = True
# launch the simulator
......@@ -68,7 +69,8 @@ import torch
import omni.log
from isaaclab.devices import Se3HandTracking, Se3Keyboard, Se3SpaceMouse
from isaaclab.devices import OpenXRDevice, Se3Keyboard, Se3SpaceMouse
from isaaclab.devices.openxr.retargeters.manipulator import GripperRetargeter, Se3AbsRetargeter, Se3RelRetargeter
from isaaclab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg
import isaaclab_tasks # noqa: F401
......@@ -122,7 +124,7 @@ def main():
"""Collect demonstrations from the environment using teleop interfaces."""
# if handtracking is selected, rate limiting is achieved via OpenXR
if args_cli.teleop_device.lower() == "handtracking":
if "handtracking" in args_cli.teleop_device.lower():
rate_limiter = None
else:
rate_limiter = RateLimiter(args_cli.step_hz)
......@@ -163,28 +165,98 @@ def main():
# create environment
env = gym.make(args_cli.task, cfg=env_cfg).unwrapped
# add teleoperation key for reset current recording instance
# Flags for controlling the demonstration recording process
should_reset_recording_instance = False
running_recording_instance = True
def reset_recording_instance():
"""Reset the current recording instance.
This function is triggered when the user indicates the current demo attempt
has failed and should be discarded. When called, it marks the environment
for reset, which will start a fresh recording instance. This is useful when:
- The robot gets into an unrecoverable state
- The user makes a mistake during demonstration
- The objects in the scene need to be reset to their initial positions
"""
nonlocal should_reset_recording_instance
should_reset_recording_instance = True
# create controller
if args_cli.teleop_device.lower() == "keyboard":
teleop_interface = Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5)
elif args_cli.teleop_device.lower() == "spacemouse":
teleop_interface = Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5)
elif args_cli.teleop_device.lower() == "handtracking":
from isaacsim.xr.openxr import OpenXRSpec
def start_recording_instance():
"""Start or resume recording the current demonstration.
teleop_interface = Se3HandTracking(env_cfg.xr, OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False, True)
teleop_interface.add_callback("RESET", reset_recording_instance)
else:
raise ValueError(
f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse', 'handtracking'."
)
This function enables active recording of robot actions. It's used when:
- Beginning a new demonstration after positioning the robot
- Resuming recording after temporarily stopping to reposition
- Continuing demonstration after pausing to adjust approach or strategy
The user can toggle between stop/start to reposition the robot without
recording those transitional movements in the final demonstration.
"""
nonlocal running_recording_instance
running_recording_instance = True
def stop_recording_instance():
"""Temporarily stop recording the current demonstration.
This function pauses the active recording of robot actions, allowing the user to:
- Reposition the robot or hand tracking device without recording those movements
- Take a break without terminating the entire demonstration
- Adjust their approach before continuing with the task
The environment will continue rendering but won't record actions or advance
the simulation until recording is resumed with start_recording_instance().
"""
nonlocal running_recording_instance
running_recording_instance = False
def create_teleop_device(device_name: str):
"""Create and configure teleoperation device for robot control.
Args:
device_name: Control device to use. Options include:
- "keyboard": Use keyboard keys for simple discrete movements
- "spacemouse": Use 3D mouse for precise 6-DOF control
- "handtracking": Use VR hand tracking for intuitive manipulation
- "handtracking_abs": Use VR hand tracking for intuitive manipulation with absolute EE pose
Returns:
DeviceBase: Configured teleoperation device ready for robot control
"""
device_name = device_name.lower()
if device_name == "keyboard":
return Se3Keyboard(pos_sensitivity=0.2, rot_sensitivity=0.5)
elif device_name == "spacemouse":
return Se3SpaceMouse(pos_sensitivity=0.2, rot_sensitivity=0.5)
elif "handtracking" in device_name:
# Create Franka retargeter with desired configuration
if "_abs" in device_name:
retargeter_device = Se3AbsRetargeter(zero_out_xy_rotation=True)
else:
retargeter_device = Se3RelRetargeter(zero_out_xy_rotation=True)
grip_retargeter = GripperRetargeter()
# Create hand tracking device with retargeter (in a list)
device = OpenXRDevice(
env_cfg.xr,
hand=OpenXRDevice.Hand.RIGHT,
retargeters=[retargeter_device, grip_retargeter],
)
device.add_callback("RESET", reset_recording_instance)
device.add_callback("START", start_recording_instance)
device.add_callback("STOP", stop_recording_instance)
nonlocal running_recording_instance
running_recording_instance = False
return device
else:
raise ValueError(
f"Invalid device interface '{device_name}'. Supported: 'keyboard', 'spacemouse', 'handtracking',"
" 'handtracking_abs'."
)
teleop_interface = create_teleop_device(args_cli.teleop_device)
teleop_interface.add_callback("R", reset_recording_instance)
print(teleop_interface)
......@@ -205,7 +277,10 @@ def main():
actions = pre_process_actions(delta_pose, gripper_command)
# perform action on environment
env.step(actions)
if running_recording_instance:
env.step(actions)
else:
env.sim.render()
if success_term is not None:
if bool(success_term.func(env, **success_term.params)[0]):
......
......@@ -118,7 +118,7 @@ Changed
* ``set_fixed_tendon_limit`` → ``set_fixed_tendon_position_limit``
0.34.10 (2025-03-04)
0.34.11 (2025-03-04)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -129,8 +129,8 @@ Fixed
outputs are requested.
0.34.9 (2025-03-04)
~~~~~~~~~~~~~~~~~~~
0.34.10 (2025-03-04)
~~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
......@@ -139,7 +139,7 @@ Fixed
with other modalities such as RGBA and depth.
0.34.8 (2025-03-04)
0.34.9 (2025-03-04)
~~~~~~~~~~~~~~~~~~~
Added
......@@ -151,7 +151,7 @@ Added
which didn't allow setting the joint position and velocity separately.
0.34.7 (2025-03-02)
0.34.8 (2025-03-02)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -162,7 +162,7 @@ Fixed
was always set to False, which led to incorrect contact sensor settings for the spawned assets.
0.34.6 (2025-03-02)
0.34.7 (2025-03-02)
~~~~~~~~~~~~~~~~~~~
Changed
......@@ -180,7 +180,7 @@ Removed
* Removed the attribute ``disable_contact_processing`` from :class:`~isaaclab.sim.SimulationContact`.
0.34.5 (2025-03-01)
0.34.6 (2025-03-01)
~~~~~~~~~~~~~~~~~~~
Added
......@@ -209,7 +209,7 @@ Changed
they should use the :attr:`isaaclab.actuators.ImplicitActuatorCfg.velocity_limit_sim` attribute instead.
0.34.4 (2025-02-28)
0.34.5 (2025-02-28)
~~~~~~~~~~~~~~~~~~~
Added
......@@ -220,6 +220,16 @@ Added
Support for other Isaac Sim builds will become available in Isaac Sim 5.0.
0.34.4 (2025-02-27)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Refactored retargeting code from Se3Handtracking class into separate modules for better modularity
* Added scaffolding for developing additional retargeters (e.g. dex)
0.34.3 (2025-02-26)
~~~~~~~~~~~~~~~~~~~
......
......@@ -22,5 +22,6 @@ the peripheral device.
from .device_base import DeviceBase
from .gamepad import Se2Gamepad, Se3Gamepad
from .keyboard import Se2Keyboard, Se3Keyboard
from .openxr import Se3HandTracking
from .openxr import OpenXRDevice
from .retargeter_base import RetargeterBase
from .spacemouse import Se2SpaceMouse, Se3SpaceMouse
......@@ -9,13 +9,32 @@ from abc import ABC, abstractmethod
from collections.abc import Callable
from typing import Any
from isaaclab.devices.retargeter_base import RetargeterBase
class DeviceBase(ABC):
"""An interface class for teleoperation devices."""
"""An interface class for teleoperation devices.
Derived classes have two implementation options:
1. Override _get_raw_data() and use the base advance() implementation:
This approach is suitable for devices that want to leverage the built-in
retargeting logic but only need to customize the raw data acquisition.
2. Override advance() completely:
This approach gives full control over the command generation process,
and _get_raw_data() can be ignored entirely.
"""
def __init__(self):
"""Initialize the teleoperation interface."""
pass
def __init__(self, retargeters: list[RetargeterBase] | None = None):
"""Initialize the teleoperation interface.
Args:
retargeters: List of components that transform device data into robot commands.
If None or empty list, the device will output its native data format.
"""
# Initialize empty list if None is provided
self._retargeters = retargeters or []
def __str__(self) -> str:
"""Returns: A string containing the information of joystick."""
......@@ -41,11 +60,41 @@ class DeviceBase(ABC):
"""
raise NotImplementedError
@abstractmethod
def _get_raw_data(self) -> Any:
"""Internal method to get the raw data from the device.
This method is intended for internal use by the advance() implementation.
Derived classes can override this method to customize raw data acquisition
while still using the base class's advance() implementation.
Returns:
Raw device data in a device-specific format
Note:
This is an internal implementation detail. Clients should call advance()
instead of this method.
"""
raise NotImplementedError("Derived class must implement _get_raw_data() or override advance()")
def advance(self) -> Any:
"""Provides the joystick event state.
"""Process current device state and return control commands.
This method retrieves raw data from the device and optionally applies
retargeting to convert it to robot commands.
Derived classes can either:
1. Override _get_raw_data() and use this base implementation, or
2. Override this method completely for custom command processing
Returns:
The processed output form the joystick.
If no retargeters: raw device data in its original format
If retargeters are provided: tuple with output from each retargeter
"""
raise NotImplementedError
raw_data = self._get_raw_data()
# If no retargeters, return raw data directly (not as a tuple)
if not self._retargeters:
return raw_data
# With multiple retargeters, return a tuple of outputs
return tuple(retargeter.retarget(raw_data) for retargeter in self._retargeters)
......@@ -5,5 +5,5 @@
"""Keyboard device for SE(2) and SE(3) control."""
from .se3_handtracking import Se3HandTracking
from .openxr_device import OpenXRDevice
from .xr_cfg import XrCfg
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
# Standard set of hand joint names based on OpenXR specification.
# Input devices for dexterous hands can use this as a reference,
# but may provide any subset or superset of these joints.
HAND_JOINT_NAMES = [
# Palm
"palm",
# Wrist
"wrist",
# Thumb
"thumb_metacarpal",
"thumb_proximal",
"thumb_distal",
"thumb_tip",
# Index
"index_metacarpal",
"index_proximal",
"index_intermediate",
"index_distal",
"index_tip",
# Middle
"middle_metacarpal",
"middle_proximal",
"middle_intermediate",
"middle_distal",
"middle_tip",
# Ring
"ring_metacarpal",
"ring_proximal",
"ring_intermediate",
"ring_distal",
"ring_tip",
# Little
"little_metacarpal",
"little_proximal",
"little_intermediate",
"little_distal",
"little_tip",
]
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import contextlib
import numpy as np
from collections.abc import Callable
from enum import Enum
from typing import Any
import carb
from isaaclab.devices.openxr.common import HAND_JOINT_NAMES
from isaaclab.devices.retargeter_base import RetargeterBase
from ..device_base import DeviceBase
from .xr_cfg import XrCfg
with contextlib.suppress(ModuleNotFoundError):
from isaacsim.xr.openxr import OpenXR, OpenXRSpec
from omni.kit.xr.core import XRCore
from isaacsim.core.prims import SingleXFormPrim
class OpenXRDevice(DeviceBase):
class Hand(Enum):
LEFT = 0
RIGHT = 1
BOTH = 2
TELEOP_COMMAND_EVENT_TYPE = "teleop_command"
"""An OpenXR-powered device for teleoperation and interaction.
This device tracks hand joints using OpenXR and makes them available as:
1. A dictionary of joint poses (when used directly)
2. Retargeted commands for robot control (when a retargeter is provided)
Data format:
* Joint poses: Each pose is a 7D vector (x, y, z, qw, qx, qy, qz) in meters and quaternion units
* Dictionary keys: Joint names from HAND_JOINT_NAMES in isaaclab.devices.openxr.common
* Supported joints include palm, wrist, and joints for thumb, index, middle, ring and little fingers
Teleop commands:
The device responds to several teleop commands that can be subscribed to via add_callback():
* "START": Resume hand tracking data flow
* "STOP": Pause hand tracking data flow
* "RESET": Reset the tracking and signal simulation reset
The device can track either the left hand, right hand, or both hands simultaneously based on
the Hand enum value passed to the constructor. When retargeters are provided, the raw joint
poses are transformed into robot control commands suitable for teleoperation.
"""
def __init__(
self,
xr_cfg: XrCfg | None,
hand: Hand,
retargeters: list[RetargeterBase] | None = None,
):
"""Initialize the hand tracking device.
Args:
hand: Which hand to track (left or right)
retargeters: List of retargeters to transform hand tracking data
"""
super().__init__(retargeters)
self._openxr = OpenXR()
self._xr_cfg = xr_cfg or XrCfg()
self._hand = hand
self._additional_callbacks = dict()
self._vc_subscription = (
XRCore.get_singleton()
.get_message_bus()
.create_subscription_to_pop_by_type(
carb.events.type_from_string(self.TELEOP_COMMAND_EVENT_TYPE), self._on_teleop_command
)
)
self._previous_joint_poses_left = np.full((26, 7), [0, 0, 0, 1, 0, 0, 0], dtype=np.float32)
self._previous_joint_poses_right = np.full((26, 7), [0, 0, 0, 1, 0, 0, 0], dtype=np.float32)
# Specify the placement of the simulation when viewed in an XR device using a prim.
xr_anchor = SingleXFormPrim("/XRAnchor", position=self._xr_cfg.anchor_pos, orientation=self._xr_cfg.anchor_rot)
carb.settings.get_settings().set_string("/persistent/xr/profile/ar/anchorMode", "custom anchor")
carb.settings.get_settings().set_string("/xrstage/profile/ar/customAnchor", xr_anchor.prim_path)
def __del__(self):
"""Clean up resources when the object is destroyed.
Properly unsubscribes from the XR message bus to prevent memory leaks
and resource issues when the device is no longer needed.
"""
if hasattr(self, "_vc_subscription") and self._vc_subscription is not None:
self._vc_subscription = None
# No need to explicitly clean up OpenXR instance as it's managed by NVIDIA Isaac Sim
def __str__(self) -> str:
"""Returns a string containing information about the OpenXR hand tracking device.
This provides details about the device configuration, tracking settings,
and available gesture commands.
Returns:
Formatted string with device information
"""
hand_str = "Both Hands" if self._hand == self.Hand.BOTH else f"{self._hand.name.title()} Hand"
msg = f"OpenXR Hand Tracking Device: {self.__class__.__name__}\n"
msg += f"\tTracking: {hand_str}\n"
msg += f"\tAnchor Position: {self._xr_cfg.anchor_pos}\n"
msg += f"\tAnchor Rotation: {self._xr_cfg.anchor_rot}\n"
# Add retargeter information
if self._retargeters:
msg += "\tRetargeters:\n"
for i, retargeter in enumerate(self._retargeters):
msg += f"\t\t{i + 1}. {retargeter.__class__.__name__}\n"
else:
msg += "\tRetargeters: None (raw joint data output)\n"
# Add available gesture commands
msg += "\t----------------------------------------------\n"
msg += "\tAvailable Gesture Commands:\n"
# Check which callbacks are registered
start_avail = "START" in self._additional_callbacks
stop_avail = "STOP" in self._additional_callbacks
reset_avail = "RESET" in self._additional_callbacks
msg += f"\t\tStart Teleoperation: {'✓' if start_avail else '✗'}\n"
msg += f"\t\tStop Teleoperation: {'✓' if stop_avail else '✗'}\n"
msg += f"\t\tReset Environment: {'✓' if reset_avail else '✗'}\n"
# Add joint tracking information
msg += "\t----------------------------------------------\n"
msg += "\tTracked Joints: All 26 XR hand joints including:\n"
msg += "\t\t- Wrist, palm\n"
msg += "\t\t- Thumb (tip, intermediate joints)\n"
msg += "\t\t- Fingers (tip, distal, intermediate, proximal)\n"
return msg
"""
Operations
"""
def reset(self):
self._previous_joint_poses_left = np.full((26, 7), [0, 0, 0, 1, 0, 0, 0], dtype=np.float32)
self._previous_joint_poses_right = np.full((26, 7), [0, 0, 0, 1, 0, 0, 0], dtype=np.float32)
def add_callback(self, key: str, func: Callable):
self._additional_callbacks[key] = func
def _get_raw_data(self) -> Any:
"""Get the latest hand tracking data.
Returns:
Dictionary of joint poses
"""
if self._hand == self.Hand.LEFT:
hand_joints = self._openxr.locate_hand_joints(OpenXRSpec.XrHandEXT.XR_HAND_LEFT_EXT)
return self._calculate_joint_poses(hand_joints, self._previous_joint_poses_left)
elif self._hand == self.Hand.RIGHT:
hand_joints = self._openxr.locate_hand_joints(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT)
return self._calculate_joint_poses(hand_joints, self._previous_joint_poses_right)
else:
return {
self.Hand.LEFT: self._calculate_joint_poses(
self._openxr.locate_hand_joints(OpenXRSpec.XrHandEXT.XR_HAND_LEFT_EXT),
self._previous_joint_poses_left,
),
self.Hand.RIGHT: self._calculate_joint_poses(
self._openxr.locate_hand_joints(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT),
self._previous_joint_poses_right,
),
}
"""
Internal helpers.
"""
def _calculate_joint_poses(self, hand_joints, previous_joint_poses) -> dict[str, np.ndarray]:
if hand_joints is None:
return self._joints_to_dict(previous_joint_poses)
hand_joints = np.array(hand_joints)
positions = np.array([[j.pose.position.x, j.pose.position.y, j.pose.position.z] for j in hand_joints])
orientations = np.array([
[j.pose.orientation.w, j.pose.orientation.x, j.pose.orientation.y, j.pose.orientation.z]
for j in hand_joints
])
location_flags = np.array([j.locationFlags for j in hand_joints])
pos_mask = (location_flags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT) != 0
ori_mask = (location_flags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) != 0
previous_joint_poses[pos_mask, 0:3] = positions[pos_mask]
previous_joint_poses[ori_mask, 3:7] = orientations[ori_mask]
return self._joints_to_dict(previous_joint_poses)
def _joints_to_dict(self, joint_data: np.ndarray) -> dict[str, np.ndarray]:
"""Convert joint array to dictionary using standard joint names.
Args:
joint_data: Array of joint data (Nx6 for N joints)
Returns:
Dictionary mapping joint names to their data
"""
return {joint_name: joint_data[i] for i, joint_name in enumerate(HAND_JOINT_NAMES)}
def _on_teleop_command(self, event: carb.events.IEvent):
msg = event.payload["message"]
if "start" in msg:
if "START" in self._additional_callbacks:
self._additional_callbacks["START"]()
elif "stop" in msg:
if "STOP" in self._additional_callbacks:
self._additional_callbacks["STOP"]()
elif "reset" in msg:
if "RESET" in self._additional_callbacks:
self._additional_callbacks["RESET"]()
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Retargeters for mapping input device data to robot commands."""
from .manipulator.gripper_retargeter import GripperRetargeter
from .manipulator.se3_abs_retargeter import Se3AbsRetargeter
from .manipulator.se3_rel_retargeter import Se3RelRetargeter
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# 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")
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Franka manipulator retargeting module.
This module provides functionality for retargeting motion to Franka robots.
"""
from .gripper_retargeter import GripperRetargeter
from .se3_abs_retargeter import Se3AbsRetargeter
from .se3_rel_retargeter import Se3RelRetargeter
# Copyright (c) 2024-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import numpy as np
from typing import Final
from isaaclab.devices.retargeter_base import RetargeterBase
class GripperRetargeter(RetargeterBase):
"""Retargeter specifically for gripper control based on hand tracking data.
This retargeter analyzes the distance between thumb and index finger tips to determine
whether the gripper should be open or closed. It includes hysteresis to prevent rapid
toggling between states when the finger distance is near the threshold.
Features:
- Tracks thumb and index finger distance
- Implements hysteresis for stable gripper control
- Outputs boolean command (True = close gripper, False = open gripper)
"""
GRIPPER_CLOSE_METERS: Final[float] = 0.03
GRIPPER_OPEN_METERS: Final[float] = 0.05
def __init__(
self,
):
"""Initialize the gripper retargeter."""
# Initialize gripper state
self._previous_gripper_command = False
def retarget(self, data: dict[str, np.ndarray]) -> bool:
"""Convert hand joint poses to gripper command.
Args:
data: Dictionary mapping joint names to their pose data,
joint names are defined in isaaclab.devices.openxr.common.HAND_JOINT_NAMES
Returns:
bool: Gripper command where True = close gripper, False = open gripper
"""
# Extract key joint poses
thumb_tip = data.get("thumb_tip")
index_tip = data.get("index_tip")
# Calculate gripper command with hysteresis
gripper_command = self._calculate_gripper_command(thumb_tip[:3], index_tip[:3])
return gripper_command
def _calculate_gripper_command(self, thumb_pos: np.ndarray, index_pos: np.ndarray) -> bool:
"""Calculate gripper command from finger positions with hysteresis.
Args:
thumb_pos: 3D position of thumb tip
index_pos: 3D position of index tip
Returns:
bool: Gripper command (True = close, False = open)
"""
distance = np.linalg.norm(thumb_pos - index_pos)
# Apply hysteresis to prevent rapid switching
if distance > self.GRIPPER_OPEN_METERS:
self._previous_gripper_command = False
elif distance < self.GRIPPER_CLOSE_METERS:
self._previous_gripper_command = True
return self._previous_gripper_command
# Copyright (c) 2024-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import numpy as np
from scipy.spatial.transform import Rotation, Slerp
from isaaclab.devices.retargeter_base import RetargeterBase
from isaaclab.markers import VisualizationMarkers
from isaaclab.markers.config import FRAME_MARKER_CFG
class Se3AbsRetargeter(RetargeterBase):
"""Retargets OpenXR hand tracking data to end-effector commands using absolute positioning.
This retargeter maps hand joint poses directly to robot end-effector positions and orientations,
rather than using relative movements. It can either:
- Use the wrist position and orientation
- Use the midpoint between thumb and index finger (pinch position)
Features:
- Optional constraint to zero out X/Y rotations (keeping only Z-axis rotation)
- Optional visualization of the target end-effector pose
"""
def __init__(
self,
zero_out_xy_rotation: bool = False,
use_wrist_rotation: bool = False,
use_wrist_position: bool = False,
enable_visualization: bool = False,
):
"""Initialize the retargeter.
Args:
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
enable_visualization: If True, visualize the target pose in the scene
"""
self._zero_out_xy_rotation = zero_out_xy_rotation
self._use_wrist_rotation = use_wrist_rotation
self._use_wrist_position = use_wrist_position
# Initialize visualization if enabled
self._enable_visualization = enable_visualization
if enable_visualization:
frame_marker_cfg = FRAME_MARKER_CFG.copy()
frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
self._goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal"))
self._goal_marker.set_visibility(True)
self._visualization_pos = np.zeros(3)
self._visualization_rot = np.array([1.0, 0.0, 0.0, 0.0])
def retarget(self, data: dict[str, np.ndarray]) -> np.ndarray:
"""Convert hand joint poses to robot end-effector command.
Args:
data: Dictionary mapping joint names to their pose data,
joint names are defined in isaaclab.devices.openxr.common.HAND_JOINT_NAMES
Returns:
np.ndarray: 7D array containing position (xyz) and orientation (quaternion)
for the robot end-effector
"""
# Extract key joint poses
thumb_tip = data.get("thumb_tip")
index_tip = data.get("index_tip")
wrist = data.get("wrist")
ee_command = self._retarget_abs(thumb_tip, index_tip, wrist)
return ee_command
def _retarget_abs(self, thumb_tip: np.ndarray, index_tip: np.ndarray, wrist: np.ndarray) -> np.ndarray:
"""Handle absolute pose retargeting.
Args:
thumb_tip: 7D array containing position (xyz) and orientation (quaternion)
for the thumb tip
index_tip: 7D array containing position (xyz) and orientation (quaternion)
for the index tip
wrist: 7D array containing position (xyz) and orientation (quaternion)
for the wrist
Returns:
np.ndarray: 7D array containing position (xyz) and orientation (quaternion)
for the robot end-effector
"""
# Get position
if self._use_wrist_position:
position = wrist[:3]
else:
position = (thumb_tip[:3] + index_tip[:3]) / 2
# Get rotation
if self._use_wrist_rotation:
# wrist is w,x,y,z but scipy expects x,y,z,w
base_rot = Rotation.from_quat([wrist[4:], wrist[3]])
else:
# Average the orientations of thumb and index using SLERP
# thumb_tip is w,x,y,z but scipy expects x,y,z,w
r0 = Rotation.from_quat([*thumb_tip[4:], thumb_tip[3]])
# index_tip is w,x,y,z but scipy expects x,y,z,w
r1 = Rotation.from_quat([*index_tip[4:], index_tip[3]])
key_times = [0, 1]
slerp = Slerp(key_times, Rotation.concatenate([r0, r1]))
base_rot = slerp([0.5])[0]
# Apply additional x-axis rotation to align with pinch gesture
final_rot = base_rot * Rotation.from_euler("x", 90, degrees=True)
if self._zero_out_xy_rotation:
z, y, x = final_rot.as_euler("ZYX")
y = 0.0 # Zero out rotation around y-axis
x = 0.0 # Zero out rotation around x-axis
final_rot = Rotation.from_euler("ZYX", [z, y, x]) * Rotation.from_euler("X", np.pi, degrees=False)
# Convert back to w,x,y,z format
quat = final_rot.as_quat()
rotation = np.array([quat[3], quat[0], quat[1], quat[2]]) # Output remains w,x,y,z
# Update visualization if enabled
if self._enable_visualization:
self._visualization_pos = position
self._visualization_rot = rotation
self._update_visualization()
return np.concatenate([position, rotation])
def _update_visualization(self):
"""Update visualization markers with current pose.
If visualization is enabled, the target end-effector pose is visualized in the scene.
"""
if self._enable_visualization:
trans = np.array([self._visualization_pos])
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)
# Copyright (c) 2024-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import numpy as np
from scipy.spatial.transform import Rotation
from isaaclab.devices.retargeter_base import RetargeterBase
from isaaclab.markers import VisualizationMarkers
from isaaclab.markers.config import FRAME_MARKER_CFG
class Se3RelRetargeter(RetargeterBase):
"""Retargets OpenXR hand tracking data to end-effector commands using relative positioning.
This retargeter calculates delta poses between consecutive hand joint poses to generate incremental robot movements.
It can either:
- Use the wrist position and orientation
- Use the midpoint between thumb and index finger (pinch position)
Features:
- Optional constraint to zero out X/Y rotations (keeping only Z-axis rotation)
- Motion smoothing with adjustable parameters
- Optional visualization of the target end-effector pose
"""
def __init__(
self,
zero_out_xy_rotation: bool = False,
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,
):
"""Initialize the relative motion retargeter.
Args:
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)
delta_pos_scale_factor: Amplification factor for position changes (higher = larger robot movements)
delta_rot_scale_factor: Amplification factor for rotation changes (higher = larger robot rotations)
alpha_pos: Position smoothing parameter (0-1); higher values track more closely to input, lower values smooth more
alpha_rot: Rotation smoothing parameter (0-1); higher values track more closely to input, lower values smooth more
enable_visualization: If True, show a visual marker representing the target end-effector pose
"""
self._zero_out_xy_rotation = zero_out_xy_rotation
self._use_wrist_rotation = use_wrist_rotation
self._use_wrist_position = use_wrist_position
self._delta_pos_scale_factor = delta_pos_scale_factor
self._delta_rot_scale_factor = delta_rot_scale_factor
self._alpha_pos = alpha_pos
self._alpha_rot = alpha_rot
# Initialize smoothing state
self._smoothed_delta_pos = np.zeros(3)
self._smoothed_delta_rot = np.zeros(3)
# Define thresholds for small movements
self._position_threshold = 0.001
self._rotation_threshold = 0.01
# Initialize visualization if enabled
self._enable_visualization = enable_visualization
if enable_visualization:
frame_marker_cfg = FRAME_MARKER_CFG.copy()
frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
self._goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal"))
self._goal_marker.set_visibility(True)
self._visualization_pos = np.zeros(3)
self._visualization_rot = np.array([1.0, 0.0, 0.0, 0.0])
self._previous_thumb_tip = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32)
self._previous_index_tip = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32)
self._previous_wrist = np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], dtype=np.float32)
def retarget(self, data: dict[str, np.ndarray]) -> np.ndarray:
"""Convert hand joint poses to robot end-effector command.
Args:
data: Dictionary mapping joint names to their pose data,
joint names are defined in isaaclab.devices.openxr.common.HAND_JOINT_NAMES
Returns:
np.ndarray: 6D array containing position (xyz) and rotation vector (rx,ry,rz)
for the robot end-effector
"""
# Extract key joint poses
thumb_tip = data.get("thumb_tip")
index_tip = data.get("index_tip")
wrist = data.get("wrist")
delta_thumb_tip = self._calculate_delta_pose(thumb_tip, self._previous_thumb_tip)
delta_index_tip = self._calculate_delta_pose(index_tip, self._previous_index_tip)
delta_wrist = self._calculate_delta_pose(wrist, self._previous_wrist)
ee_command = self._retarget_rel(delta_thumb_tip, delta_index_tip, delta_wrist)
self._previous_thumb_tip = thumb_tip.copy()
self._previous_index_tip = index_tip.copy()
self._previous_wrist = wrist.copy()
return ee_command
def _calculate_delta_pose(self, joint_pose: np.ndarray, previous_joint_pose: np.ndarray) -> np.ndarray:
"""Calculate delta pose from previous joint pose.
Args:
joint_pose: Current joint pose (position and orientation)
previous_joint_pose: Previous joint pose for the same joint
Returns:
np.ndarray: 6D array with position delta (xyz) and rotation delta as axis-angle (rx,ry,rz)
"""
delta_pos = joint_pose[:3] - previous_joint_pose[:3]
abs_rotation = Rotation.from_quat([*joint_pose[4:7], joint_pose[3]])
previous_rot = Rotation.from_quat([*previous_joint_pose[4:7], previous_joint_pose[3]])
relative_rotation = abs_rotation * previous_rot.inv()
return np.concatenate([delta_pos, relative_rotation.as_rotvec()])
def _retarget_rel(self, thumb_tip: np.ndarray, index_tip: np.ndarray, wrist: np.ndarray) -> np.ndarray:
"""Handle relative (delta) pose retargeting.
Args:
thumb_tip: Delta pose of thumb tip
index_tip: Delta pose of index tip
wrist: Delta pose of wrist
Returns:
np.ndarray: 6D array with position delta (xyz) and rotation delta (rx,ry,rz)
"""
# Get position
if self._use_wrist_position:
position = wrist[:3]
else:
position = (thumb_tip[:3] + index_tip[:3]) / 2
# Get rotation
if self._use_wrist_rotation:
rotation = wrist[3:6] # rx, ry, rz
else:
rotation = (thumb_tip[3:6] + index_tip[3:6]) / 2
# Apply zero_out_xy_rotation regardless of rotation source
if self._zero_out_xy_rotation:
rotation[0] = 0 # x-axis
rotation[1] = 0 # y-axis
# Smooth and scale position
self._smoothed_delta_pos = self._alpha_pos * position + (1 - self._alpha_pos) * self._smoothed_delta_pos
if np.linalg.norm(self._smoothed_delta_pos) < self._position_threshold:
self._smoothed_delta_pos = np.zeros(3)
position = self._smoothed_delta_pos * self._delta_pos_scale_factor
# Smooth and scale rotation
self._smoothed_delta_rot = self._alpha_rot * rotation + (1 - self._alpha_rot) * self._smoothed_delta_rot
if np.linalg.norm(self._smoothed_delta_rot) < self._rotation_threshold:
self._smoothed_delta_rot = np.zeros(3)
rotation = self._smoothed_delta_rot * self._delta_rot_scale_factor
# Update visualization if enabled
if self._enable_visualization:
# Convert rotation vector to quaternion and combine with current rotation
delta_quat = Rotation.from_rotvec(rotation).as_quat() # x, y, z, w format
current_rot = Rotation.from_quat([self._visualization_rot[1:], self._visualization_rot[0]])
new_rot = Rotation.from_quat(delta_quat) * current_rot
self._visualization_pos = self._visualization_pos + position
# Convert back to w, x, y, z format
self._visualization_rot = np.array([new_rot.as_quat()[3], *new_rot.as_quat()[:3]])
self._update_visualization()
return np.concatenate([position, rotation])
def _update_visualization(self):
"""Update visualization markers with current pose."""
if self._enable_visualization:
trans = np.array([self._visualization_pos])
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)
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
import carb
from omni.kit.xr.core import XRCore
TELEOP_COMMAND_EVENT_TYPE = "teleop_command"
# The event type for outgoing teleop command response text message.
TELEOP_COMMAND_RESPONSE_EVENT_TYPE = "teleop_command_response"
class Actions:
"""Class for action options."""
START: str = "START"
STOP: str = "STOP"
RESET: str = "RESET"
UNKNOWN: str = "UNKNOWN"
class TeleopCommand:
"""This class handles the message process with key word."""
def __init__(self) -> None:
self._message_bus = XRCore.get_singleton().get_message_bus()
self._incoming_message_event = carb.events.type_from_string(TELEOP_COMMAND_EVENT_TYPE)
self._outgoing_message_event = carb.events.type_from_string(TELEOP_COMMAND_RESPONSE_EVENT_TYPE)
self._subscription = self._message_bus.create_subscription_to_pop_by_type(
self._incoming_message_event, self._on_message
)
def _process_message(self, event: carb.events.IEvent):
"""Processes the received message using key word."""
message_in = event.payload["message"]
if "start" in message_in:
message_out = Actions.START
elif "stop" in message_in:
message_out = Actions.STOP
elif "reset" in message_in:
message_out = Actions.RESET
else:
message_out = Actions.UNKNOWN
print(f"[VC-Keyword] message_out: {message_out}")
# Send the response back through the message bus.
self._message_bus.push(self._outgoing_message_event, payload={"message": message_out})
carb.log_info(f"Sent response: {message_out}")
def _on_message(self, event: carb.events.IEvent):
carb.log_info(f"Received message: {event.payload['message']}")
self._process_message(event)
def unsubscribe(self):
self._subscription.unsubscribe()
......@@ -23,9 +23,8 @@ import unittest
import carb
import omni.usd
from isaacsim.core.prims import XFormPrim
from isaacsim.xr.openxr import OpenXRSpec
from isaaclab.devices import Se3HandTracking
from isaaclab.devices import OpenXRDevice
from isaaclab.devices.openxr import XrCfg
from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg
from isaaclab.scene import InteractiveSceneCfg
......@@ -62,8 +61,8 @@ class EmptyEnvCfg(ManagerBasedEnvCfg):
self.sim.render_interval = 2
class TestSe3HandTracking(unittest.TestCase):
"""Test for Se3HandTracking"""
class TestOpenXRDevice(unittest.TestCase):
"""Test for OpenXRDevice"""
def test_xr_anchor(self):
env_cfg = EmptyEnvCfg()
......@@ -74,7 +73,7 @@ class TestSe3HandTracking(unittest.TestCase):
# Create environment.
env = ManagerBasedEnv(cfg=env_cfg)
device = Se3HandTracking(env_cfg.xr, OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT)
device = OpenXRDevice(env_cfg.xr, OpenXRDevice.Hand.RIGHT)
# Check that the xr anchor prim is created with the correct pose.
xr_anchor_prim = XFormPrim("/XRAnchor")
......@@ -98,7 +97,7 @@ class TestSe3HandTracking(unittest.TestCase):
# Create environment.
env = ManagerBasedEnv(cfg=env_cfg)
device = Se3HandTracking(None, OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT)
device = OpenXRDevice(None, OpenXRDevice.Hand.RIGHT)
# Check that the xr anchor prim is created with the correct default pose.
xr_anchor_prim = XFormPrim("/XRAnchor")
......@@ -122,8 +121,8 @@ class TestSe3HandTracking(unittest.TestCase):
# Create environment.
env = ManagerBasedEnv(cfg=env_cfg)
device_1 = Se3HandTracking(None, OpenXRSpec.XrHandEXT.XR_HAND_LEFT_EXT)
device_2 = Se3HandTracking(None, OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT)
device_1 = OpenXRDevice(None, OpenXRDevice.Hand.LEFT)
device_2 = OpenXRDevice(None, OpenXRDevice.Hand.RIGHT)
# Check that the xr anchor prim is created with the correct default pose.
xr_anchor_prim = XFormPrim("/XRAnchor")
......
# Copyright (c) 2022-2025, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
from abc import ABC, abstractmethod
from typing import Any
class RetargeterBase(ABC):
"""Base interface for input data retargeting.
This abstract class defines the interface for components that transform
raw device data into robot control commands. Implementations can handle
various types of transformations including:
- Hand joint data to end-effector poses
- Input device commands to robot movements
- Sensor data to control signals
"""
@abstractmethod
def retarget(self, data: Any) -> Any:
"""Retarget input data to desired output format.
Args:
data: Raw input data to be transformed
Returns:
Retargeted data in implementation-specific format
"""
pass
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