Commit a2c0cdbd authored by rwiltz's avatar rwiltz Committed by Kelly Guo

Adds XR hand tracking input device (#160)

- Added the SE3_HandTracking input device which allows for XR teleop via
the SE3 interface.
- Updated the teleop_se3_agent to support hand tracking via the
`--teleop_device handtracking` flag
- Created a new .kit file `isaaclab.python.xr.openxr.kit` for supporting
teleop experiances

- New feature (non-breaking change which adds functionality)

- [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 avatarpeterd-NV <peterd@nvidia.com>
Co-authored-by: 's avatarpeterd-NV <peterd@nvidia.com>
Co-authored-by: 's avatarCY Chen <cyc@nvidia.com>
Co-authored-by: 's avataroahmednv <oahmed@Nvidia.com>
Co-authored-by: 's avatarToni-SM <aserranomuno@nvidia.com>
parent 57d96d7a
##
# Adapted from: _isaac_sim/apps/isaacsim.exp.xr.openxr.kit
##
[package]
title = "Isaac Lab Python OpenXR"
description = "An app for running Isaac Lab with OpenXR"
version = "1.2.0"
# That makes it browsable in UI with "experience" filter
keywords = ["experience", "app", "usd"]
[settings]
# Note: This path was adapted to be respective to the kit-exe file location
app.versionFile = "${exe-path}/VERSION"
app.folder = "${exe-path}/"
app.name = "Isaac-Sim"
app.version = "4.5.0"
[dependencies]
"isaaclab.python" = {}
"isaacsim.xr.openxr" = {}
# Kit extensions
"omni.kit.xr.system.openxr" = {}
"omni.kit.xr.profile.ar" = {}
[settings]
# xr settings
xr.ui.enabled = false
xrstage.profile.ar.anchorMode = "scene origin"
# Register extension folder from this repo in kit
[settings.app.exts]
folders = [
"${exe-path}/exts", # kit extensions
"${exe-path}/extscore", # kit core extensions
"${exe-path}/../exts", # isaac extensions
"${exe-path}/../extsDeprecated", # deprecated isaac extensions
"${exe-path}/../extscache", # isaac cache extensions
"${exe-path}/../extsPhysics", # isaac physics extensions
"${exe-path}/../isaacsim/exts", # isaac extensions for pip
"${exe-path}/../isaacsim/extsDeprecated", # deprecated isaac extensions
"${exe-path}/../isaacsim/extscache", # isaac cache extensions for pip
"${exe-path}/../isaacsim/extsPhysics", # isaac physics extensions for pip
"${app}", # needed to find other app files
"${app}/../extensions", # needed to find extensions in Isaac Lab
]
......@@ -163,7 +163,7 @@ Added
* Added full buffer property to :class:`omni.isaac.lab.utils.buffers.circular_buffer.CircularBuffer`
0.27.29 (2024-12-15)
0.27.30 (2024-12-15)
~~~~~~~~~~~~~~~~~~~~
Added
......@@ -172,7 +172,7 @@ Added
* Added action clip to all :class:`omni.isaac.lab.envs.mdp.actions`.
0.27.28 (2024-12-14)
0.27.29 (2024-12-14)
~~~~~~~~~~~~~~~~~~~~
Changed
......@@ -181,7 +181,7 @@ Changed
* Added check for error below threshold in state machines to ensure the state has been reached.
0.27.27 (2024-12-13)
0.27.28 (2024-12-13)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -190,7 +190,7 @@ Fixed
* Fixed the shape of ``quat_w`` in the ``apply_actions`` method of :attr:`~omni.isaac.lab.env.mdp.NonHolonomicAction` (previously (N,B,4), now (N,4) since the number of root bodies B is required to be 1). Previously ``apply_actions`` errored because ``euler_xyz_from_quat`` requires inputs of shape (N,4).
0.27.26 (2024-12-11)
0.27.27 (2024-12-11)
~~~~~~~~~~~~~~~~~~~~
Changed
......@@ -201,7 +201,7 @@ Changed
* Improved documentation to clarify the usage of the :meth:`~omni.isaac.lab.envs.mdp.rewards.base_height_l2` function in both flat and rough terrain settings.
0.27.25 (2024-12-11)
0.27.26 (2024-12-11)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -211,7 +211,7 @@ Fixed
Jacobian computed w.r.t. to the root frame of the robot. This helps ensure that root pose does not affect the tracking.
0.27.24 (2024-12-09)
0.27.25 (2024-12-09)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -221,7 +221,7 @@ Fixed
return only the states of the specified environment IDs.
0.27.23 (2024-12-06)
0.27.24 (2024-12-06)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -231,7 +231,7 @@ Fixed
:attr:`~omni.isaac.lab.assets.Articulation.root_physx_view` level.
0.27.22 (2024-12-06)
0.27.23 (2024-12-06)
~~~~~~~~~~~~~~~~~~~~
Changed
......@@ -242,7 +242,7 @@ Changed
disabled. Using an articulation root for rigid bodies is not needed and decreases overall performance.
0.27.21 (2024-12-06)
0.27.22 (2024-12-06)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -252,7 +252,7 @@ Fixed
Earlier, the projection names used snakecase instead of camelcase.
0.27.20 (2024-12-06)
0.27.21 (2024-12-06)
~~~~~~~~~~~~~~~~~~~~
Added
......@@ -270,7 +270,7 @@ Changed
:class:`~omni.isaac.lab.sensors.Camera` did not clip them and had a different behavior for both types.
0.27.19 (2024-12-05)
0.27.20 (2024-12-05)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -279,7 +279,7 @@ Fixed
* Fixed the condition in ``isaaclab.sh`` that checks whether ``pre-commit`` is installed before attempting installation.
0.27.18 (2024-12-04)
0.27.19 (2024-12-04)
~~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -288,7 +288,7 @@ Fixed
* Fixed the order of the incoming parameters in :class:`omni.isaac.lab.envs.DirectMARLEnv` to correctly use ``NoiseModel`` in marl-envs.
0.27.17 (2024-12-02)
0.27.18 (2024-12-04)
~~~~~~~~~~~~~~~~~~~~
Added
......@@ -303,7 +303,7 @@ Added
* Added ``replay_demos.py`` script to replay demos loaded from an HDF5 file.
0.27.16 (2024-11-21)
0.27.17 (2024-12-02)
~~~~~~~~~~~~~~~~~~~~
Changed
......@@ -312,6 +312,15 @@ Changed
* Changed :class:`omni.isaac.lab.envs.DirectMARLEnv` to inherit from ``Gymnasium.Env`` due to requirement from Gymnasium v1.0.0 requiring all environments to be a subclass of ``Gymnasium.Env`` when using the ``make`` interface.
0.27.16 (2024-11-15)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added the class :class:`~omni.isaac.lab.devices.Se3HandTracking` which enables XR teleop for manipulators.
0.27.15 (2024-11-09)
~~~~~~~~~~~~~~~~~~~~
......
......@@ -10,6 +10,7 @@ Currently, the following categories of devices are supported:
* **Keyboard**: Standard keyboard with WASD and arrow keys.
* **Spacemouse**: 3D mouse with 6 degrees of freedom.
* **Gamepad**: Gamepad with 2D two joysticks and buttons. Example: Xbox controller.
* **OpenXR**: Uses hand tracking of index/thumb tip avg to drive the target pose. Gripping is done with pinching.
All device interfaces inherit from the :class:`DeviceBase` class, which provides a
common interface for all devices. The device interface reads the input data when
......@@ -21,4 +22,5 @@ the peripheral device.
from .device_base import DeviceBase
from .gamepad import Se2Gamepad, Se3Gamepad
from .keyboard import Se2Keyboard, Se3Keyboard
from .openxr import Se3HandTracking
from .spacemouse import Se2SpaceMouse, Se3SpaceMouse
# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""Keyboard device for SE(2) and SE(3) control."""
from .se3_handtracking import Se3HandTracking
# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
# All rights reserved.
#
# SPDX-License-Identifier: BSD-3-Clause
"""OpenXR handtracking controller for SE(3) control."""
import contextlib
import numpy as np
from collections.abc import Callable
from scipy.spatial.transform import Rotation, Slerp
from typing import Final
import carb
from ..device_base import DeviceBase
with contextlib.suppress(ModuleNotFoundError):
from isaacsim.xr.openxr import OpenXR, OpenXRSpec
from omni.kit.xr.core import XRCore
from . import teleop_command
class Se3HandTracking(DeviceBase):
"""A OpenXR powered hand tracker for sending SE(3) commands as delta poses
as well as teleop commands via callbacks.
The command comprises of two parts as well as callbacks for teleop commands:
* delta pose: a 6D vector of (x, y, z, roll, pitch, yaw) in meters and radians.
* gripper: a binary command to open or close the gripper.
*
* Teleop commands can be subscribed via the add_callback function, the available keys are:
* - "START": Resumes hand tracking flow
* - "STOP": Stops hand tracking flow
* - "RESET": signals simulation reset
"""
GRIP_HYSTERESIS_METERS: Final[float] = 0.0254 # 1.0 inch
DELTA_POS_SCALE_FACTOR = 20
DELTA_ROT_SCALE_FACTOR = 5
def __init__(self, hand, abs=True):
self._openxr = OpenXR()
self._previous_pos = np.zeros(3)
self._previous_rot = np.zeros(3)
self._previous_grip_distance = 0.0
self._previous_gripper_command = False
self._hand = hand
self._abs = abs
self._additional_callbacks = dict()
self._vc = teleop_command.TeleopCommand()
self._vc_subscription = (
XRCore.get_singleton()
.get_message_bus()
.create_subscription_to_pop_by_type(
carb.events.type_from_string(teleop_command.TELEOP_COMMAND_RESPONSE_EVENT_TYPE), self._on_teleop_command
)
)
self._tracking = False
def __del__(self):
return
def __str__(self) -> str:
return ""
"""
Operations
"""
def reset(self):
self._previous_pos = np.zeros(3)
self._previous_rot = np.zeros(3)
self._previous_grip_distance = 0.0
self._previous_gripper_command = False
def add_callback(self, key: str, func: Callable):
self._additional_callbacks[key] = func
def advance(self) -> tuple[np.ndarray, bool]:
"""Provides the result from spacemouse event state.
Returns:
A tuple containing the delta pose command and gripper commands.
"""
hand_joints = self._openxr.locate_hand_joints(self._hand)
if hand_joints is None:
return np.zeros(6), self._previous_gripper_command
if self._abs:
pose = self._calculate_abs_target_pose(hand_joints)
else:
pose = self._calculate_target_delta_pose(hand_joints)
return pose, self._calculate_gripper_command(hand_joints)
"""
Internal helpers.
"""
def _calculate_abs_target_pose(self, hand_joints):
if not self._tracking:
return np.zeros(6)
index_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_INDEX_TIP_EXT]
thumb_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_THUMB_TIP_EXT]
# position:
if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT:
target_pos = self._previous_pos
if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT:
target_pos = self._previous_pos
else:
index_tip_pos = index_tip.pose.position
index_tip_position = np.array([index_tip_pos.x, index_tip_pos.y, index_tip_pos.z], dtype=np.float32)
thumb_tip_pos = thumb_tip.pose.position
thumb_tip_position = np.array([thumb_tip_pos.x, thumb_tip_pos.y, thumb_tip_pos.z], dtype=np.float32)
target_pos = (index_tip_position + thumb_tip_position) / 2
self._previous_pos = target_pos
# rotation
if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT:
target_rot = self._previous_rot
if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT:
target_rot = self._previous_rot
else:
index_tip_quat = index_tip.pose.orientation
index_tip_quat = np.array(
[index_tip_quat.x, index_tip_quat.y, index_tip_quat.z, index_tip_quat.w], dtype=np.float32
)
thumb_tip_quat = thumb_tip.pose.orientation
thumb_tip_quat = np.array(
[thumb_tip_quat.x, thumb_tip_quat.y, thumb_tip_quat.z, thumb_tip_quat.w], dtype=np.float32
)
r0 = Rotation.from_quat(index_tip_quat)
r1 = Rotation.from_quat(thumb_tip_quat)
key_times = [0, 1]
slerp = Slerp(key_times, Rotation.concatenate([r0, r1]))
interp_time = [0.5]
interp_rotation = slerp(interp_time)[0]
target_rot = interp_rotation.as_rotvec()
self._previous_rot = target_rot
return np.concatenate([target_pos, target_rot])
def _calculate_target_delta_pose(self, hand_joints):
index_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_INDEX_TIP_EXT]
thumb_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_THUMB_TIP_EXT]
# position:
if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT:
delta_pos = np.zeros(3)
if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT:
delta_pos = np.zeros(3)
else:
index_tip_pos = index_tip.pose.position
index_tip_position = np.array([index_tip_pos.x, index_tip_pos.y, index_tip_pos.z], dtype=np.float32)
thumb_tip_pos = thumb_tip.pose.position
thumb_tip_position = np.array([thumb_tip_pos.x, thumb_tip_pos.y, thumb_tip_pos.z], dtype=np.float32)
target_pos = (index_tip_position + thumb_tip_position) / 2
delta_pos = target_pos - self._previous_pos
self._previous_pos = target_pos
# rotation
if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT:
delta_rot = np.zeros(3)
if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT:
delta_rot = np.zeros(3)
else:
index_tip_quat = index_tip.pose.orientation
index_tip_quat = np.array(
[index_tip_quat.x, index_tip_quat.y, index_tip_quat.z, index_tip_quat.w], dtype=np.float32
)
thumb_tip_quat = thumb_tip.pose.orientation
thumb_tip_quat = np.array(
[thumb_tip_quat.x, thumb_tip_quat.y, thumb_tip_quat.z, thumb_tip_quat.w], dtype=np.float32
)
r0 = Rotation.from_quat(index_tip_quat)
r1 = Rotation.from_quat(thumb_tip_quat)
key_times = [0, 1]
slerp = Slerp(key_times, Rotation.concatenate([r0, r1]))
interp_time = [0.5]
interp_rotation = slerp(interp_time)[0]
target_rot = interp_rotation.as_rotvec()
delta_rot = target_rot - self._previous_rot
self._previous_rot = target_rot
# if not tracking still update prev positions but return no delta pose
if not self._tracking:
return np.zeros(6)
return np.concatenate([delta_pos * self.DELTA_POS_SCALE_FACTOR, delta_rot * self.DELTA_ROT_SCALE_FACTOR])
def _calculate_gripper_command(self, hand_joints):
index_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_INDEX_TIP_EXT]
thumb_tip = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_THUMB_TIP_EXT]
if not self._tracking:
return self._previous_gripper_command
if not index_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT:
return self._previous_gripper_command
if not thumb_tip.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT:
return self._previous_gripper_command
index_tip_pos = index_tip.pose.position
index_tip_pos = np.array([index_tip_pos.x, index_tip_pos.y, index_tip_pos.z], dtype=np.float32)
thumb_tip_pos = thumb_tip.pose.position
thumb_tip_pos = np.array([thumb_tip_pos.x, thumb_tip_pos.y, thumb_tip_pos.z], dtype=np.float32)
distance = np.linalg.norm(index_tip_pos - thumb_tip_pos)
if distance > self._previous_grip_distance + self.GRIP_HYSTERESIS_METERS:
self._previous_grip_distance = distance
self._previous_gripper_command = False
elif distance < self._previous_grip_distance - self.GRIP_HYSTERESIS_METERS:
self._previous_grip_distance = distance
self._previous_gripper_command = True
return self._previous_gripper_command
def _on_teleop_command(self, event: carb.events.IEvent):
msg = event.payload["message"]
if teleop_command.Actions.START in msg:
if "START" in self._additional_callbacks:
self._additional_callbacks["START"]()
self._tracking = True
elif teleop_command.Actions.STOP in msg:
if "STOP" in self._additional_callbacks:
self._additional_callbacks["STOP"]()
self._tracking = False
elif teleop_command.Actions.RESET in msg:
if "RESET" in self._additional_callbacks:
self._additional_callbacks["RESET"]()
# Copyright (c) 2022-2024, 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()
......@@ -8,6 +8,7 @@
"""Launch Isaac Sim Simulator first."""
import argparse
import os
from omni.isaac.lab.app import AppLauncher
......@@ -25,8 +26,14 @@ AppLauncher.add_app_launcher_args(parser)
# parse the arguments
args_cli = parser.parse_args()
app_launcher_args = {}
if args_cli.teleop_device.lower() == "handtracking":
app_launcher_args = {
"experience": f'{os.environ["ISAACLAB_PATH"]}/source/apps/isaaclab.python.xr.openxr.kit',
}
# launch omniverse app
app_launcher = AppLauncher(headless=args_cli.headless)
app_launcher = AppLauncher(app_launcher_args, headless=args_cli.headless)
simulation_app = app_launcher.app
"""Rest everything follows."""
......@@ -37,7 +44,7 @@ import torch
import omni.log
from omni.isaac.lab.devices import Se3Gamepad, Se3Keyboard, Se3SpaceMouse
from omni.isaac.lab.devices import Se3Gamepad, Se3HandTracking, Se3Keyboard, Se3SpaceMouse
from omni.isaac.lab.managers import TerminationTermCfg as DoneTerm
import omni.isaac.lab_tasks # noqa: F401
......@@ -94,6 +101,11 @@ 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
teleop_interface = Se3HandTracking(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT, False)
teleop_interface.add_callback("RESET", env.reset)
else:
raise ValueError(f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse'.")
# add teleoperation key for env reset
......
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