Commit 3f1be462 authored by rwiltz's avatar rwiltz Committed by Kelly Guo

Migrates OpenXRDevice from isaacsim.xr.openxr to omni.xr.kitxr (#391)

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link:
https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html
-->

IsaacSim has deprecated isaacsim.xr.openxr extension for hand tracking
in favor of omni.xr.kitxr, so this change will migrate the OpenXRDevice
to omni.xr.kitxr. This also allows for additional features like XCR
record and replay.

Fixes # (issue)

<!-- As a practice, it is recommended to open an issue to have
discussions on the proposed pull request.
This makes it easier for the community to keep track of what is being
developed or added, and if a given feature
is demanded by more than one party. -->

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

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

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 c37ccce8
......@@ -44,6 +44,8 @@ defaults.xr.profile.ar.renderQuality = "off"
defaults.xr.profile.ar.anchorMode = "custom anchor"
rtx.rendermode = "RaytracedLighting"
persistent.xr.profile.ar.render.nearPlane = 0.15
xr.openxr.components."omni.kit.xr.openxr.ext.hand_tracking".enabled = true
xr.openxr.components."isaacsim.xr.openxr.hand_tracking".enabled = true
# Register extension folder from this repo in kit
[settings.app.exts]
......
......@@ -51,9 +51,6 @@ import torch
import omni.log
if "handtracking" in args_cli.teleop_device.lower():
from isaacsim.xr.openxr import OpenXRSpec
from isaaclab.devices import OpenXRDevice, Se3Gamepad, Se3Keyboard, Se3SpaceMouse
if args_cli.enable_pinocchio:
......@@ -197,7 +194,7 @@ def main():
# Create GR1T2 retargeter with desired configuration
gr1t2_retargeter = GR1T2Retargeter(
enable_visualization=True,
num_open_xr_hand_joints=2 * (int(OpenXRSpec.HandJointEXT.XR_HAND_JOINT_LITTLE_TIP_EXT) + 1),
num_open_xr_hand_joints=2 * 26, # OpenXR hand tracking spec has 26 joints
device=env.unwrapped.device,
hand_joint_names=env.scene["robot"].data.joint_names[-22:],
)
......
......@@ -79,9 +79,6 @@ if "handtracking" in args_cli.teleop_device.lower():
app_launcher = AppLauncher(args_cli)
simulation_app = app_launcher.app
if "handtracking" in args_cli.teleop_device.lower():
from isaacsim.xr.openxr import OpenXRSpec
# Omniverse logger
import omni.log
import omni.ui as ui
......@@ -298,7 +295,7 @@ def main():
# Create GR1T2 retargeter with desired configuration
gr1t2_retargeter = GR1T2Retargeter(
enable_visualization=True,
num_open_xr_hand_joints=2 * (int(OpenXRSpec.HandJointEXT.XR_HAND_JOINT_LITTLE_TIP_EXT) + 1),
num_open_xr_hand_joints=2 * 26, # OpenXR hand tracking spec has 26 joints
device=env.unwrapped.device,
hand_joint_names=env.scene["robot"].data.joint_names[-22:],
)
......
......@@ -391,6 +391,15 @@ Added
* Added :meth:`~isaaclab.envs.mdp.observations.body_projected_gravity_b`
0.37.2 (2025-05-06)
~~~~~~~~~~~~~~~~~~~~
Changed
^^^^^^^
* Migrated OpenXR device to use the new OpenXR handtracking API from omni.kit.xr.core.
0.37.1 (2025-05-05)
~~~~~~~~~~~~~~~~~~~~
......
......@@ -20,9 +20,7 @@ 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 omni.kit.xr.core import XRCore, XRPoseValidityFlags
from isaacsim.core.prims import SingleXFormPrim
......@@ -77,7 +75,6 @@ class OpenXRDevice(DeviceBase):
If None or empty list, raw tracking data will be returned.
"""
super().__init__(retargeters)
self._openxr = OpenXR()
self._xr_cfg = xr_cfg or XrCfg()
self._additional_callbacks = dict()
self._vc_subscription = (
......@@ -87,9 +84,12 @@ class OpenXRDevice(DeviceBase):
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)
self._previous_headpose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32)
# Initialize dictionaries instead of arrays
default_pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32)
self._previous_joint_poses_left = {name: default_pose.copy() for name in HAND_JOINT_NAMES}
self._previous_joint_poses_right = {name: default_pose.copy() for name in HAND_JOINT_NAMES}
self._previous_headpose = default_pose.copy()
# 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)
......@@ -157,9 +157,10 @@ class OpenXRDevice(DeviceBase):
"""
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)
self._previous_headpose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32)
default_pose = np.array([0, 0, 0, 1, 0, 0, 0], dtype=np.float32)
self._previous_joint_poses_left = {name: default_pose.copy() for name in HAND_JOINT_NAMES}
self._previous_joint_poses_right = {name: default_pose.copy() for name in HAND_JOINT_NAMES}
self._previous_headpose = default_pose.copy()
def add_callback(self, key: str, func: Callable):
"""Add additional functions to bind to client messages.
......@@ -185,11 +186,11 @@ class OpenXRDevice(DeviceBase):
"""
return {
self.TrackingTarget.HAND_LEFT: self._calculate_joint_poses(
self._openxr.locate_hand_joints(OpenXRSpec.XrHandEXT.XR_HAND_LEFT_EXT),
XRCore.get_singleton().get_input_device("/user/hand/left"),
self._previous_joint_poses_left,
),
self.TrackingTarget.HAND_RIGHT: self._calculate_joint_poses(
self._openxr.locate_hand_joints(OpenXRSpec.XrHandEXT.XR_HAND_RIGHT_EXT),
XRCore.get_singleton().get_input_device("/user/hand/right"),
self._previous_joint_poses_right,
),
self.TrackingTarget.HEAD: self._calculate_headpose(),
......@@ -199,25 +200,54 @@ class OpenXRDevice(DeviceBase):
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])
def _calculate_joint_poses(
self, hand_device: Any, previous_joint_poses: dict[str, np.ndarray]
) -> dict[str, np.ndarray]:
"""Calculate and update joint poses for a hand device.
pos_mask = (location_flags & OpenXRSpec.XR_SPACE_LOCATION_POSITION_VALID_BIT) != 0
ori_mask = (location_flags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) != 0
This function retrieves the current joint poses from the OpenXR hand device and updates
the previous joint poses with the new data. If a joint's position or orientation is not
valid, it will use the previous values.
previous_joint_poses[pos_mask, 0:3] = positions[pos_mask]
previous_joint_poses[ori_mask, 3:7] = orientations[ori_mask]
Args:
hand_device: The OpenXR input device for a hand (/user/hand/left or /user/hand/right).
previous_joint_poses: Dictionary mapping joint names to their previous poses.
Each pose is a 7-element array: [x, y, z, qw, qx, qy, qz].
return self._joints_to_dict(previous_joint_poses)
Returns:
Updated dictionary of joint poses with the same structure as previous_joint_poses.
Each pose is represented as a 7-element numpy array: [x, y, z, qw, qx, qy, qz]
where the first 3 elements are position and the last 4 are quaternion orientation.
"""
if hand_device is None:
return previous_joint_poses
joint_poses = hand_device.get_all_virtual_world_poses()
# Update each joint that is present in the current data
for joint_name, joint_pose in joint_poses.items():
if joint_name in HAND_JOINT_NAMES:
# Extract translation and rotation
if joint_pose.validity_flags & XRPoseValidityFlags.POSITION_VALID:
position = joint_pose.pose_matrix.ExtractTranslation()
else:
position = previous_joint_poses[joint_name][:3]
if joint_pose.validity_flags & XRPoseValidityFlags.ORIENTATION_VALID:
quat = joint_pose.pose_matrix.ExtractRotationQuat()
quati = quat.GetImaginary()
quatw = quat.GetReal()
else:
quatw = previous_joint_poses[joint_name][3]
quati = previous_joint_poses[joint_name][4:]
# Directly update the dictionary with new data
previous_joint_poses[joint_name] = np.array(
[position[0], position[1], position[2], quatw, quati[0], quati[1], quati[2]], dtype=np.float32
)
# No need for conversion, just return the updated dictionary
return previous_joint_poses
def _calculate_headpose(self) -> np.ndarray:
"""Calculate the head pose from OpenXR.
......@@ -225,7 +255,7 @@ class OpenXRDevice(DeviceBase):
Returns:
numpy.ndarray: 7-element array containing head position (xyz) and orientation (wxyz)
"""
head_device = XRCore.get_singleton().get_input_device("displayDevice")
head_device = XRCore.get_singleton().get_input_device("/user/head")
if head_device:
hmd = head_device.get_virtual_world_pose("")
position = hmd.ExtractTranslation()
......@@ -246,17 +276,6 @@ class OpenXRDevice(DeviceBase):
return self._previous_headpose
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"]
......
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