Improves hand tracking using XR teleop (#180)

<!--
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/source/refs/contributing.html
-->

- Add configuration option to zero out rotation around the x/y axes.
- Introduce smoothing and thresholding for position and rotation deltas
to reduce jitter.
- Switch position reference to the wrist joint pose for more stable
relative-based positioning.
- Make rotation reference configurable, allowing the use of wrist-based
orientation instead of fingertip averaging.

<!-- 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)

<!--
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`
- [ ] I have made corresponding changes to the documentation
- [ ] My changes generate no new warnings
- [ ] 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
-->

---------
Signed-off-by: Kelly Guo <kellyguo123@hotmail.com>
Co-authored-by: Kelly Guo <kellyg@nvidia.com>
Co-authored-by: Kelly Guo <kellyguo123@hotmail.com>

CONTRIBUTORS.md

@@ -41,6 +41,7 @@ Guidelines for modifications:
 * Brayden Zhang
 * Cameron Upright
 * Calvin Yu
+* Cheng-Rong Lai
 * Chenyu Yang
 * CY (Chien-Ying) Chen
 * David Yang

source/extensions/omni.isaac.lab/docs/CHANGELOG.rst

@@ -163,7 +163,7 @@ Added
 * Added full buffer property to :class:`omni.isaac.lab.utils.buffers.circular_buffer.CircularBuffer`
 
 
-0.27.35 (2024-12-15)
+0.27.36 (2024-12-15)
 ~~~~~~~~~~~~~~~~~~~~
 
 Added
@@ -172,7 +172,7 @@ Added
 * Added action clip to all :class:`omni.isaac.lab.envs.mdp.actions`.
 
 
-0.27.34 (2024-12-14)
+0.27.35 (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.33 (2024-12-13)
+0.27.34 (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.32 (2024-12-11)
+0.27.33 (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.31 (2024-12-11)
+0.27.32 (2024-12-11)
 ~~~~~~~~~~~~~~~~~~~~
 
 Fixed
@@ -211,6 +211,20 @@ 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.31 (2024-12-09)
+~~~~~~~~~~~~~~~~~~~~
+
+Changed
+^^^^^^^
+
+* Introduced configuration options in :class:`Se3HandTracking` to:
+  - Zero out rotation around the x/y axes
+  - Apply smoothing and thresholding to position and rotation deltas for reduced jitter
+  - Use wrist-based rotation reference as an alternative to fingertip-based rotation
+
+* Switched the default position reference in :class:`Se3HandTracking` to the wrist joint pose, providing more stable relative-based positioning.
+
+
 0.27.30 (2024-12-09)
 ~~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.lab/omni/isaac/lab/devices/openxr/se3_handtracking.py

@@ -36,11 +36,11 @@ class Se3HandTracking(DeviceBase):
     * - "RESET": signals simulation reset
     """
 
-    GRIP_HYSTERESIS_METERS: Final[float] = 0.0254  # 1.0 inch
-    DELTA_POS_SCALE_FACTOR = 20
-    DELTA_ROT_SCALE_FACTOR = 5
+    GRIP_HYSTERESIS_METERS: Final[float] = 0.05  # 2.0 inch
+    DELTA_POS_SCALE_FACTOR = 10
+    DELTA_ROT_SCALE_FACTOR = 10
 
-    def __init__(self, hand, abs=True):
+    def __init__(self, hand, abs=True, zero_out_xy_rotation=False, use_wrist_rotation=False):
         self._openxr = OpenXR()
         self._previous_pos = np.zeros(3)
         self._previous_rot = np.zeros(3)
@@ -48,6 +48,8 @@ class Se3HandTracking(DeviceBase):
         self._previous_gripper_command = False
         self._hand = hand
         self._abs = abs
+        self._zero_out_xy_rotation = zero_out_xy_rotation
+        self._use_wrist_rotation = use_wrist_rotation
         self._additional_callbacks = dict()
         self._vc = teleop_command.TeleopCommand()
         self._vc_subscription = (
@@ -59,6 +61,11 @@ class Se3HandTracking(DeviceBase):
         )
         self._tracking = False
 
+        self._alpha_pos = 0.5
+        self._alpha_rot = 0.5
+        self._smoothed_delta_pos = np.zeros(3)
+        self._smoothed_delta_rot = np.zeros(3)
+
     def __del__(self):
         return
 
@@ -154,24 +161,43 @@ class Se3HandTracking(DeviceBase):
     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]
+        wrist = hand_joints[OpenXRSpec.HandJointEXT.XR_HAND_JOINT_WRIST_EXT]
+
+        # Define thresholds
+        POSITION_THRESHOLD = 0.001
+        ROTATION_THRESHOLD = 0.01
 
         # 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:
+        if not wrist.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)
+            wrist_pos = wrist.pose.position
+            target_pos = np.array([wrist_pos.x, wrist_pos.y, wrist_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
 
+        self._smoothed_delta_pos = (self._alpha_pos * delta_pos) + ((1 - self._alpha_pos) * self._smoothed_delta_pos)
+
+        # Apply position threshold
+        if np.linalg.norm(self._smoothed_delta_pos) < POSITION_THRESHOLD:
+            self._smoothed_delta_pos = np.zeros(3)
+
         # rotation
+        if self._use_wrist_rotation:
+            # Rotation using wrist orientation only
+            if not wrist.locationFlags & OpenXRSpec.XR_SPACE_LOCATION_ORIENTATION_VALID_BIT:
+                delta_rot = np.zeros(3)
+            else:
+                wrist_quat = wrist.pose.orientation
+                wrist_quat = np.array([wrist_quat.x, wrist_quat.y, wrist_quat.z, wrist_quat.w], dtype=np.float32)
+
+                wrist_rotation = Rotation.from_quat(wrist_quat)
+                target_rot = wrist_rotation.as_rotvec()
+
+                delta_rot = target_rot - self._previous_rot
+                self._previous_rot = target_rot
+        else:
             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:
@@ -198,6 +224,19 @@ class Se3HandTracking(DeviceBase):
                 delta_rot = target_rot - self._previous_rot
                 self._previous_rot = target_rot
 
+        if self._zero_out_xy_rotation:
+            delta_rot[0] = 0.0  # Zero out rotation around x-axis
+            delta_rot[1] = 0.0  # Zero out rotation around y-axis
+
+        self._smoothed_delta_rot = (self._alpha_rot * delta_rot) + ((1 - self._alpha_rot) * self._smoothed_delta_rot)
+
+        # Apply rotation threshold
+        if np.linalg.norm(self._smoothed_delta_rot) < ROTATION_THRESHOLD:
+            self._smoothed_delta_rot = np.zeros(3)
+
+        delta_pos = self._smoothed_delta_pos
+        delta_rot = self._smoothed_delta_rot
+
         # if not tracking still update prev positions but return no delta pose
         if not self._tracking:
             return np.zeros(6)

source/standalone/tools/record_demos.py

@@ -23,6 +23,7 @@ optional arguments:
 """Launch Isaac Sim Simulator first."""
 
 import argparse
+import os
 
 from omni.isaac.lab.app import AppLauncher
 
@@ -39,6 +40,9 @@ AppLauncher.add_app_launcher_args(parser)
 # parse the arguments
 args_cli = parser.parse_args()
 
+if args_cli.teleop_device.lower() == "handtracking":
+    vars(args_cli)["experience"] = f'{os.environ["ISAACLAB_PATH"]}/source/apps/isaaclab.python.xr.openxr.kit'
+
 # launch the simulator
 app_launcher = AppLauncher(args_cli)
 simulation_app = app_launcher.app
@@ -47,11 +51,10 @@ simulation_app = app_launcher.app
 
 import contextlib
 import gymnasium as gym
-import os
 import time
 import torch
 
-from omni.isaac.lab.devices import Se3Keyboard, Se3SpaceMouse
+from omni.isaac.lab.devices import Se3HandTracking, Se3Keyboard, Se3SpaceMouse
 from omni.isaac.lab.envs.mdp.recorders.recorders_cfg import ActionStateRecorderManagerCfg
 
 import omni.isaac.lab_tasks  # noqa: F401
@@ -131,14 +134,6 @@ def main():
     # create environment
     env = gym.make(args_cli.task, cfg=env_cfg)
 
-    # 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)
-    else:
-        raise ValueError(f"Invalid device interface '{args_cli.teleop_device}'. Supported: 'keyboard', 'spacemouse'.")
-
     # add teleoperation key for reset current recording instance
     should_reset_recording_instance = False
 
@@ -146,6 +141,21 @@ def main():
         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
+
+        teleop_interface = Se3HandTracking(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'."
+        )
+
     teleop_interface.add_callback("R", reset_recording_instance)
     print(teleop_interface)