From 04e300f37e6d7d155e363f8040e0f18ec8b1b216 Mon Sep 17 00:00:00 2001 From: Huadong Zhang Date: Thu, 17 Sep 2026 14:28:30 -0700 Subject: [PATCH] RealHand plugin: dexterous hands, robot arms, and data gloves Signed-off-by: Huadong Zhang --- CMakeLists.txt | 7 + docs/source/_data/devices.yaml | 19 + docs/source/device/realhand_ffg_glove.rst | 119 +++++ docs/source/index.rst | 1 + docs/source/references/retargeting/index.rst | 9 + .../references/retargeting/realhand.rst | 128 +++++ .../teleop/python/assets/realhand/.gitignore | 6 + .../teleop/python/assets/realhand/README.md | 30 ++ .../python/p7_realhand_bimanual_example.py | 383 +++++++++++++ .../python/realhand_ffg_glove_calibration.py | 205 +++++++ .../python/scripts/fetch_realhand_assets.py | 300 +++++++++++ src/plugins/CMakeLists.txt | 3 + src/plugins/realhand_ffg_glove/CMakeLists.txt | 25 + src/plugins/realhand_ffg_glove/README.md | 49 ++ src/plugins/realhand_ffg_glove/main.cpp | 101 ++++ src/plugins/realhand_ffg_glove/plugin.yaml | 14 + .../realhand_ffg_glove_plugin.cpp | 272 ++++++++++ .../realhand_ffg_glove_plugin.hpp | 71 +++ .../realhand_ffg_glove_protocol.cpp | 176 ++++++ .../realhand_ffg_glove_protocol.hpp | 76 +++ .../realhand_ffg_glove_serial.cpp | 224 ++++++++ .../realhand_ffg_glove_serial.hpp | 47 ++ .../isaaccapture/retargeters/__init__.py | 56 ++ .../retargeters/realhand/__init__.py | 54 ++ .../isaaccapture/retargeters/realhand/arm.py | 230 ++++++++ .../isaaccapture/retargeters/realhand/hand.py | 503 ++++++++++++++++++ .../retargeters/realhand/profiles.py | 320 +++++++++++ .../realhand/realhand_ffg_glove.py | 425 +++++++++++++++ tests/cpp/plugins/CMakeLists.txt | 4 + .../plugins/realhand_ffg_glove/CMakeLists.txt | 11 + .../test_realhand_ffg_glove_protocol.cpp | 125 +++++ .../core/retargeting_engine/conftest.py | 34 ++ .../core/retargeting_engine/pyproject.toml | 2 + .../test_realhand_asset_fetcher.py | 103 ++++ .../test_realhand_example.py | 61 +++ .../test_realhand_retargeters.py | 424 +++++++++++++++ 36 files changed, 4617 insertions(+) create mode 100644 docs/source/device/realhand_ffg_glove.rst create mode 100644 docs/source/references/retargeting/realhand.rst create mode 100644 examples/teleop/python/assets/realhand/.gitignore create mode 100644 examples/teleop/python/assets/realhand/README.md create mode 100644 examples/teleop/python/p7_realhand_bimanual_example.py create mode 100644 examples/teleop/python/realhand_ffg_glove_calibration.py create mode 100755 examples/teleop/python/scripts/fetch_realhand_assets.py create mode 100644 src/plugins/realhand_ffg_glove/CMakeLists.txt create mode 100644 src/plugins/realhand_ffg_glove/README.md create mode 100644 src/plugins/realhand_ffg_glove/main.cpp create mode 100644 src/plugins/realhand_ffg_glove/plugin.yaml create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.cpp create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.hpp create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.cpp create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.hpp create mode 100644 src/python/isaaccapture/retargeters/realhand/__init__.py create mode 100644 src/python/isaaccapture/retargeters/realhand/arm.py create mode 100644 src/python/isaaccapture/retargeters/realhand/hand.py create mode 100644 src/python/isaaccapture/retargeters/realhand/profiles.py create mode 100644 src/python/isaaccapture/retargeters/realhand/realhand_ffg_glove.py create mode 100644 tests/cpp/plugins/realhand_ffg_glove/CMakeLists.txt create mode 100644 tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_protocol.cpp create mode 100644 tests/python/core/retargeting_engine/test_realhand_asset_fetcher.py create mode 100644 tests/python/core/retargeting_engine/test_realhand_example.py create mode 100644 tests/python/core/retargeting_engine/test_realhand_retargeters.py diff --git a/CMakeLists.txt b/CMakeLists.txt index da2a078192..39384e59c1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,6 +63,13 @@ option(BUILD_PLUGIN_OAK_CAMERA "Build OAK camera plugin (requires vcpkg for Dept option(BUILD_PLUGIN_NOITOM_MOCAP "Build Noitom mocap plugin (downloads MocapApi SDK)" OFF) option(BUILD_PLUGIN_OGLO "Build OGLO tactile glove plugin (BLE, Linux only; fetches SimpleBLE + nlohmann/json)" OFF) option(BUILD_PLUGIN_WUJI_GLOVE "Build Wuji glove plugin (requires the wuji_sdk C SDK)" OFF) +if(UNIX) + set(_realhand_ffg_glove_default ON) +else() + set(_realhand_ffg_glove_default OFF) +endif() +option(BUILD_PLUGIN_REALHAND_FFG_GLOVE "Build RealHand FFG Glove USB serial plugin (POSIX only)" ${_realhand_ffg_glove_default}) +unset(_realhand_ffg_glove_default) option(BUILD_EXAMPLES "Build examples" ON) option(BUILD_EXAMPLE_TELEOP_ROS2 "Build only the teleop_ros2 ROS 2 reference integration (e.g. for Docker)" OFF) option(BUILD_TESTING "Build unit tests" ON) diff --git a/docs/source/_data/devices.yaml b/docs/source/_data/devices.yaml index d10595e2c5..bbd313fac6 100644 --- a/docs/source/_data/devices.yaml +++ b/docs/source/_data/devices.yaml @@ -223,6 +223,25 @@ devices: - label: haptikos.tech url: https://haptikos.tech/ + - id: realhand-ffg-glove + name: RealHand FFG Glove + url: https://github.com/RealHand-Robotics/FFG_realhand_pure_python + group: peripheral + modes: Finger joint sensing over USB + since: main + details: + setup: + - label: RealHand FFG Glove docs + doc: /device/realhand_ffg_glove + - label: RealHand FFG Glove Plugin + url: https://github.com/NVIDIA/IsaacCapture/tree/main/src/plugins/realhand_ffg_glove + requirements: + - Linux workstation + - USB serial access through the dialout group + acquire: + - label: RealHand FFG Glove SDK source + url: https://github.com/RealHand-Robotics/FFG_realhand_pure_python + - id: oglo-tactile-glove name: OGLO Tactile Glove url: https://www.opengraphlabs.com/ diff --git a/docs/source/device/realhand_ffg_glove.rst b/docs/source/device/realhand_ffg_glove.rst new file mode 100644 index 0000000000..65853e81ce --- /dev/null +++ b/docs/source/device/realhand_ffg_glove.rst @@ -0,0 +1,119 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +RealHand FFG Glove +================== + +The Linux RealHand FFG Glove plugin reads one or two gloves over USB serial and publishes each +glove as a standard Isaac Teleop ``JointStateOutput`` collection. It discovers serial ports and +baud rates, queries the firmware-reported side, and reconnects after a disconnect. Either side can +operate alone; a missing glove does not control the opposite hand. + +.. contents:: On this page + :local: + :depth: 2 + +Data flow +--------- + +.. code-block:: text + + RealHand FFG Glove USB ─► realhand_ffg_glove plugin ─► JointStateSource ─► RealHandFFGGloveRetargeter + side discovery 21 sensors L6 / O6 / L20 joints + +The serial protocol follows the Apache-2.0 +`RealHand FFG Glove pure-Python SDK +`_. The plugin implements the protocol +directly in C++, so the Python SDK is not redistributed or required at runtime. + +Build +----- + +The plugin is enabled by default on POSIX systems. Configure and build Isaac Teleop normally: + +.. code-block:: console + + $ cmake -S . -B build + $ cmake --build build --target realhand_ffg_glove_plugin + $ cmake --install build --component realhand_ffg_glove + +Set ``-DBUILD_PLUGIN_REALHAND_FFG_GLOVE=OFF`` to exclude it from a POSIX build. The serial backend +is not built by default on Windows. + +USB permissions +--------------- + +The current user must be able to open the glove's serial device. On Ubuntu, add the user to the +``dialout`` group, then log out and back in: + +.. code-block:: console + + $ sudo usermod -aG dialout "$USER" + +Do not run Isaac Teleop as root. Check permissions with ``ls -l /dev/ttyUSB*`` and membership with +``id -nG``. + +Run +--- + +Start the CloudXR runtime, source its OpenXR environment, and launch the installed plugin: + +.. code-block:: console + + $ python -m isaaccapture.cloudxr.service start + $ source ~/.cloudxr/run/cloudxr.env + $ ./install/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin + +By default the plugin scans ``/dev/ttyUSB*``, ``/dev/ttyACM*``, ``/dev/ttyXRUSB*``, and +``/dev/ttyOBC*`` using the supported baud rates. Restrict discovery when diagnosing a connection: + +.. code-block:: console + + $ ./install/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin \ + --ports=/dev/ttyUSB0,/dev/ttyUSB1 \ + --baudrates=2000000,460800 + +The left and right collections are ``realhand_ffg_glove_left`` and +``realhand_ffg_glove_right``. Each publishes ``sensor_0`` through ``sensor_20``. + +Retargeting and calibration +--------------------------- + +RealHand FFG Glove samples are glove measurements, not robot joint commands. Use one +``RealHandFFGGloveRetargeter`` per connected side and select ``l6``, ``o6``, or ``l20``. Calibration +must be captured while wearing the glove in its final fit. Hold each pose steadily and avoid +pressing the fingertips together hard enough to deform the glove. + +For L6 and O6, capture these poses: + +* **Open palm:** wrist neutral, palm flat, all five digits naturally straight and separated. +* **Full fist:** all four fingers fully flexed; thumb folded naturally across the index and middle + fingers rather than forced into the palm. +* **Thumb-only curl:** four fingers stay open; move the thumb through opposition and flexion toward + the palm without closing another finger. +* **Thumb to index:** touch the two fingertip pads lightly while the other fingers stay relaxed. +* **Thumb to middle:** touch the two fingertip pads lightly while the other fingers stay relaxed. + +For L20, additionally capture thumb-to-ring and thumb-to-pinky poses. These samples describe the +thumb's calibrated response surface. At runtime, thumb outputs use only thumb sensors, and every +other digit uses only its own sensors; there is no contact snapping or cross-finger pose trigger. +Samples that continue beyond a calibrated endpoint saturate at that endpoint instead of reopening +the corresponding mechanical-hand joints. + +Capture a calibration from the standard DeviceIO stream: + +.. code-block:: console + + $ python examples/teleop/python/realhand_ffg_glove_calibration.py \ + --hand-model l20 --side both \ + --plugin-path install/plugins \ + --output /path/to/calibration_l20.yml + +Use ``--side left`` or ``--side right`` when one glove is connected. A later run for the other +side updates the same YAML and preserves the existing side. Calibration files contain +user- and glove-specific measurements. Keep them outside the source tree and pass the resulting +path explicitly with ``--realhand-ffg-glove-calibration`` when running an FFG example. + +.. seealso:: + + :doc:`/references/retargeting/realhand` describes the P7 and RealHand output pipelines. diff --git a/docs/source/index.rst b/docs/source/index.rst index 09a8fcc2d0..0ec421ddfb 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -66,6 +66,7 @@ Table of Contents device/body_tracking device/haptic_feedback device/manus + device/realhand_ffg_glove device/oak device/oglo device/wuji_glove diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 31a9daa0a2..e592539de8 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -21,6 +21,14 @@ Source Nodes Available Retargeters --------------------- +.. dropdown:: P7 and RealHand L6 / O6 / L20 + + ``P7ControllerPoseRetargeter`` and ``P7HandPoseRetargeter`` map an absolute OpenXR pose into + the calibrated P7 workspace. ``RealHandHandTrackingRetargeter``, + ``ControllerTriggerRealHandRetargeter``, and ``RealHandFFGGloveRetargeter`` produce joint commands + for the L6, O6, and L20 hands. See :doc:`realhand` for the full input-mode matrix and pipeline + example. + .. dropdown:: Se3AbsRetargeter / Se3RelRetargeter Maps hand or controller tracking to end-effector pose. ``Se3AbsRetargeter`` outputs a 7D @@ -373,3 +381,4 @@ See the :doc:`Contributing Guide <../../getting_started/contributing>` for detai so101 joint_space wuji + realhand diff --git a/docs/source/references/retargeting/realhand.rst b/docs/source/references/retargeting/realhand.rst new file mode 100644 index 0000000000..c033afb52b --- /dev/null +++ b/docs/source/references/retargeting/realhand.rst @@ -0,0 +1,128 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +Retargeters: P7 and RealHand +============================ + +The RealHand integration separates device acquisition, robot-independent retargeting, and +simulation control along Isaac Teleop's existing boundaries. + +Components +---------- + +.. list-table:: + :header-rows: 1 + :widths: 30 30 40 + + * - Component + - Input + - Output + * - ``P7ControllerPoseRetargeter`` + - One OpenXR controller grip pose + - Absolute seven-value P7 end-effector pose + * - ``P7HandPoseRetargeter`` + - One OpenXR hand wrist pose + - Absolute seven-value P7 end-effector pose + * - ``ControllerTriggerRealHandRetargeter`` + - One controller trigger / squeeze value + - L6, O6, or L20 hand joints + * - ``RealHandHandTrackingRetargeter`` + - One 26-joint OpenXR hand + - L6, O6, or L20 hand joints + * - ``RealHandFFGGloveRetargeter`` + - One RealHand FFG Glove 21-sensor ``JointStateSource`` + - L6, O6, or L20 hand joints + +P7 arm control +-------------- + +The P7 nodes perform only the calibrated absolute workspace transform, pose limiting, and temporal +filtering. They deliberately do not solve robot joint angles. In Isaac Lab, feed their two +``ee_pose`` outputs to ``PinkInverseKinematicsActionCfg`` so collision settings, joint limits, and +the simulated robot state remain owned by the environment. + +RealHand control +---------------- + +``get_realhand_profile()`` provides the active joint limits and the combined bimanual action order +for each hand model. Controller mode maps the analog trigger linearly from open to the model's +closed posture. Hand-tracking mode derives each digit from OpenXR joint geometry. RealHand FFG +Glove mode maps a calibrated glove independently per side and per digit. + +Run the combined example +------------------------ + +Install the lightweight retargeting dependency and build the package first: + +.. code-block:: console + + $ pip install 'isaaccapture[retargeters-lite]' + $ cmake --build build --target python_package + +The example supports all three hand models and all three input modes: + +.. code-block:: console + + $ python examples/teleop/python/p7_realhand_bimanual_example.py \ + --mode handtracking --hand-model l20 + + $ python examples/teleop/python/p7_realhand_bimanual_example.py \ + --mode controller --hand-model l6 + + $ python examples/teleop/python/p7_realhand_bimanual_example.py \ + --mode ffg --hand-model o6 \ + --plugin-path install/plugins \ + --realhand-ffg-glove-calibration /path/to/calibration_o6.yml + +The action layout is always ``left_ee_pose + right_ee_pose + profile.action_joint_names``. Its +width is 36 for L6, 26 for O6, and 46 for L20. Use ``--dry-run`` to validate graph construction +without starting OpenXR. + +Robot assets +------------ + +The P7 + L6, O6, and L20 URDF assemblies and meshes are published separately to keep binary robot +assets out of the Isaac Teleop source repository. Download all three assemblies from the repository +root with: + +.. code-block:: console + + $ python3 examples/teleop/python/scripts/fetch_realhand_assets.py + +To download only one hand model or to use a cache outside the source tree: + +.. code-block:: console + + $ python3 examples/teleop/python/scripts/fetch_realhand_assets.py \ + --hand-model l20 \ + --output-dir ~/.cache/isaaccapture/realhand + +The script uses the immutable commit behind RealHand Teleop v0.2.0 and verifies the downloaded +files before use. Personal glove calibrations, source CAD files, controller mounts, scenes, and +demonstrations are not downloaded. Resolve the selected assembly in an Isaac Lab environment with +``get_realhand_profile("l20").resolve_urdf(asset_root)``. + +Complete Isaac Lab reference package +------------------------------------ + +A complete reference package containing the P7 robot environments, RealHand L6, O6, and L20 +assets, and runnable controller, hand-tracking, and RealHand FFG Glove examples is available from +the `RealHand Teleop v0.2.0 release +`_. + +This package is optional and is not required to build or use the Isaac Teleop plugins and +retargeters in this repository. + +Coordinate calibration +---------------------- + +``P7WorkspacePoseConfig`` exposes controller or wrist center, robot workspace center, per-axis +scale and limits, fixed orientation and local position offsets, and filtering limits. The example +contains the measured neutral P7 configurations used by the reference setup. A different robot +mount, OpenXR anchor, or controller-on-glove bracket should override those values in the consuming +Isaac Lab environment rather than changing the device plugin. + +.. seealso:: + + :doc:`/device/realhand_ffg_glove` covers RealHand FFG Glove discovery, permissions, and + calibration poses. diff --git a/examples/teleop/python/assets/realhand/.gitignore b/examples/teleop/python/assets/realhand/.gitignore new file mode 100644 index 0000000000..4eb456db83 --- /dev/null +++ b/examples/teleop/python/assets/realhand/.gitignore @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +* +!.gitignore +!README.md diff --git a/examples/teleop/python/assets/realhand/README.md b/examples/teleop/python/assets/realhand/README.md new file mode 100644 index 0000000000..e9658f10dd --- /dev/null +++ b/examples/teleop/python/assets/realhand/README.md @@ -0,0 +1,30 @@ + + +# P7 and RealHand Robot Assets + +This directory receives the P7 + L6, O6, and L20 URDF assemblies and their meshes. The binary +assets are published separately in the public +[RealHand Teleop repository](https://huggingface.co/realhandinc/realhand-teleop) and are not stored +in Isaac Teleop. + +From the Isaac Teleop repository root, download all three assemblies with: + +```bash +python3 examples/teleop/python/scripts/fetch_realhand_assets.py +``` + +Download one assembly or choose another destination with: + +```bash +python3 examples/teleop/python/scripts/fetch_realhand_assets.py \ + --hand-model l20 \ + --output-dir ~/.cache/isaaccapture/realhand +``` + +The fetcher pins the immutable commit behind RealHand Teleop v0.2.0, validates Hugging Face LFS +objects and the main URDF, and checks that every mesh referenced by the URDF exists. It downloads +robot assets and license notices only; personal glove calibrations, STEP files, controller mounts, +scenes, and demonstrations are excluded. diff --git a/examples/teleop/python/p7_realhand_bimanual_example.py b/examples/teleop/python/p7_realhand_bimanual_example.py new file mode 100644 index 0000000000..09bc486557 --- /dev/null +++ b/examples/teleop/python/p7_realhand_bimanual_example.py @@ -0,0 +1,383 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""P7 bimanual teleoperation with RealHand L6, O6, or L20 hands. + +The pipeline emits two absolute end-effector poses followed by the selected hand model's +joint commands. An Isaac Lab environment consumes the first 14 values with Pink IK and sends +the remaining values to the hand actuators. +""" + +from __future__ import annotations + +import argparse +import time +from pathlib import Path + +import numpy as np +from scipy.spatial.transform import Rotation + +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.retargeters import ( + ControllerTriggerRealHandRetargeter, + ControllerTriggerRealHandRetargeterConfig, + RealHandFFGGloveRetargeter, + RealHandFFGGloveRetargeterConfig, + P7ControllerPoseRetargeter, + P7HandPoseRetargeter, + P7WorkspacePoseConfig, + RealHandHandTrackingRetargeter, + RealHandHandTrackingRetargeterConfig, + TensorReorderer, +) +from isaaccapture.retargeters.realhand import ( + REALHAND_FFG_GLOVE_SENSOR_NAMES, + get_realhand_profile, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + ControllersSource, + HandsSource, + JointStateSource, +) +from isaaccapture.retargeting_engine.interface import OutputCombiner +from isaaccapture.teleop_session_manager import ( + PluginConfig, + TeleopSession, + TeleopSessionConfig, +) + +_LEFT_EE = [ + "l_pos_x", + "l_pos_y", + "l_pos_z", + "l_quat_x", + "l_quat_y", + "l_quat_z", + "l_quat_w", +] +_RIGHT_EE = [ + "r_pos_x", + "r_pos_y", + "r_pos_z", + "r_quat_x", + "r_quat_y", + "r_quat_z", + "r_quat_w", +] + +_HOME_POSES = { + "l6": { + "left": ( + (-0.41977179, -0.00000670, 0.72252082), + (-0.70637390, 0.70783417, 0.00075116, 0.00247797), + ), + "right": ( + (0.41976724, -0.00000958, 0.71870925), + (-0.70629016, 0.70773326, -0.01312369, 0.00977843), + ), + }, + "o6": { + "left": ( + (-0.41990048, 0.00000132, 0.72069988), + (-0.70637390, 0.70783417, 0.00075116, 0.00247797), + ), + "right": ( + (0.41989615, -0.00000103, 0.72070892), + (-0.70635324, 0.70785451, -0.00077004, -0.00254952), + ), + }, + "l20": { + "left": ( + (-0.41980995, 0.00017019, 0.68370038), + (-0.70637390, 0.70783417, 0.00075116, 0.00247797), + ), + "right": ( + (0.41980285, -0.00017463, 0.68370944), + (-0.70635324, 0.70785451, -0.00077004, -0.00254952), + ), + }, +} + +_INPUT_CENTERS = { + "left": (-0.25, 0.0, 1.10), + "right": (0.25, 0.0, 1.10), +} +_WORKSPACE_CENTERS = { + "left": (-0.25, 0.0, 0.82), + "right": (0.25, 0.0, 0.82), +} +_REALHAND_FFG_GLOVE_HAND_OFFSETS = { + "l6": (0.115, 0.0, -0.075), + "o6": (0.110, 0.0, -0.065), + "l20": (0.115, 0.0, -0.090), +} + + +def _home_rpy(model: str, side: str) -> tuple[float, float, float]: + quaternion = _HOME_POSES[model][side][1] + return tuple(Rotation.from_quat(quaternion).as_euler("XYZ", degrees=True)) + + +def _realhand_ffg_glove_rpy(model: str, side: str) -> tuple[float, float, float]: + base = Rotation.from_euler("XYZ", _home_rpy(model, side), degrees=True) + correction = Rotation.from_euler("XYZ", (-90.0, 0.0, 90.0), degrees=True) + fingertip_correction = ( + 90.0 if model in ("l6", "o6") else 180.0 if side == "right" else 0.0 + ) + rotation = ( + base * correction * Rotation.from_euler("X", fingertip_correction, degrees=True) + ) + return tuple(rotation.as_euler("XYZ", degrees=True)) + + +def _pose_config(model: str, side: str, mode: str) -> P7WorkspacePoseConfig: + home_position, home_rotation = _HOME_POSES[model][side] + uses_realhand_ffg_glove = mode == "ffg" + return P7WorkspacePoseConfig( + input_device=( + ControllersSource.LEFT if side == "left" else ControllersSource.RIGHT + ) + if mode != "handtracking" + else (HandsSource.LEFT if side == "left" else HandsSource.RIGHT), + fallback_position=home_position, + fallback_rotation=home_rotation, + input_center=( + _WORKSPACE_CENTERS[side] + if uses_realhand_ffg_glove + else _INPUT_CENTERS[side] + ), + workspace_center=_WORKSPACE_CENTERS[side], + position_scale=(1.0, 1.0, 1.0), + max_delta=(0.65, 0.65, 0.75), + rotation_offset_rpy_deg=( + _realhand_ffg_glove_rpy(model, side) + if uses_realhand_ffg_glove + else _home_rpy(model, side) + ), + position_offset_local=( + _REALHAND_FFG_GLOVE_HAND_OFFSETS[model] + if uses_realhand_ffg_glove + else (0.0, 0.0, 0.0) + ), + max_position_step_m=0.25, + position_smoothing_alpha=1.0, + orientation_smoothing_alpha=0.85, + max_orientation_step_deg=90.0, + ) + + +def build_pipeline(mode: str, model: str, calibration: Path | None): + """Build the official DeviceIO -> retargeters -> flat P7 action graph.""" + profile = get_realhand_profile(model) + left_joint_names = profile.joint_names("left") + right_joint_names = profile.joint_names("right") + + if mode == "handtracking": + source = HandsSource(name="hands") + left_arm = P7HandPoseRetargeter( + _pose_config(model, "left", mode), name="p7_left_arm" + ) + right_arm = P7HandPoseRetargeter( + _pose_config(model, "right", mode), name="p7_right_arm" + ) + left_hand = RealHandHandTrackingRetargeter( + RealHandHandTrackingRetargeterConfig( + input_device=HandsSource.LEFT, + joint_names=left_joint_names, + side="left", + hand_model=model, + ), + name=f"{model}_left_hand", + ) + right_hand = RealHandHandTrackingRetargeter( + RealHandHandTrackingRetargeterConfig( + input_device=HandsSource.RIGHT, + joint_names=right_joint_names, + side="right", + hand_model=model, + ), + name=f"{model}_right_hand", + ) + left_input = source.output(HandsSource.LEFT) + right_input = source.output(HandsSource.RIGHT) + left_arm_input_name = HandsSource.LEFT + right_arm_input_name = HandsSource.RIGHT + left_hand_input_name = HandsSource.LEFT + right_hand_input_name = HandsSource.RIGHT + hand_output = "hand_joints" + else: + source = ControllersSource(name="controllers") + left_arm = P7ControllerPoseRetargeter( + _pose_config(model, "left", mode), name="p7_left_arm" + ) + right_arm = P7ControllerPoseRetargeter( + _pose_config(model, "right", mode), name="p7_right_arm" + ) + left_input = source.output(ControllersSource.LEFT) + right_input = source.output(ControllersSource.RIGHT) + left_arm_input_name = ControllersSource.LEFT + right_arm_input_name = ControllersSource.RIGHT + if mode == "controller": + left_hand = ControllerTriggerRealHandRetargeter( + ControllerTriggerRealHandRetargeterConfig( + input_device=ControllersSource.LEFT, + joint_names=left_joint_names, + side="left", + hand_model=model, + ), + name=f"{model}_left_hand", + ) + right_hand = ControllerTriggerRealHandRetargeter( + ControllerTriggerRealHandRetargeterConfig( + input_device=ControllersSource.RIGHT, + joint_names=right_joint_names, + side="right", + hand_model=model, + ), + name=f"{model}_right_hand", + ) + left_hand_input_name = ControllersSource.LEFT + right_hand_input_name = ControllersSource.RIGHT + else: + if calibration is None: + raise ValueError("RealHand FFG Glove mode requires a calibration YAML") + left_glove = JointStateSource( + name="realhand_ffg_glove_left", + collection_id="realhand_ffg_glove_left", + joint_names=list(REALHAND_FFG_GLOVE_SENSOR_NAMES), + ) + right_glove = JointStateSource( + name="realhand_ffg_glove_right", + collection_id="realhand_ffg_glove_right", + joint_names=list(REALHAND_FFG_GLOVE_SENSOR_NAMES), + ) + left_hand = RealHandFFGGloveRetargeter( + RealHandFFGGloveRetargeterConfig( + input_device=JointStateSource.JOINTS, + joint_names=left_joint_names, + side="left", + hand_model=model, + calibration_path=str(calibration), + ), + name=f"{model}_left_hand", + ) + right_hand = RealHandFFGGloveRetargeter( + RealHandFFGGloveRetargeterConfig( + input_device=JointStateSource.JOINTS, + joint_names=right_joint_names, + side="right", + hand_model=model, + calibration_path=str(calibration), + ), + name=f"{model}_right_hand", + ) + left_input = left_glove.output(JointStateSource.JOINTS) + right_input = right_glove.output(JointStateSource.JOINTS) + left_hand_input_name = JointStateSource.JOINTS + right_hand_input_name = JointStateSource.JOINTS + hand_output = "hand_joints" + + connected_left_arm = left_arm.connect( + {left_arm_input_name: source.output(source.LEFT)} + ) + connected_right_arm = right_arm.connect( + {right_arm_input_name: source.output(source.RIGHT)} + ) + connected_left_hand = left_hand.connect({left_hand_input_name: left_input}) + connected_right_hand = right_hand.connect({right_hand_input_name: right_input}) + + action_order = _LEFT_EE + _RIGHT_EE + list(profile.action_joint_names) + reorderer = TensorReorderer( + input_config={ + "left_ee_pose": _LEFT_EE, + "right_ee_pose": _RIGHT_EE, + "left_hand_joints": left_joint_names, + "right_hand_joints": right_joint_names, + }, + output_order=action_order, + name="p7_realhand_action", + input_types={ + "left_ee_pose": "array", + "right_ee_pose": "array", + "left_hand_joints": "scalar", + "right_hand_joints": "scalar", + }, + ) + connected_action = reorderer.connect( + { + "left_ee_pose": connected_left_arm.output("ee_pose"), + "right_ee_pose": connected_right_arm.output("ee_pose"), + "left_hand_joints": connected_left_hand.output(hand_output), + "right_hand_joints": connected_right_hand.output(hand_output), + } + ) + return OutputCombiner({"action": connected_action.output("output")}), action_order + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + parser.add_argument( + "--mode", choices=("controller", "handtracking", "ffg"), default="handtracking" + ) + parser.add_argument("--hand-model", choices=("l6", "o6", "l20"), default="l6") + parser.add_argument( + "--realhand-ffg-glove-calibration", + type=Path, + help="User-specific calibration YAML generated by realhand_ffg_glove_calibration.py", + ) + parser.add_argument( + "--plugin-path", + type=Path, + help="Directory containing realhand_ffg_glove/plugin.yaml", + ) + parser.add_argument("--duration", type=float, default=360.0) + parser.add_argument( + "--dry-run", + action="store_true", + help="Build and validate the graph without OpenXR", + ) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args() + + calibration = args.realhand_ffg_glove_calibration + if args.mode == "ffg" and calibration is None: + parser.error("--realhand-ffg-glove-calibration is required when --mode ffg") + pipeline, action_order = build_pipeline(args.mode, args.hand_model, calibration) + print( + f"P7 + {args.hand_model.upper()} mode={args.mode}; action_dim={len(action_order)}" + ) + print("action order:", action_order) + if args.dry_run: + return + + plugins = [] + if args.mode == "ffg" and args.plugin_path is not None: + plugins.append( + PluginConfig( + plugin_name="realhand_ffg_glove_plugin", + plugin_root_id="realhand_ffg_glove", + search_paths=[args.plugin_path], + required=True, + ) + ) + config = TeleopSessionConfig( + app_name="P7RealHandBimanual", + trackers=[], + pipeline=pipeline, + plugins=plugins, + ) + with CloudXRLauncher.launch_context(args), TeleopSession(config) as session: + deadline = time.time() + args.duration + while time.time() < deadline: + result = session.step() + if session.frame_count % 60 == 0: + action = np.asarray(result["action"][0]) + print( + f"frame={session.frame_count} left_ee={np.round(action[:7], 3)} " + f"right_ee={np.round(action[7:14], 3)}" + ) + time.sleep(0.016) + + +if __name__ == "__main__": + main() diff --git a/examples/teleop/python/realhand_ffg_glove_calibration.py b/examples/teleop/python/realhand_ffg_glove_calibration.py new file mode 100644 index 0000000000..ee20ba4f4f --- /dev/null +++ b/examples/teleop/python/realhand_ffg_glove_calibration.py @@ -0,0 +1,205 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Capture user-specific RealHand FFG Glove calibration through DeviceIO.""" + +from __future__ import annotations + +import argparse +import time +from datetime import datetime, timezone +from pathlib import Path + +import numpy as np +import yaml + +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.retargeters.realhand import REALHAND_FFG_GLOVE_SENSOR_NAMES +from isaaccapture.retargeting_engine.deviceio_source_nodes import JointStateSource +from isaaccapture.retargeting_engine.interface import OutputCombiner +from isaaccapture.teleop_session_manager import ( + PluginConfig, + TeleopSession, + TeleopSessionConfig, +) + +_COLLECTIONS = {"left": "realhand_ffg_glove_left", "right": "realhand_ffg_glove_right"} +_POSES = { + "l6": ( + ("open palm", "jointangleoriginal"), + ("full fist", "jointanglefist"), + ("thumb-only curl", "jointanglethumb_curl"), + ("thumb touching index fingertip", "jointangleopose_index"), + ("thumb touching middle fingertip", "jointangleopose_middle"), + ), + "o6": ( + ("open palm", "jointangleoriginal"), + ("full fist", "jointanglefist"), + ("thumb-only curl", "jointanglethumb_curl"), + ("thumb touching index fingertip", "jointangleopose_index"), + ("thumb touching middle fingertip", "jointangleopose_middle"), + ), + "l20": ( + ("open palm", "jointangleoriginal"), + ("full fist", "jointanglefist"), + ("thumb-only curl", "jointanglethumb_curl"), + ("thumb touching index fingertip", "jointangleopose_index"), + ("thumb touching middle fingertip", "jointangleopose_middle"), + ("thumb touching ring fingertip", "jointangleopose_ring"), + ("thumb touching pinky fingertip", "jointangleopose_pinky"), + ), +} + + +def _build_raw_pipeline(sides: tuple[str, ...]): + outputs = {} + for side in sides: + source = JointStateSource( + name=f"realhand_ffg_glove_{side}", + collection_id=_COLLECTIONS[side], + joint_names=list(REALHAND_FFG_GLOVE_SENSOR_NAMES), + ) + outputs[side] = source.output(JointStateSource.JOINTS) + return OutputCombiner(outputs) + + +def _sample_group(result, side: str) -> np.ndarray | None: + group = result[side] + if group.is_none: + return None + values = np.asarray( + [float(group[index]) for index in range(len(REALHAND_FFG_GLOVE_SENSOR_NAMES))] + ) + if not np.all(np.isfinite(values)): + return None + return values + + +def _wait_for_sides( + session: TeleopSession, sides: tuple[str, ...], timeout: float +) -> None: + pending = set(sides) + deadline = time.monotonic() + timeout + while pending and time.monotonic() < deadline: + result = session.step() + for side in tuple(pending): + if _sample_group(result, side) is not None: + pending.remove(side) + time.sleep(0.02) + if pending: + names = ", ".join(sorted(pending)) + raise RuntimeError(f"No RealHand FFG Glove data received for: {names}") + + +def _capture_pose( + session: TeleopSession, + sides: tuple[str, ...], + sample_count: int, + timeout: float, +) -> dict[str, list[float]]: + samples: dict[str, list[np.ndarray]] = {side: [] for side in sides} + deadline = time.monotonic() + timeout + while time.monotonic() < deadline and any( + len(samples[side]) < sample_count for side in sides + ): + result = session.step() + for side in sides: + if len(samples[side]) >= sample_count: + continue + values = _sample_group(result, side) + if values is not None: + samples[side].append(values) + time.sleep(0.01) + missing = { + side: sample_count - len(values) + for side, values in samples.items() + if len(values) < sample_count + } + if missing: + raise RuntimeError(f"Calibration capture timed out; missing samples: {missing}") + return { + side: np.median(np.asarray(values), axis=0).tolist() + for side, values in samples.items() + } + + +def _load_output(path: Path, model: str) -> dict[str, object]: + if not path.exists(): + return {} + with path.open("r", encoding="utf-8") as stream: + existing = yaml.safe_load(stream) or {} + if not isinstance(existing, dict): + raise ValueError(f"Existing calibration must be a YAML mapping: {path}") + existing_model = existing.get("model") + if existing_model not in (None, model): + raise ValueError( + f"Existing calibration is for {existing_model!r}, not requested model {model!r}" + ) + return existing + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + parser.add_argument("--hand-model", choices=tuple(_POSES), required=True) + parser.add_argument("--side", choices=("left", "right", "both"), default="both") + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--samples", type=int, default=120) + parser.add_argument("--timeout", type=float, default=20.0) + parser.add_argument( + "--plugin-path", + type=Path, + help="Directory containing realhand_ffg_glove/plugin.yaml", + ) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args() + if args.samples < 10: + parser.error("--samples must be at least 10") + + sides = ("left", "right") if args.side == "both" else (args.side,) + pipeline = _build_raw_pipeline(sides) + plugins = [] + if args.plugin_path is not None: + plugins.append( + PluginConfig( + plugin_name="realhand_ffg_glove_plugin", + plugin_root_id="realhand_ffg_glove", + search_paths=[args.plugin_path], + required=True, + ) + ) + config = TeleopSessionConfig( + app_name="FFGGloveCalibration", + trackers=[], + pipeline=pipeline, + plugins=plugins, + ) + calibration = _load_output(args.output, args.hand_model) + calibration.update( + { + "timestamp": datetime.now(timezone.utc).isoformat(), + "model": args.hand_model, + "format": "realhand-ffg-glove-raw-calibration-v2", + } + ) + + with CloudXRLauncher.launch_context(args), TeleopSession(config) as session: + print(f"Waiting for RealHand FFG Glove stream: {', '.join(sides)}") + _wait_for_sides(session, sides, args.timeout) + for label, key in _POSES[args.hand_model]: + input(f"Hold {label}. Press Enter when ready, then remain still...") + captured = _capture_pose(session, sides, args.samples, args.timeout) + for side, values in captured.items(): + suffix = "l" if side == "left" else "r" + calibration[f"{key}_{suffix}"] = values + if key == "jointangleopose_index": + calibration[f"jointangleopose_{suffix}"] = values + print(f"Captured {label}") + + args.output.parent.mkdir(parents=True, exist_ok=True) + with args.output.open("w", encoding="utf-8") as stream: + yaml.safe_dump(calibration, stream, sort_keys=False) + print(f"Calibration saved to {args.output}") + + +if __name__ == "__main__": + main() diff --git a/examples/teleop/python/scripts/fetch_realhand_assets.py b/examples/teleop/python/scripts/fetch_realhand_assets.py new file mode 100755 index 0000000000..b2212241fb --- /dev/null +++ b/examples/teleop/python/scripts/fetch_realhand_assets.py @@ -0,0 +1,300 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Fetch pinned P7 and RealHand robot assets from the official Hugging Face release.""" + +from __future__ import annotations + +import argparse +import concurrent.futures +import hashlib +import json +import os +import sys +import urllib.error +import urllib.parse +import urllib.request +import xml.etree.ElementTree as ET +from dataclasses import dataclass +from pathlib import Path, PurePosixPath +from typing import Any + + +REPO_ID = "realhandinc/realhand-teleop" +RELEASE_NAME = "v0.2.0" +REVISION = "e20d1f7dfe27d84784270012f291ae16f4762b0c" +REMOTE_ASSET_ROOT = "examples/isaac_lab/p7_realhand_bimanual/assets/robots" +MODEL_ASSET_DIRS = {"l6": "p7_l6", "o6": "p7_o6", "l20": "p7_l20"} +MODEL_URDFS = { + "l6": "P7_l6_bimanual.urdf", + "o6": "P7_o6_bimanual.urdf", + "l20": "P7_L20_bimanual.urdf", +} +URDF_SHA256 = { + "l6": "5a504b7fb055a2401c0e28e8be1e2f9d642b28a00c4d9e38021072ed4c4a72d2", + "o6": "1d5750bc6ee0b0d8a90ac7ea2a747d42d2a5d73cadd0244493e4815ddd370b32", + "l20": "6194e9dafd98067d5c08f7200ec6c61aefa31578d453da2f858fbe10ad7dc99a", +} +LICENSE_FILES = { + "LICENSE", + "NOTICE", + "LICENSES/Apache-2.0.txt", + "LICENSES/BSD-3-Clause.txt", +} + + +@dataclass(frozen=True) +class RemoteFile: + path: str + size: int + sha256: str | None + + +def _example_root() -> Path: + return Path(__file__).resolve().parents[1] + + +def _default_output_dir() -> Path: + return _example_root() / "assets" / "realhand" + + +def _request_json(url: str) -> dict[str, Any]: + request = urllib.request.Request( + url, headers={"User-Agent": "IsaacCapture-RealHand-assets/1.0"} + ) + try: + with urllib.request.urlopen(request, timeout=30) as response: + return json.load(response) + except (urllib.error.URLError, json.JSONDecodeError) as exc: + raise RuntimeError( + f"Failed to read Hugging Face metadata from {url}: {exc}" + ) from exc + + +def _repository_metadata() -> dict[str, Any]: + quoted_repo = urllib.parse.quote(REPO_ID, safe="/") + quoted_revision = urllib.parse.quote(REVISION, safe="") + metadata = _request_json( + f"https://huggingface.co/api/models/{quoted_repo}/revision/{quoted_revision}?blobs=true" + ) + actual_revision = metadata.get("sha") + if actual_revision != REVISION: + raise RuntimeError( + f"Hugging Face resolved revision {REVISION} to {actual_revision!r}; refusing an unpinned download" + ) + return metadata + + +def _normalize_remote_path(path: str) -> str: + normalized = PurePosixPath(path) + if normalized.is_absolute() or ".." in normalized.parts: + raise ValueError(f"Unsafe repository path: {path!r}") + return normalized.as_posix() + + +def _selected_files(metadata: dict[str, Any], models: set[str]) -> list[RemoteFile]: + prefixes = {f"{REMOTE_ASSET_ROOT}/{MODEL_ASSET_DIRS[model]}/" for model in models} + selected: list[RemoteFile] = [] + for sibling in metadata.get("siblings", []): + path = _normalize_remote_path(str(sibling.get("rfilename", ""))) + if path not in LICENSE_FILES and not any( + path.startswith(prefix) for prefix in prefixes + ): + continue + size = sibling.get("size") + if not isinstance(size, int) or size < 0: + raise RuntimeError(f"Missing file size in Hugging Face metadata for {path}") + lfs = sibling.get("lfs") + sha256 = lfs.get("sha256") if isinstance(lfs, dict) else None + if sha256 is not None and (not isinstance(sha256, str) or len(sha256) != 64): + raise RuntimeError( + f"Invalid LFS SHA-256 in Hugging Face metadata for {path}" + ) + selected.append(RemoteFile(path=path, size=size, sha256=sha256)) + + required_prefixes = { + prefix + for prefix in prefixes + if not any(item.path.startswith(prefix) for item in selected) + } + missing_licenses = LICENSE_FILES - {item.path for item in selected} + if required_prefixes or missing_licenses: + missing = sorted(required_prefixes | missing_licenses) + raise RuntimeError( + f"Pinned RealHand release is missing required paths: {', '.join(missing)}" + ) + return sorted(selected, key=lambda item: item.path) + + +def _local_path(remote: RemoteFile, output_dir: Path) -> Path: + prefix = f"{REMOTE_ASSET_ROOT}/" + relative = ( + remote.path[len(prefix) :] if remote.path.startswith(prefix) else remote.path + ) + destination = (output_dir / Path(relative)).resolve() + output_root = output_dir.resolve() + if destination != output_root and output_root not in destination.parents: + raise ValueError(f"Unsafe output path for {remote.path!r}") + return destination + + +def _sha256(path: Path) -> str: + digest = hashlib.sha256() + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _is_complete(path: Path, remote: RemoteFile) -> bool: + if not path.is_file() or path.stat().st_size != remote.size: + return False + return remote.sha256 is None or _sha256(path) == remote.sha256 + + +def _download(remote: RemoteFile, output_dir: Path, force: bool) -> tuple[Path, bool]: + destination = _local_path(remote, output_dir) + if not force and _is_complete(destination, remote): + return destination, False + + quoted_repo = urllib.parse.quote(REPO_ID, safe="/") + quoted_revision = urllib.parse.quote(REVISION, safe="") + quoted_path = urllib.parse.quote(remote.path, safe="/") + url = ( + f"https://huggingface.co/{quoted_repo}/resolve/{quoted_revision}/{quoted_path}" + ) + request = urllib.request.Request( + url, headers={"User-Agent": "IsaacCapture-RealHand-assets/1.0"} + ) + destination.parent.mkdir(parents=True, exist_ok=True) + temporary = destination.with_name(f".{destination.name}.part-{os.getpid()}") + digest = hashlib.sha256() + size = 0 + try: + with ( + urllib.request.urlopen(request, timeout=120) as response, + temporary.open("wb") as stream, + ): + while chunk := response.read(1024 * 1024): + stream.write(chunk) + digest.update(chunk) + size += len(chunk) + if size != remote.size: + raise RuntimeError( + f"Downloaded {remote.path} has size {size}, expected {remote.size}" + ) + if remote.sha256 is not None and digest.hexdigest() != remote.sha256: + raise RuntimeError( + f"Downloaded {remote.path} has SHA-256 {digest.hexdigest()}, expected {remote.sha256}" + ) + temporary.replace(destination) + except (OSError, urllib.error.URLError) as exc: + raise RuntimeError(f"Failed to download {remote.path}: {exc}") from exc + finally: + temporary.unlink(missing_ok=True) + return destination, True + + +def _validate_model(output_dir: Path, model: str) -> Path: + asset_dir = output_dir / MODEL_ASSET_DIRS[model] + urdf_path = asset_dir / MODEL_URDFS[model] + if not urdf_path.is_file(): + raise FileNotFoundError( + f"Downloaded {model.upper()} URDF is missing: {urdf_path}" + ) + actual_sha256 = _sha256(urdf_path) + if actual_sha256 != URDF_SHA256[model]: + raise RuntimeError( + f"{urdf_path} has SHA-256 {actual_sha256}, expected {URDF_SHA256[model]}" + ) + + root = ET.parse(urdf_path).getroot() + if root.tag != "robot": + raise ValueError(f"Expected a URDF robot root in {urdf_path}") + missing_meshes: list[str] = [] + for mesh in root.findall(".//mesh"): + filename = mesh.attrib.get("filename") + if not filename: + raise ValueError(f"URDF mesh without a filename in {urdf_path}") + mesh_path = PurePosixPath(filename) + if mesh_path.is_absolute() or ".." in mesh_path.parts or "://" in filename: + raise ValueError(f"Unsupported mesh path {filename!r} in {urdf_path}") + if not (asset_dir / Path(mesh_path.as_posix())).is_file(): + missing_meshes.append(filename) + if missing_meshes: + preview = ", ".join(sorted(set(missing_meshes))[:8]) + raise FileNotFoundError(f"{urdf_path} references missing meshes: {preview}") + return urdf_path + + +def _parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--hand-model", + choices=("l6", "o6", "l20", "all"), + default="all", + help="Robot-hand assembly to download (default: all).", + ) + parser.add_argument( + "--output-dir", + type=Path, + default=_default_output_dir(), + help="Destination containing p7_l6, p7_o6, and p7_l20 directories.", + ) + parser.add_argument( + "--force", + action="store_true", + help="Download files that already pass validation.", + ) + parser.add_argument( + "--dry-run", + action="store_true", + help="List the pinned files without downloading them.", + ) + parser.add_argument( + "--workers", type=int, default=6, help="Parallel downloads (default: 6)." + ) + return parser.parse_args() + + +def main() -> int: + args = _parse_args() + models = set(MODEL_ASSET_DIRS) if args.hand_model == "all" else {args.hand_model} + metadata = _repository_metadata() + files = _selected_files(metadata, models) + total_bytes = sum(item.size for item in files) + print( + f"RealHand assets {RELEASE_NAME} ({REVISION}) from {REPO_ID}: " + f"{len(files)} files, {total_bytes / (1024 * 1024):.1f} MiB" + ) + if args.dry_run: + for item in files: + print(item.path) + return 0 + if args.workers < 1: + raise ValueError("--workers must be at least 1") + + downloaded = 0 + with concurrent.futures.ThreadPoolExecutor(max_workers=args.workers) as executor: + futures = [ + executor.submit(_download, item, args.output_dir, args.force) + for item in files + ] + for index, future in enumerate( + concurrent.futures.as_completed(futures), start=1 + ): + path, changed = future.result() + downloaded += int(changed) + print( + f"[{index}/{len(files)}] {'Downloaded' if changed else 'Verified'} {path}" + ) + + for model in sorted(models): + print(f"Validated {model.upper()}: {_validate_model(args.output_dir, model)}") + print(f"Ready: downloaded {downloaded}, reused {len(files) - downloaded}") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 127717f682..5c53599c96 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -36,3 +36,6 @@ endif() if(BUILD_PLUGIN_WUJI_GLOVE) add_subdirectory(wuji_glove) endif() +if(BUILD_PLUGIN_REALHAND_FFG_GLOVE) + add_subdirectory(realhand_ffg_glove) +endif() diff --git a/src/plugins/realhand_ffg_glove/CMakeLists.txt b/src/plugins/realhand_ffg_glove/CMakeLists.txt new file mode 100644 index 0000000000..72d7aa5d04 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/CMakeLists.txt @@ -0,0 +1,25 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(realhand_ffg_glove_plugin + main.cpp + realhand_ffg_glove_plugin.cpp + realhand_ffg_glove_protocol.cpp + realhand_ffg_glove_serial.cpp +) + +target_link_libraries(realhand_ffg_glove_plugin PRIVATE + Teleop::plugin_utils + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema +) + +install(TARGETS realhand_ffg_glove_plugin + RUNTIME DESTINATION plugins/realhand_ffg_glove + COMPONENT realhand_ffg_glove +) +install(FILES plugin.yaml README.md + DESTINATION plugins/realhand_ffg_glove + COMPONENT realhand_ffg_glove +) diff --git a/src/plugins/realhand_ffg_glove/README.md b/src/plugins/realhand_ffg_glove/README.md new file mode 100644 index 0000000000..dbdd2d1f8a --- /dev/null +++ b/src/plugins/realhand_ffg_glove/README.md @@ -0,0 +1,49 @@ + + +# RealHand FFG Glove plugin + +Streams the 21 joint sensors from one or two RealHand FFG Gloves as the standard Isaac Teleop +`JointStateOutput` schema. The plugin scans supported USB serial devices, queries each glove for +its firmware-reported hand side, and publishes independent `realhand_ffg_glove_left` and +`realhand_ffg_glove_right` tensor collections. A single connected glove is supported; the absent side +remains disconnected and does not produce samples. + +The serial protocol implementation follows the Apache-2.0 +[RealHand FFG Glove pure-Python SDK](https://github.com/RealHand-Robotics/FFG_realhand_pure_python). +The external Python SDK is not redistributed or required at runtime. + +## Build and run + +The plugin is enabled by default on POSIX systems: + +```bash +cmake -S . -B build +cmake --build build --target realhand_ffg_glove_plugin +./build/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin +``` + +Use `-DBUILD_PLUGIN_REALHAND_FFG_GLOVE=OFF` to exclude it. The POSIX serial backend is not built by +default on Windows. + +By default it scans `/dev/ttyUSB*`, `/dev/ttyACM*`, `/dev/ttyXRUSB*`, and `/dev/ttyOBC*` at +2,000,000, 460,800, 1,000,000, and 921,600 baud. Optional overrides are available: + +```bash +realhand_ffg_glove_plugin --ports=/dev/ttyUSB0,/dev/ttyUSB1 --baudrates=2000000,460800 +``` + +On Linux, the user must be able to open the serial device. Common distributions grant this via +the `dialout` group: + +```bash +sudo usermod -aG dialout "$USER" +``` + +Log out and back in after changing group membership. Do not run the Isaac Teleop process as root. + +The consumer side uses `JointStateSource` with collection ID `realhand_ffg_glove_left` or +`realhand_ffg_glove_right` and joint names `sensor_0` through `sensor_20`. Mechanical-hand mapping and +calibration belong to the RealHand retargeters rather than this device plugin. diff --git a/src/plugins/realhand_ffg_glove/main.cpp b/src/plugins/realhand_ffg_glove/main.cpp new file mode 100644 index 0000000000..9b46f315d0 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/main.cpp @@ -0,0 +1,101 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "realhand_ffg_glove_plugin.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +std::atomic g_stop_requested{ false }; + +void signal_handler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) + { + g_stop_requested.store(true, std::memory_order_relaxed); + } +} + +std::vector split_csv(const std::string& value) +{ + std::vector result; + std::stringstream stream(value); + std::string item; + while (std::getline(stream, item, ',')) + { + if (!item.empty()) + { + result.push_back(item); + } + } + return result; +} + +std::vector parse_baudrates(const std::string& value) +{ + std::vector result; + for (const auto& item : split_csv(value)) + { + result.push_back(std::stoi(item)); + } + return result; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + std::vector ports; + std::vector baudrates; + std::string plugin_root_id = "realhand_ffg_glove"; + for (int i = 1; i < argc; ++i) + { + const std::string argument = argv[i]; + if (argument.starts_with("--ports=")) + { + ports = split_csv(argument.substr(8)); + } + else if (argument.starts_with("--baudrates=")) + { + baudrates = parse_baudrates(argument.substr(12)); + } + else if (argument.starts_with("--plugin-root-id=")) + { + plugin_root_id = argument.substr(17); + } + else + { + throw std::invalid_argument("unknown argument: " + argument); + } + } + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + plugins::realhand_ffg_glove::RealHandFFGGlovePlugin plugin(std::move(ports), std::move(baudrates), plugin_root_id); + std::cout << "RealHand FFG Glove plugin running. Press Ctrl+C to stop." << std::endl; + + constexpr auto frame_period = std::chrono::nanoseconds(1'000'000'000 / 90); + while (!g_stop_requested.load(std::memory_order_relaxed)) + { + const auto frame_start = std::chrono::steady_clock::now(); + plugin.update(); + std::this_thread::sleep_until(frame_start + frame_period); + } + return 0; +} +catch (const std::exception& error) +{ + std::cerr << argv[0] << ": " << error.what() << std::endl; + return 1; +} diff --git a/src/plugins/realhand_ffg_glove/plugin.yaml b/src/plugins/realhand_ffg_glove/plugin.yaml new file mode 100644 index 0000000000..3c79ee0449 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/plugin.yaml @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: realhand_ffg_glove_plugin +description: "RealHand FFG Glove joint sensors" +command: "./realhand_ffg_glove_plugin" +version: "1.0.0" +devices: + - path: "/glove/realhand_ffg_glove_left" + type: "joint_state" + description: "Left RealHand FFG Glove (21 joint sensors over USB serial)" + - path: "/glove/realhand_ffg_glove_right" + type: "joint_state" + description: "Right RealHand FFG Glove (21 joint sensors over USB serial)" diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp new file mode 100644 index 0000000000..d9794ecee1 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp @@ -0,0 +1,272 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "realhand_ffg_glove_plugin.hpp" + +#include "realhand_ffg_glove_serial.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins::realhand_ffg_glove +{ + +namespace +{ + +constexpr std::size_t kMaxFlatbufferSize = 4096; +constexpr std::size_t kForceChannelCount = 5; +constexpr auto kQueryPeriod = std::chrono::milliseconds(16); +constexpr auto kStaleTimeout = std::chrono::seconds(1); +constexpr auto kDiscoveryPeriod = std::chrono::seconds(2); +constexpr std::array kCollectionIds = { "realhand_ffg_glove_left", "realhand_ffg_glove_right" }; +constexpr std::array kDevicePaths = { "/glove/realhand_ffg_glove_left", + "/glove/realhand_ffg_glove_right" }; + +std::size_t side_index(HandSide side) +{ + return side == HandSide::Left ? 0 : 1; +} + +const char* side_name(std::size_t side) +{ + return side == 0 ? "left" : "right"; +} + +std::vector required_extensions() +{ + auto extensions = core::SchemaPusher::get_required_extensions(); + for (const auto& extension : plugin_utils::PluginDeviceStatusPublisher::get_required_extensions()) + { + if (std::find(extensions.begin(), extensions.end(), extension) == extensions.end()) + { + extensions.push_back(extension); + } + } + return extensions; +} + +} // namespace + +RealHandFFGGlovePlugin::RealHandFFGGlovePlugin(std::vector configured_ports, + std::vector baudrates, + std::string plugin_root_id) + : configured_ports_(std::move(configured_ports)), + baudrates_(std::move(baudrates)), + session_(std::make_shared("RealHandFFGGlovePlugin", required_extensions())) +{ + if (baudrates_.empty()) + { + baudrates_.assign(kDefaultBaudrates.begin(), kDefaultBaudrates.end()); + } + for (std::size_t side = 0; side < pushers_.size(); ++side) + { + pushers_[side] = std::make_unique( + session_->get_handles(), + core::SchemaPusherConfig{ .collection_id = kCollectionIds[side], + .max_flatbuffer_size = kMaxFlatbufferSize, + .tensor_identifier = "joint_state", + .localized_name = std::string("RealHand FFG Glove ") + side_name(side), + .app_name = "RealHandFFGGlovePlugin" }); + } + status_publisher_ = + std::make_unique(session_->get_handles(), plugin_root_id); + last_discovery_ = std::chrono::steady_clock::time_point::min(); +} + +RealHandFFGGlovePlugin::~RealHandFFGGlovePlugin() = default; + +void RealHandFFGGlovePlugin::discover() +{ + const auto now = std::chrono::steady_clock::now(); + if (now - last_discovery_ < kDiscoveryPeriod) + { + return; + } + last_discovery_ = now; + + std::vector ports = configured_ports_.empty() ? discover_serial_ports() : configured_ports_; + std::set occupied; + for (const auto& endpoint : endpoints_) + { + if (endpoint) + { + occupied.insert(endpoint->serial->port()); + } + } + + for (const auto& port : ports) + { + if (occupied.contains(port)) + { + continue; + } + for (int baudrate : baudrates_) + { + try + { + auto serial = std::make_unique(port, baudrate); + auto version = probe_glove(*serial); + if (!version) + { + continue; + } + const std::size_t side = side_index(version->side); + if (endpoints_[side]) + { + std::cerr << "RealHandFFGGlovePlugin: ignoring duplicate " << side_name(side) << " glove on " + << port << std::endl; + break; + } + auto endpoint = std::make_unique(); + endpoint->serial = std::move(serial); + endpoint->side = version->side; + endpoint->version = version->version; + endpoint->last_receive = now; + endpoint->last_query = std::chrono::steady_clock::time_point::min(); + endpoints_[side] = std::move(endpoint); + errors_[side].clear(); + occupied.insert(port); + std::cout << "RealHandFFGGlovePlugin: connected " << side_name(side) << " glove " << version->version + << " on " << port << " at " << baudrate << " baud" << std::endl; + break; + } + catch (const std::exception& error) + { + for (std::size_t side = 0; side < errors_.size(); ++side) + { + if (!endpoints_[side]) + { + errors_[side] = error.what(); + } + } + } + } + } +} + +void RealHandFFGGlovePlugin::drop_endpoint(std::size_t side, const std::string& error) +{ + std::cerr << "RealHandFFGGlovePlugin: disconnected " << side_name(side) << " glove: " << error << std::endl; + endpoints_[side].reset(); + errors_[side] = error; + last_discovery_ = std::chrono::steady_clock::time_point::min(); +} + +void RealHandFFGGlovePlugin::poll_endpoint(std::size_t side) +{ + auto& endpoint = endpoints_[side]; + if (!endpoint) + { + return; + } + try + { + const auto now = std::chrono::steady_clock::now(); + if (now - endpoint->last_query >= kQueryPeriod) + { + endpoint->serial->send(Command::Position); + endpoint->last_query = now; + } + for (const auto& frame : endpoint->serial->read_available(0)) + { + endpoint->last_receive = now; + if (auto positions = decode_positions(frame)) + { + endpoint->positions = *positions; + endpoint->has_positions = true; + if (frame.command == Command::A6Position) + { + // A6 firmware expects an A7 response before publishing the next sample. + endpoint->serial->send(Command::A7Force, std::vector(kForceChannelCount * sizeof(float), 0)); + } + } + } + if (now - endpoint->last_receive > kStaleTimeout) + { + drop_endpoint(side, "no serial response for one second"); + return; + } + if (endpoint->has_positions) + { + push_state(side); + } + } + catch (const std::exception& error) + { + drop_endpoint(side, error.what()); + } +} + +void RealHandFFGGlovePlugin::push_state(std::size_t side) +{ + core::JointStateOutputT output; + output.device_id = std::string(kCollectionIds[side]); + output.has_velocity = false; + output.has_effort = false; + output.ee_pose_valid = false; + output.joints.reserve(kNumSensors); + for (std::size_t sensor = 0; sensor < kNumSensors; ++sensor) + { + auto joint = std::make_shared(); + joint->name = "sensor_" + std::to_string(sensor); + joint->position = endpoints_[side]->positions[sensor]; + joint->valid = true; + output.joints.push_back(std::move(joint)); + } + + const int64_t sample_time = core::os_monotonic_now_ns(); + flatbuffers::FlatBufferBuilder builder(kMaxFlatbufferSize); + builder.Finish(core::JointStateOutput::Pack(builder, &output)); + pushers_[side]->push_buffer(builder.GetBufferPointer(), builder.GetSize(), sample_time, sample_time); +} + +void RealHandFFGGlovePlugin::publish_status() +{ + std::vector entries; + entries.reserve(2); + for (std::size_t side = 0; side < endpoints_.size(); ++side) + { + plugin_utils::PluginDeviceStatusEntry entry{ .path = kDevicePaths[side] }; + if (!endpoints_[side]) + { + entry.state = core::PluginDeviceState_DISCONNECTED; + entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + entry.error = errors_[side]; + } + else if (!endpoints_[side]->has_positions) + { + entry.state = core::PluginDeviceState_DEGRADED; + entry.reason = core::PluginDeviceReason_NO_CURRENT_DATA; + entry.error = "glove connected but no position frame has arrived"; + } + else + { + entry.state = core::PluginDeviceState_CONNECTED; + entry.reason = core::PluginDeviceReason_NONE; + } + entries.push_back(std::move(entry)); + } + status_publisher_->publish_if_changed(entries, core::os_monotonic_now_ns()); +} + +void RealHandFFGGlovePlugin::update() +{ + discover(); + poll_endpoint(0); + poll_endpoint(1); + publish_status(); +} + +} // namespace plugins::realhand_ffg_glove diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp new file mode 100644 index 0000000000..07bd52b4e2 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp @@ -0,0 +1,71 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "realhand_ffg_glove_protocol.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace core +{ +class OpenXRSession; +} + +namespace plugin_utils +{ +class PluginDeviceStatusPublisher; +} + +namespace plugins::realhand_ffg_glove +{ + +class SerialGlove; + +class RealHandFFGGlovePlugin +{ +public: + explicit RealHandFFGGlovePlugin(std::vector configured_ports = {}, + std::vector baudrates = {}, + std::string plugin_root_id = "realhand_ffg_glove"); + ~RealHandFFGGlovePlugin(); + + void update(); + +private: + struct Endpoint + { + std::unique_ptr serial; + HandSide side = HandSide::Left; + std::string version; + std::array positions{}; + bool has_positions = false; + std::chrono::steady_clock::time_point last_receive{}; + std::chrono::steady_clock::time_point last_query{}; + }; + + void discover(); + void poll_endpoint(std::size_t side_index); + void drop_endpoint(std::size_t side_index, const std::string& error); + void push_state(std::size_t side_index); + void publish_status(); + + std::vector configured_ports_; + std::vector baudrates_; + std::array, 2> endpoints_; + std::array errors_; + std::chrono::steady_clock::time_point last_discovery_{}; + + std::shared_ptr session_; + std::array, 2> pushers_; + std::unique_ptr status_publisher_; +}; + +} // namespace plugins::realhand_ffg_glove diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.cpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.cpp new file mode 100644 index 0000000000..ce8499bf16 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.cpp @@ -0,0 +1,176 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "realhand_ffg_glove_protocol.hpp" + +#include +#include +#include +#include +#include + +namespace plugins::realhand_ffg_glove +{ + +namespace +{ + +constexpr float kDegreesToRadians = 0.01745329251994329577f; + +template +T read_little_endian(const uint8_t* bytes) +{ + std::array native{}; + if constexpr (std::endian::native == std::endian::little) + { + std::memcpy(native.data(), bytes, sizeof(T)); + } + else + { + for (std::size_t i = 0; i < sizeof(T); ++i) + { + native[i] = bytes[sizeof(T) - 1 - i]; + } + } + T value{}; + std::memcpy(&value, native.data(), sizeof(T)); + return value; +} + +} // namespace + +void FrameParser::reset() +{ + state_ = State::Header; + command_ = 0; + payload_size_ = 0; + checksum_ = 0; + payload_.clear(); +} + +std::optional FrameParser::process(uint8_t byte) +{ + switch (state_) + { + case State::Header: + if (byte == kFrameHeader) + { + checksum_ = byte; + state_ = State::Command; + } + return std::nullopt; + case State::Command: + command_ = byte; + checksum_ = static_cast(checksum_ + byte); + state_ = State::Length; + return std::nullopt; + case State::Length: + payload_size_ = byte; + checksum_ = static_cast(checksum_ + byte); + payload_.clear(); + payload_.reserve(payload_size_); + state_ = payload_size_ == 0 ? State::Checksum : State::Payload; + return std::nullopt; + case State::Payload: + payload_.push_back(byte); + checksum_ = static_cast(checksum_ + byte); + if (payload_.size() == payload_size_) + { + state_ = State::Checksum; + } + return std::nullopt; + case State::Checksum: + if (byte == checksum_) + { + Frame frame{ static_cast(command_), payload_ }; + reset(); + return frame; + } + reset(); + if (byte == kFrameHeader) + { + checksum_ = byte; + state_ = State::Command; + } + return std::nullopt; + } + reset(); + return std::nullopt; +} + +std::vector pack_frame(Command command, const std::vector& payload) +{ + if (payload.size() > std::numeric_limits::max()) + { + return {}; + } + std::vector frame; + frame.reserve(payload.size() + 4); + frame.push_back(kFrameHeader); + frame.push_back(static_cast(command)); + frame.push_back(static_cast(payload.size())); + frame.insert(frame.end(), payload.begin(), payload.end()); + + uint8_t checksum = 0; + for (uint8_t byte : frame) + { + checksum = static_cast(checksum + byte); + } + frame.push_back(checksum); + return frame; +} + +std::optional decode_version(const Frame& frame) +{ + if (frame.command != Command::Version || frame.payload.size() < 5 || frame.payload[4] > 1) + { + return std::nullopt; + } + const uint32_t packed = read_little_endian(frame.payload.data()); + char digits[16]{}; + std::snprintf(digits, sizeof(digits), "%05u", packed); + const std::string text(digits); + VersionInfo result; + result.version = std::to_string(text[0] - '0') + "." + std::to_string(std::stoi(text.substr(1, 2))) + "." + + std::to_string(std::stoi(text.substr(3, 2))); + result.side = frame.payload[4] == 0 ? HandSide::Left : HandSide::Right; + return result; +} + +std::optional> decode_positions(const Frame& frame) +{ + std::array result{}; + if (frame.command == Command::Position || frame.command == Command::A3Position) + { + if (frame.payload.size() != kNumSensors * sizeof(float)) + { + return std::nullopt; + } + for (std::size_t i = 0; i < kNumSensors; ++i) + { + const float degrees = read_little_endian(frame.payload.data() + i * sizeof(float)); + if (!std::isfinite(degrees)) + { + return std::nullopt; + } + result[i] = degrees * kDegreesToRadians; + } + return result; + } + if (frame.command == Command::A6Position) + { + if (frame.payload.size() != kNumSensors * sizeof(int16_t)) + { + return std::nullopt; + } + for (std::size_t i = 0; i < kNumSensors; ++i) + { + const int16_t hundredth_degrees = read_little_endian(frame.payload.data() + i * sizeof(int16_t)); + result[i] = static_cast(hundredth_degrees) * 0.01f * kDegreesToRadians; + } + return result; + } + return std::nullopt; +} + +} // namespace plugins::realhand_ffg_glove diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.hpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.hpp new file mode 100644 index 0000000000..963616cf23 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.hpp @@ -0,0 +1,76 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace plugins::realhand_ffg_glove +{ + +inline constexpr uint8_t kFrameHeader = 0x5D; +inline constexpr std::size_t kNumSensors = 21; +inline constexpr std::array kDefaultBaudrates = { 2'000'000, 460'800, 1'000'000, 921'600 }; + +enum class Command : uint8_t +{ + Version = 0x01, + SetFlag = 0x02, + Position = 0x03, + ForceFeedback = 0x04, + A3Position = 0xA3, + A6Position = 0xA6, + A7Force = 0xA7, +}; + +enum class HandSide +{ + Left, + Right, +}; + +struct Frame +{ + Command command; + std::vector payload; +}; + +struct VersionInfo +{ + std::string version; + HandSide side; +}; + +class FrameParser +{ +public: + std::optional process(uint8_t byte); + void reset(); + +private: + enum class State + { + Header, + Command, + Length, + Payload, + Checksum, + }; + + State state_ = State::Header; + uint8_t command_ = 0; + uint8_t payload_size_ = 0; + uint8_t checksum_ = 0; + std::vector payload_; +}; + +std::vector pack_frame(Command command, const std::vector& payload = {}); +std::optional decode_version(const Frame& frame); +std::optional> decode_positions(const Frame& frame); + +} // namespace plugins::realhand_ffg_glove diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.cpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.cpp new file mode 100644 index 0000000000..f8f19361d3 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.cpp @@ -0,0 +1,224 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "realhand_ffg_glove_serial.hpp" + +#include +#include +#include +#include +#include + +#ifndef _WIN32 +# include + +# include +# include +# include +#endif + +namespace plugins::realhand_ffg_glove +{ + +#ifndef _WIN32 +namespace +{ + +speed_t baud_to_termios(int baudrate) +{ + switch (baudrate) + { +# ifdef B2000000 + case 2'000'000: + return B2000000; +# endif +# ifdef B1000000 + case 1'000'000: + return B1000000; +# endif +# ifdef B921600 + case 921'600: + return B921600; +# endif +# ifdef B460800 + case 460'800: + return B460800; +# endif + default: + throw std::runtime_error("RealHand FFG Glove: unsupported serial baud rate " + std::to_string(baudrate)); + } +} + +void append_glob(const char* pattern, std::vector& paths) +{ + glob_t matches{}; + if (::glob(pattern, GLOB_NOSORT, nullptr, &matches) == 0) + { + for (std::size_t i = 0; i < matches.gl_pathc; ++i) + { + paths.emplace_back(matches.gl_pathv[i]); + } + } + ::globfree(&matches); +} + +} // namespace +#endif + +SerialGlove::SerialGlove(const std::string& port, int baudrate) : port_(port), baudrate_(baudrate) +{ +#ifdef _WIN32 + throw std::runtime_error("RealHand FFG Glove serial backend currently supports POSIX systems only"); +#else + fd_ = ::open(port.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (fd_ < 0) + { + throw std::runtime_error("cannot open " + port + ": " + std::strerror(errno)); + } + termios tty{}; + if (::tcgetattr(fd_, &tty) != 0) + { + const std::string error = std::strerror(errno); + ::close(fd_); + fd_ = -1; + throw std::runtime_error("tcgetattr failed for " + port + ": " + error); + } + ::cfmakeraw(&tty); + const speed_t speed = baud_to_termios(baudrate); + ::cfsetispeed(&tty, speed); + ::cfsetospeed(&tty, speed); + tty.c_cflag |= CLOCAL | CREAD; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~PARENB; + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; +# ifdef CRTSCTS + tty.c_cflag &= ~CRTSCTS; +# endif + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 0; + if (::tcsetattr(fd_, TCSANOW, &tty) != 0) + { + const std::string error = std::strerror(errno); + ::close(fd_); + fd_ = -1; + throw std::runtime_error("tcsetattr failed for " + port + ": " + error); + } + ::tcflush(fd_, TCIOFLUSH); +#endif +} + +SerialGlove::~SerialGlove() +{ +#ifndef _WIN32 + if (fd_ >= 0) + { + ::close(fd_); + } +#endif +} + +void SerialGlove::send(Command command, const std::vector& payload) +{ +#ifndef _WIN32 + const auto frame = pack_frame(command, payload); + std::size_t sent = 0; + while (sent < frame.size()) + { + const ssize_t count = ::write(fd_, frame.data() + sent, frame.size() - sent); + if (count < 0) + { + if (errno == EINTR || errno == EAGAIN) + { + continue; + } + throw std::runtime_error("write failed for " + port_ + ": " + std::strerror(errno)); + } + sent += static_cast(count); + } +#else + (void)command; + (void)payload; +#endif +} + +std::vector SerialGlove::read_available(int timeout_ms) +{ + std::vector frames; +#ifndef _WIN32 + fd_set read_set; + FD_ZERO(&read_set); + FD_SET(fd_, &read_set); + timeval timeout{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 }; + const int ready = ::select(fd_ + 1, &read_set, nullptr, nullptr, &timeout); + if (ready < 0 && errno != EINTR) + { + throw std::runtime_error("select failed for " + port_ + ": " + std::strerror(errno)); + } + if (ready <= 0) + { + return frames; + } + + std::array bytes{}; + while (true) + { + const ssize_t count = ::read(fd_, bytes.data(), bytes.size()); + if (count == 0 || (count < 0 && (errno == EAGAIN || errno == EINTR))) + { + break; + } + if (count < 0) + { + throw std::runtime_error("read failed for " + port_ + ": " + std::strerror(errno)); + } + for (ssize_t i = 0; i < count; ++i) + { + if (auto frame = parser_.process(bytes[static_cast(i)])) + { + frames.push_back(std::move(*frame)); + } + } + if (count < static_cast(bytes.size())) + { + break; + } + } +#else + (void)timeout_ms; +#endif + return frames; +} + +std::vector discover_serial_ports() +{ + std::vector paths; +#ifndef _WIN32 + append_glob("/dev/ttyUSB*", paths); + append_glob("/dev/ttyACM*", paths); + append_glob("/dev/ttyXRUSB*", paths); + append_glob("/dev/ttyOBC*", paths); + std::sort(paths.begin(), paths.end()); + paths.erase(std::unique(paths.begin(), paths.end()), paths.end()); +#endif + return paths; +} + +std::optional probe_glove(SerialGlove& glove, int timeout_ms) +{ + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(timeout_ms); + while (std::chrono::steady_clock::now() < deadline) + { + glove.send(Command::Version); + for (const auto& frame : glove.read_available(40)) + { + if (auto version = decode_version(frame)) + { + return version; + } + } + } + return std::nullopt; +} + +} // namespace plugins::realhand_ffg_glove diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.hpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.hpp new file mode 100644 index 0000000000..4fd4e761c3 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.hpp @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "realhand_ffg_glove_protocol.hpp" + +#include +#include +#include +#include +#include + +namespace plugins::realhand_ffg_glove +{ + +class SerialGlove +{ +public: + SerialGlove(const std::string& port, int baudrate); + ~SerialGlove(); + + SerialGlove(const SerialGlove&) = delete; + SerialGlove& operator=(const SerialGlove&) = delete; + + void send(Command command, const std::vector& payload = {}); + std::vector read_available(int timeout_ms); + const std::string& port() const + { + return port_; + } + int baudrate() const + { + return baudrate_; + } + +private: + int fd_ = -1; + std::string port_; + int baudrate_ = 0; + FrameParser parser_; +}; + +std::vector discover_serial_ports(); +std::optional probe_glove(SerialGlove& glove, int timeout_ms = 250); + +} // namespace plugins::realhand_ffg_glove diff --git a/src/python/isaaccapture/retargeters/__init__.py b/src/python/isaaccapture/retargeters/__init__.py index bc0179ca61..b02b0fbe51 100644 --- a/src/python/isaaccapture/retargeters/__init__.py +++ b/src/python/isaaccapture/retargeters/__init__.py @@ -130,6 +130,52 @@ "WujiHandRetargeterConfig", "wuji", ), + # .realhand (P7 arm and L6/O6/L20 hands) + "P7ControllerPoseRetargeter": ( + ".realhand.arm", + "P7ControllerPoseRetargeter", + "retargeters-lite", + ), + "P7HandPoseRetargeter": ( + ".realhand.arm", + "P7HandPoseRetargeter", + "retargeters-lite", + ), + "P7WorkspacePoseConfig": ( + ".realhand.arm", + "P7WorkspacePoseConfig", + "retargeters-lite", + ), + "RealHandHandTrackingRetargeter": ( + ".realhand.hand", + "RealHandHandTrackingRetargeter", + None, + ), + "RealHandHandTrackingRetargeterConfig": ( + ".realhand.hand", + "RealHandHandTrackingRetargeterConfig", + None, + ), + "ControllerTriggerRealHandRetargeter": ( + ".realhand.hand", + "ControllerTriggerRealHandRetargeter", + None, + ), + "ControllerTriggerRealHandRetargeterConfig": ( + ".realhand.hand", + "ControllerTriggerRealHandRetargeterConfig", + None, + ), + "RealHandFFGGloveRetargeter": ( + ".realhand.realhand_ffg_glove", + "RealHandFFGGloveRetargeter", + None, + ), + "RealHandFFGGloveRetargeterConfig": ( + ".realhand.realhand_ffg_glove", + "RealHandFFGGloveRetargeterConfig", + None, + ), # .joint_space (generic joint-space devices: leader arms, exoskeletons, ...) "JointStateRetargeter": ( ".joint_space.joint_state_retargeter", @@ -256,6 +302,16 @@ def __getattr__(name: str): # Wuji hand retargeters (require wuji extra: wuji-sdk[retarget]) "WujiHandRetargeter", "WujiHandRetargeterConfig", + # RealHand P7 arm and L6/O6/L20 hand retargeters + "P7ControllerPoseRetargeter", + "P7HandPoseRetargeter", + "P7WorkspacePoseConfig", + "RealHandHandTrackingRetargeter", + "RealHandHandTrackingRetargeterConfig", + "ControllerTriggerRealHandRetargeter", + "ControllerTriggerRealHandRetargeterConfig", + "RealHandFFGGloveRetargeter", + "RealHandFFGGloveRetargeterConfig", # Generic joint-space device retargeters (leader arms, exoskeletons, ...) "JointStateRetargeter", "JointStateRetargeterConfig", diff --git a/src/python/isaaccapture/retargeters/realhand/__init__.py b/src/python/isaaccapture/retargeters/realhand/__init__.py new file mode 100644 index 0000000000..64acacac81 --- /dev/null +++ b/src/python/isaaccapture/retargeters/realhand/__init__.py @@ -0,0 +1,54 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""RealHand P7 arm and L6/O6/L20 hand retargeters.""" + +from .arm import ( + P7ControllerPoseRetargeter, + P7HandPoseRetargeter, + P7WorkspacePoseConfig, +) +from .hand import ( + ControllerTriggerRealHandRetargeter, + ControllerTriggerRealHandRetargeterConfig, + RealHandHandTrackingRetargeter, + RealHandHandTrackingRetargeterConfig, + realhand_handtracking_thumb_defaults, +) +from .realhand_ffg_glove import ( + RealHandFFGGloveRetargeter, + RealHandFFGGloveRetargeterConfig, + REALHAND_FFG_GLOVE_SENSOR_NAMES, + load_realhand_ffg_glove_calibration, +) +from .profiles import ( + L6_PROFILE, + L20_PROFILE, + O6_PROFILE, + REALHAND_PROFILES, + RealHandJointSpec, + RealHandProfile, + get_realhand_profile, +) + +__all__ = [ + "ControllerTriggerRealHandRetargeter", + "ControllerTriggerRealHandRetargeterConfig", + "RealHandFFGGloveRetargeter", + "RealHandFFGGloveRetargeterConfig", + "REALHAND_FFG_GLOVE_SENSOR_NAMES", + "L6_PROFILE", + "L20_PROFILE", + "O6_PROFILE", + "P7ControllerPoseRetargeter", + "P7HandPoseRetargeter", + "P7WorkspacePoseConfig", + "REALHAND_PROFILES", + "RealHandHandTrackingRetargeter", + "RealHandHandTrackingRetargeterConfig", + "RealHandJointSpec", + "RealHandProfile", + "get_realhand_profile", + "load_realhand_ffg_glove_calibration", + "realhand_handtracking_thumb_defaults", +] diff --git a/src/python/isaaccapture/retargeters/realhand/arm.py b/src/python/isaaccapture/retargeters/realhand/arm.py new file mode 100644 index 0000000000..4e43cbbb00 --- /dev/null +++ b/src/python/isaaccapture/retargeters/realhand/arm.py @@ -0,0 +1,230 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""P7 absolute controller and wrist pose mapping. + +The nodes emit the standard seven-element ``ee_pose`` expected by an Isaac Lab task-space +controller. Robot inverse kinematics remains in the environment; these nodes only transform an +OpenXR pose into the calibrated P7 workspace. +""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np +from scipy.spatial.transform import Rotation + +from isaaccapture.retargeting_engine.interface import BaseRetargeter +from isaaccapture.retargeting_engine.interface.retargeter_core_types import ( + RetargeterIO, + RetargeterIOType, +) +from isaaccapture.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaaccapture.retargeting_engine.tensor_types import ( + ControllerInput, + ControllerInputIndex, + DLDataType, + HandInput, + HandInputIndex, + HandJointIndex, + NDArrayType, +) + + +def _as_np(value) -> np.ndarray: + return np.from_dlpack(value) + + +def _normalized_quaternion(quaternion: np.ndarray) -> np.ndarray: + quaternion = np.asarray(quaternion, dtype=np.float64) + norm = np.linalg.norm(quaternion) + if norm < 1.0e-8: + return np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float64) + return quaternion / norm + + +def _step_towards( + current: np.ndarray, target: np.ndarray, max_step: float +) -> np.ndarray: + delta = target - current + norm = np.linalg.norm(delta) + if max_step <= 0.0 or norm <= max_step: + return target + return current + delta * (max_step / norm) + + +def _rotation_towards( + current: Rotation, target: Rotation, alpha: float, max_step_deg: float +) -> Rotation: + relative = target * current.inv() + angle = relative.magnitude() + if angle < 1.0e-8: + return target + max_step = np.deg2rad(max_step_deg) + fraction = min( + float(np.clip(alpha, 0.0, 1.0)), + max_step / angle if max_step > 0.0 else 1.0, + ) + return Rotation.from_rotvec(relative.as_rotvec() * fraction) * current + + +@dataclass +class P7WorkspacePoseConfig: + """Shared absolute workspace mapping parameters for one P7 arm.""" + + input_device: str + fallback_position: tuple[float, float, float] + fallback_rotation: tuple[float, float, float, float] + input_center: tuple[float, float, float] + workspace_center: tuple[float, float, float] + position_scale: tuple[float, float, float] = (1.0, 1.0, 1.0) + max_delta: tuple[float, float, float] = (0.65, 0.65, 0.75) + rotation_offset_rpy_deg: tuple[float, float, float] = (0.0, 0.0, 0.0) + position_offset_local: tuple[float, float, float] = (0.0, 0.0, 0.0) + max_position_step_m: float = 1.0 + position_smoothing_alpha: float = 1.0 + orientation_smoothing_alpha: float = 1.0 + max_orientation_step_deg: float = 180.0 + + +class _P7WorkspacePoseRetargeter(BaseRetargeter): + def __init__(self, config: P7WorkspacePoseConfig, name: str) -> None: + self._config = config + self._fallback_position = np.asarray(config.fallback_position, dtype=np.float64) + self._fallback_rotation = Rotation.from_quat( + _normalized_quaternion(np.asarray(config.fallback_rotation)) + ) + self._input_center = np.asarray(config.input_center, dtype=np.float64) + self._workspace_center = np.asarray(config.workspace_center, dtype=np.float64) + self._position_scale = np.asarray(config.position_scale, dtype=np.float64) + self._max_delta = np.asarray(config.max_delta, dtype=np.float64) + self._rotation_offset = Rotation.from_euler( + "XYZ", config.rotation_offset_rpy_deg, degrees=True + ) + self._position_offset_local = np.asarray( + config.position_offset_local, dtype=np.float64 + ) + self._smoothed_position = self._fallback_position.copy() + self._smoothed_rotation = self._fallback_rotation + self._has_valid_pose = False + self._last_pose = np.concatenate( + [self._fallback_position, self._fallback_rotation.as_quat()] + ).astype(np.float32) + super().__init__(name=name) + + def output_spec(self) -> RetargeterIOType: + return { + "ee_pose": TensorGroupType( + "ee_pose", + [ + NDArrayType( + "pose", shape=(7,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + ], + ) + } + + def _update_pose( + self, + input_position: np.ndarray, + input_rotation: Rotation, + output, + reset: bool, + ) -> None: + delta = (input_position - self._input_center) * self._position_scale + delta = np.clip(delta, -self._max_delta, self._max_delta) + target_rotation = input_rotation * self._rotation_offset + target_position = ( + self._workspace_center + + delta + + target_rotation.apply(self._position_offset_local) + ) + + if reset or not self._has_valid_pose: + self._smoothed_position = target_position.copy() + self._smoothed_rotation = target_rotation + self._has_valid_pose = True + else: + alpha = float(np.clip(self._config.position_smoothing_alpha, 0.0, 1.0)) + filtered = alpha * target_position + (1.0 - alpha) * self._smoothed_position + self._smoothed_position = _step_towards( + self._smoothed_position, + filtered, + self._config.max_position_step_m, + ) + + target_quaternion = target_rotation.as_quat() + if np.dot(target_quaternion, self._smoothed_rotation.as_quat()) < 0.0: + target_quaternion = -target_quaternion + self._smoothed_rotation = _rotation_towards( + self._smoothed_rotation, + Rotation.from_quat(_normalized_quaternion(target_quaternion)), + self._config.orientation_smoothing_alpha, + self._config.max_orientation_step_deg, + ) + + self._last_pose = np.concatenate( + [self._smoothed_position, self._smoothed_rotation.as_quat()] + ).astype(np.float32) + output[0] = self._last_pose + + +class P7ControllerPoseRetargeter(_P7WorkspacePoseRetargeter): + """Map an absolute OpenXR controller grip pose into one P7 arm workspace.""" + + def input_spec(self) -> RetargeterIOType: + return {self._config.input_device: OptionalType(ControllerInput())} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + output = outputs["ee_pose"] + controller = inputs[self._config.input_device] + if controller.is_none or not bool( + controller[ControllerInputIndex.GRIP_IS_VALID] + ): + output[0] = self._last_pose + return + self._update_pose( + np.asarray( + _as_np(controller[ControllerInputIndex.GRIP_POSITION]), + dtype=np.float64, + ), + Rotation.from_quat( + _normalized_quaternion( + _as_np(controller[ControllerInputIndex.GRIP_ORIENTATION]) + ) + ), + output, + context.execution_events.reset, + ) + + +class P7HandPoseRetargeter(_P7WorkspacePoseRetargeter): + """Map an absolute OpenXR wrist pose into one P7 arm workspace.""" + + def input_spec(self) -> RetargeterIOType: + return {self._config.input_device: OptionalType(HandInput())} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + output = outputs["ee_pose"] + hand = inputs[self._config.input_device] + if hand.is_none: + output[0] = self._last_pose + return + valid = _as_np(hand[HandInputIndex.JOINT_VALID]) + if valid[HandJointIndex.WRIST] == 0: + output[0] = self._last_pose + return + positions = _as_np(hand[HandInputIndex.JOINT_POSITIONS]) + orientations = _as_np(hand[HandInputIndex.JOINT_ORIENTATIONS]) + self._update_pose( + np.asarray(positions[HandJointIndex.WRIST], dtype=np.float64), + Rotation.from_quat( + _normalized_quaternion(orientations[HandJointIndex.WRIST]) + ), + output, + context.execution_events.reset, + ) diff --git a/src/python/isaaccapture/retargeters/realhand/hand.py b/src/python/isaaccapture/retargeters/realhand/hand.py new file mode 100644 index 0000000000..19edb4d00f --- /dev/null +++ b/src/python/isaaccapture/retargeters/realhand/hand.py @@ -0,0 +1,503 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Model-aware OpenXR hand and controller-trigger retargeters.""" + +from __future__ import annotations + +import os +from dataclasses import dataclass + +import numpy as np +from isaaccapture.retargeting_engine.interface import BaseRetargeter +from isaaccapture.retargeting_engine.interface.retargeter_core_types import ( + RetargeterIO, + RetargeterIOType, +) +from isaaccapture.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaaccapture.retargeting_engine.tensor_types import ( + ControllerInput, + ControllerInputIndex, + FloatType, + HandInput, + HandInputIndex, + HandJointIndex, +) + +from .profiles import RealHandJointSpec, get_realhand_profile + +_FINGER_JOINTS = { + "index": ( + HandJointIndex.INDEX_METACARPAL, + HandJointIndex.INDEX_PROXIMAL, + HandJointIndex.INDEX_INTERMEDIATE, + HandJointIndex.INDEX_DISTAL, + HandJointIndex.INDEX_TIP, + ), + "middle": ( + HandJointIndex.MIDDLE_METACARPAL, + HandJointIndex.MIDDLE_PROXIMAL, + HandJointIndex.MIDDLE_INTERMEDIATE, + HandJointIndex.MIDDLE_DISTAL, + HandJointIndex.MIDDLE_TIP, + ), + "ring": ( + HandJointIndex.RING_METACARPAL, + HandJointIndex.RING_PROXIMAL, + HandJointIndex.RING_INTERMEDIATE, + HandJointIndex.RING_DISTAL, + HandJointIndex.RING_TIP, + ), + "pinky": ( + HandJointIndex.LITTLE_METACARPAL, + HandJointIndex.LITTLE_PROXIMAL, + HandJointIndex.LITTLE_INTERMEDIATE, + HandJointIndex.LITTLE_DISTAL, + HandJointIndex.LITTLE_TIP, + ), +} + + +def _as_np(value) -> np.ndarray: + return np.from_dlpack(value) + + +def _safe_normalized(vector: np.ndarray) -> np.ndarray | None: + norm = np.linalg.norm(vector) + if norm < 1.0e-6: + return None + return vector / norm + + +def _angle_between(first: np.ndarray, second: np.ndarray) -> float: + first_n = _safe_normalized(first) + second_n = _safe_normalized(second) + if first_n is None or second_n is None: + return 0.0 + return float(np.arccos(np.clip(np.dot(first_n, second_n), -1.0, 1.0))) + + +def _env_debug_enabled() -> bool: + value = os.getenv("P7_REALHAND_DEBUG_CONTROLLER", "") + return value.strip().lower() in ("1", "true", "yes", "on") + + +def _env_debug_every() -> int: + value = os.getenv("P7_REALHAND_DEBUG_CONTROLLER_EVERY", "30") + try: + return max(1, int(value)) + except ValueError: + return 30 + + +@dataclass +class RealHandHandTrackingRetargeterConfig: + input_device: str + joint_names: list[str] + side: str + hand_model: str = "l6" + smoothing_alpha: float = 0.65 + finger_curl_scale: float = 0.70 + thumb_flex_scale: float = 0.55 + finger_close_gain: float = 1.0 + thumb_close_gain: float = 1.1 + thumb_opposition_gain: float = 1.0 + finger_spread_gain: float = 1.0 + curl_response_exponent: float = 1.0 + thumb_curl_response_exponent: float = 1.0 + thumb_opposition_to_flex: float = 0.85 + calibrate_open_on_first_frame: bool = True + adaptive_open_baseline: bool = True + + +def realhand_handtracking_thumb_defaults(hand_model: str) -> dict[str, float]: + """Return model-specific thumb response defaults for OpenXR handtracking.""" + model = get_realhand_profile(hand_model).model + if model in ("l6", "o6"): + return { + "thumb_flex_scale": 0.55, + "thumb_close_gain": 1.10, + "thumb_opposition_gain": 1.00, + "thumb_curl_response_exponent": 1.00, + "thumb_opposition_to_flex": 0.85, + } + return { + "thumb_flex_scale": 1.15, + "thumb_close_gain": 1.80, + "thumb_opposition_gain": 1.80, + "thumb_curl_response_exponent": 0.75, + "thumb_opposition_to_flex": 0.85, + } + + +class RealHandHandTrackingRetargeter(BaseRetargeter): + """Retarget OpenXR joint geometry to an L6, O6, or L20 hand.""" + + def __init__(self, config: RealHandHandTrackingRetargeterConfig, name: str) -> None: + if config.side not in ("left", "right"): + raise ValueError(f"side must be left or right, got {config.side!r}") + self._config = config + self._profile = get_realhand_profile(config.hand_model) + expected_names = self._profile.joint_names(config.side) + if set(config.joint_names) != set(expected_names): + missing = sorted(set(expected_names) - set(config.joint_names)) + extra = sorted(set(config.joint_names) - set(expected_names)) + raise ValueError( + f"{self._profile.display_name} {config.side} joint set mismatch; missing={missing}, extra={extra}" + ) + self._specs = [ + self._profile.joint_spec(config.side, name) for name in config.joint_names + ] + self._last_output = np.zeros(len(config.joint_names), dtype=np.float64) + self._open_baseline: np.ndarray | None = None + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return {self._config.input_device: OptionalType(HandInput())} + + def output_spec(self) -> RetargeterIOType: + return { + "hand_joints": TensorGroupType( + f"{self._profile.model}_{self._config.side}_hand_joints", + [FloatType(name) for name in self._config.joint_names], + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + output = outputs["hand_joints"] + hand_group = inputs[self._config.input_device] + + if context.execution_events.reset: + self._last_output[:] = 0.0 + self._open_baseline = None + + if hand_group.is_none: + self._write_output(output) + return + + positions = _as_np(hand_group[HandInputIndex.JOINT_POSITIONS]).astype( + np.float64 + ) + valid = _as_np(hand_group[HandInputIndex.JOINT_VALID]) + + raw_target = np.array( + [self._raw_joint_target(spec, positions, valid) for spec in self._specs], + dtype=np.float64, + ) + if self._config.calibrate_open_on_first_frame and self._open_baseline is None: + self._open_baseline = raw_target.copy() + elif self._config.adaptive_open_baseline and self._open_baseline is not None: + for index, spec in enumerate(self._specs): + if not spec.semantic.endswith("_spread"): + self._open_baseline[index] = min( + self._open_baseline[index], raw_target[index] + ) + + target = ( + raw_target.copy() + if self._open_baseline is None + else raw_target - self._open_baseline + ) + for index, spec in enumerate(self._specs): + target[index] = self._shape_target(target[index], spec) + + alpha = float(np.clip(self._config.smoothing_alpha, 0.0, 1.0)) + self._last_output = alpha * target + (1.0 - alpha) * self._last_output + self._write_output(output) + + def _write_output(self, output) -> None: + for index, value in enumerate(self._last_output): + output[index] = float(value) + + @staticmethod + def _valid(valid: np.ndarray, *indices: int) -> bool: + return all(valid[index] > 0 for index in indices) + + def _raw_joint_target( + self, spec: RealHandJointSpec, positions: np.ndarray, valid: np.ndarray + ) -> float: + semantic = spec.semantic + if semantic == "thumb_opposition" or semantic == "thumb_rotation": + value = self._thumb_opposition_close(positions, valid) * spec.upper + elif semantic == "thumb_flex": + flexion = self._thumb_flexion_angles(positions, valid) + value = sum(flexion) * self._config.thumb_flex_scale + value = max( + value, + self._thumb_opposition_close(positions, valid) + * spec.upper + * self._config.thumb_opposition_to_flex, + ) + elif semantic == "thumb_base_flex": + base_flexion, _ = self._thumb_flexion_angles(positions, valid) + value = base_flexion * self._config.thumb_flex_scale + elif semantic == "thumb_distal_flex": + _, distal_flexion = self._thumb_flexion_angles(positions, valid) + value = distal_flexion * self._config.thumb_flex_scale + value = max( + value, + self._thumb_opposition_close(positions, valid) + * spec.upper + * self._config.thumb_opposition_to_flex, + ) + else: + finger = semantic.split("_", 1)[0] + if semantic.endswith("_curl"): + _, distal = self._finger_flexion_angles(finger, positions, valid) + value = distal * self._config.finger_curl_scale + elif semantic.endswith("_base_flex"): + base, _ = self._finger_flexion_angles(finger, positions, valid) + value = base * self._config.finger_curl_scale + elif semantic.endswith("_distal_flex"): + _, distal = self._finger_flexion_angles(finger, positions, valid) + value = distal * self._config.finger_curl_scale + elif semantic.endswith("_spread"): + value = self._finger_spread(finger, positions, valid) + else: + raise ValueError(f"unsupported hand-joint semantic {semantic!r}") + return float(np.clip(value, spec.lower, spec.upper)) + + def _shape_target(self, value: float, spec: RealHandJointSpec) -> float: + if spec.semantic.endswith("_spread"): + return float( + np.clip(value * self._config.finger_spread_gain, spec.lower, spec.upper) + ) + + upper = max(spec.upper, 1.0e-6) + normalized = np.clip(value / upper, 0.0, 1.0) + is_thumb = spec.semantic.startswith("thumb") + if spec.semantic in ("thumb_opposition", "thumb_rotation"): + gain = self._config.thumb_opposition_gain + else: + gain = ( + self._config.thumb_close_gain + if is_thumb + else self._config.finger_close_gain + ) + shaped = np.clip(normalized * gain, 0.0, 1.0) + exponent = ( + max(float(self._config.thumb_curl_response_exponent), 1.0e-3) + if is_thumb + else max(float(self._config.curl_response_exponent), 1.0e-3) + ) + return float(np.clip((shaped**exponent) * upper, spec.lower, spec.upper)) + + def _finger_flexion_angles( + self, finger: str, positions: np.ndarray, valid: np.ndarray + ) -> tuple[float, float]: + joints = _FINGER_JOINTS[finger] + if not self._valid(valid, *joints): + return 0.0, 0.0 + metacarpal, proximal, intermediate, distal, tip = ( + positions[index] for index in joints + ) + metacarpal_segment = proximal - metacarpal + proximal_segment = intermediate - proximal + intermediate_segment = distal - intermediate + distal_segment = tip - distal + base = _angle_between(metacarpal_segment, proximal_segment) + distal_curl = _angle_between( + proximal_segment, intermediate_segment + ) + _angle_between(intermediate_segment, distal_segment) + return base, distal_curl + + def _finger_spread( + self, finger: str, positions: np.ndarray, valid: np.ndarray + ) -> float: + metacarpal, proximal, _, _, _ = _FINGER_JOINTS[finger] + required = ( + HandJointIndex.WRIST, + HandJointIndex.PALM, + HandJointIndex.INDEX_METACARPAL, + HandJointIndex.LITTLE_METACARPAL, + metacarpal, + proximal, + ) + if not self._valid(valid, *required): + return 0.0 + forward = _safe_normalized( + positions[HandJointIndex.PALM] - positions[HandJointIndex.WRIST] + ) + lateral = _safe_normalized( + positions[HandJointIndex.INDEX_METACARPAL] + - positions[HandJointIndex.LITTLE_METACARPAL] + ) + segment = _safe_normalized(positions[proximal] - positions[metacarpal]) + if forward is None or lateral is None or segment is None: + return 0.0 + return float(np.arctan2(np.dot(segment, lateral), np.dot(segment, forward))) + + def _thumb_flexion_angles( + self, positions: np.ndarray, valid: np.ndarray + ) -> tuple[float, float]: + joints = ( + HandJointIndex.THUMB_METACARPAL, + HandJointIndex.THUMB_PROXIMAL, + HandJointIndex.THUMB_DISTAL, + HandJointIndex.THUMB_TIP, + ) + if not self._valid(valid, *joints): + return 0.0, 0.0 + metacarpal, proximal, distal, tip = (positions[index] for index in joints) + first = proximal - metacarpal + second = distal - proximal + third = tip - distal + return _angle_between(first, second), _angle_between(second, third) + + def _thumb_opposition_close( + self, positions: np.ndarray, valid: np.ndarray + ) -> float: + required = ( + HandJointIndex.THUMB_TIP, + HandJointIndex.INDEX_PROXIMAL, + HandJointIndex.LITTLE_PROXIMAL, + ) + if not self._valid(valid, *required): + return 0.0 + + thumb_tip = positions[HandJointIndex.THUMB_TIP] + palm_width = max( + np.linalg.norm( + positions[HandJointIndex.INDEX_PROXIMAL] + - positions[HandJointIndex.LITTLE_PROXIMAL] + ), + 1.0e-3, + ) + closes = [] + for tip_index in ( + HandJointIndex.INDEX_TIP, + HandJointIndex.MIDDLE_TIP, + HandJointIndex.RING_TIP, + HandJointIndex.LITTLE_TIP, + ): + if not self._valid(valid, tip_index): + continue + distance = np.linalg.norm(thumb_tip - positions[tip_index]) + closes.append( + np.clip((1.8 * palm_width - distance) / (1.5 * palm_width), 0.0, 1.0) + ) + return float(max(closes)) if closes else 0.0 + + +@dataclass +class ControllerTriggerRealHandRetargeterConfig: + input_device: str + joint_names: list[str] + side: str + hand_model: str = "l6" + close_at_trigger_one: bool = True + smoothing_alpha: float = 0.8 + trigger_source: str = "max" + max_close_step: float = 1.0 + max_open_step: float = 1.0 + + +class ControllerTriggerRealHandRetargeter(BaseRetargeter): + """Map one controller's trigger to its matching RealHand posture.""" + + def __init__( + self, config: ControllerTriggerRealHandRetargeterConfig, name: str + ) -> None: + if config.side not in ("left", "right"): + raise ValueError(f"side must be left or right, got {config.side!r}") + if config.trigger_source not in ("trigger", "squeeze", "max"): + raise ValueError( + f"trigger_source must be trigger, squeeze, or max, got {config.trigger_source!r}" + ) + if config.max_close_step <= 0.0 or config.max_open_step <= 0.0: + raise ValueError("max_close_step and max_open_step must both be positive") + self._config = config + self._profile = get_realhand_profile(config.hand_model) + self._specs = [ + self._profile.joint_spec(config.side, name) for name in config.joint_names + ] + self._last_output = np.zeros(len(config.joint_names), dtype=np.float64) + self._last_close_amount = 0.0 + self._debug_enabled = _env_debug_enabled() + self._debug_every = _env_debug_every() + self._debug_counter = 0 + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return {self._config.input_device: OptionalType(ControllerInput())} + + def output_spec(self) -> RetargeterIOType: + return { + "hand_joints": TensorGroupType( + f"{self._profile.model}_{self._config.side}_trigger_joints", + [FloatType(name) for name in self._config.joint_names], + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + output = outputs["hand_joints"] + controller_group = inputs[self._config.input_device] + if controller_group.is_none or not bool( + controller_group[ControllerInputIndex.GRIP_IS_VALID] + ): + self._last_output[:] = 0.0 + self._last_close_amount = 0.0 + self._write_output(output) + return + + trigger_value = float( + np.clip(controller_group[ControllerInputIndex.TRIGGER_VALUE], 0.0, 1.0) + ) + squeeze_value = float( + np.clip(controller_group[ControllerInputIndex.SQUEEZE_VALUE], 0.0, 1.0) + ) + if self._config.trigger_source == "trigger": + close_input = trigger_value + elif self._config.trigger_source == "squeeze": + close_input = squeeze_value + else: + close_input = max(trigger_value, squeeze_value) + requested = ( + close_input if self._config.close_at_trigger_one else 1.0 - close_input + ) + + if context.execution_events.reset: + self._last_output[:] = 0.0 + self._last_close_amount = 0.0 + delta = requested - self._last_close_amount + max_step = ( + self._config.max_close_step if delta >= 0.0 else self._config.max_open_step + ) + self._last_close_amount = float( + np.clip( + self._last_close_amount + np.clip(delta, -max_step, max_step), 0.0, 1.0 + ) + ) + target = np.array( + [self._last_close_amount * spec.trigger_closed for spec in self._specs], + dtype=np.float64, + ) + alpha = float(np.clip(self._config.smoothing_alpha, 0.0, 1.0)) + self._last_output = alpha * target + (1.0 - alpha) * self._last_output + self._write_output(output) + + if self._debug_enabled: + self._debug_counter += 1 + if self._debug_counter == 1 or self._debug_counter % self._debug_every == 0: + print( + f"[P7 {self._profile.display_name} controller trigger]" + f" side={self._config.side} trigger={trigger_value:.3f}" + f" squeeze={squeeze_value:.3f} close={self._last_close_amount:.3f}", + flush=True, + ) + + def _write_output(self, output) -> None: + for index, value in enumerate(self._last_output): + output[index] = float(value) + + +__all__ = [ + "ControllerTriggerRealHandRetargeter", + "ControllerTriggerRealHandRetargeterConfig", + "RealHandHandTrackingRetargeter", + "RealHandHandTrackingRetargeterConfig", + "realhand_handtracking_thumb_defaults", +] diff --git a/src/python/isaaccapture/retargeters/realhand/profiles.py b/src/python/isaaccapture/retargeters/realhand/profiles.py new file mode 100644 index 0000000000..7731b7a81e --- /dev/null +++ b/src/python/isaaccapture/retargeters/realhand/profiles.py @@ -0,0 +1,320 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Kinematic and command profiles for RealHand simulation models.""" + +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path + + +@dataclass(frozen=True) +class RealHandJointSpec: + """Description of one actively controlled hand joint.""" + + short_name: str + lower: float + upper: float + trigger_closed: float + semantic: str + ffg_motor_index: int + ffg_command_at_lower: float = 255.0 + ffg_command_at_upper: float = 0.0 + + +@dataclass(frozen=True) +class RealHandProfile: + """Model-specific joints while keeping retargeting code model agnostic.""" + + model: str + display_name: str + asset_dir_name: str + urdf_name: str + left_joints: tuple[RealHandJointSpec, ...] + right_joints: tuple[RealHandJointSpec, ...] + action_joint_names: tuple[str, ...] + mimic_joint_names: tuple[str, ...] = () + + def joints(self, side: str) -> tuple[RealHandJointSpec, ...]: + if side == "left": + return self.left_joints + if side == "right": + return self.right_joints + raise ValueError(f"side must be left or right, got {side!r}") + + def joint_names(self, side: str) -> list[str]: + return [f"{side}_hand_{joint.short_name}" for joint in self.joints(side)] + + def joint_spec(self, side: str, joint_name: str) -> RealHandJointSpec: + short_name = joint_name.removeprefix(f"{side}_hand_") + for joint in self.joints(side): + if joint.short_name == short_name: + return joint + raise KeyError( + f"{joint_name!r} is not an active {self.display_name} {side}-hand joint" + ) + + def resolve_urdf(self, asset_root: str | Path) -> Path: + """Resolve this profile's downloaded bimanual URDF.""" + path = Path(asset_root).expanduser() / self.asset_dir_name / self.urdf_name + if not path.is_file(): + raise FileNotFoundError( + f"{self.display_name} URDF not found at {path}. Run " + "examples/teleop/python/scripts/fetch_realhand_assets.py first." + ) + return path.resolve() + + @property + def motor_count(self) -> int: + return 1 + max( + joint.ffg_motor_index for joint in self.left_joints + self.right_joints + ) + + +def _joint( + name: str, + upper: float, + semantic: str, + motor: int, + *, + lower: float = 0.0, + closed: float | None = None, + command_lower: float = 255.0, + command_upper: float = 0.0, +) -> RealHandJointSpec: + return RealHandJointSpec( + short_name=name, + lower=lower, + upper=upper, + trigger_closed=upper if closed is None else closed, + semantic=semantic, + ffg_motor_index=motor, + ffg_command_at_lower=command_lower, + ffg_command_at_upper=command_upper, + ) + + +_L6_LEFT = ( + _joint("index_mcp_pitch", 1.26, "index_curl", 2), + _joint("index_dip", 1.14, "index_curl", 2), + _joint("middle_mcp_pitch", 1.26, "middle_curl", 3), + _joint("middle_dip", 1.14, "middle_curl", 3), + _joint("pinky_mcp_pitch", 1.26, "pinky_curl", 5), + _joint("pinky_dip", 1.14, "pinky_curl", 5), + _joint("ring_mcp_pitch", 1.26, "ring_curl", 4), + _joint("ring_dip", 1.14, "ring_curl", 4), + _joint("thunb_cmc_roll", 1.39, "thumb_opposition", 1), + _joint("thumb_cmc_pitch", 0.99, "thumb_flex", 0), + _joint("thumb_dip", 1.22, "thumb_flex", 0), +) + +_L6_RIGHT = ( + _joint("index_mcp_pitch", 1.26, "index_curl", 2), + _joint("index_dip", 1.14, "index_curl", 2), + _joint("middle_mcp_pitch", 1.26, "middle_curl", 3), + _joint("middle_dip", 1.14, "middle_curl", 3), + _joint("pinky_mcp_pitch", 1.26, "pinky_curl", 5), + _joint("pinky_dip", 1.14, "pinky_curl", 5), + _joint("ring_mcp_pitch", 1.26, "ring_curl", 4), + _joint("ring_dip", 1.14, "ring_curl", 4), + _joint("thunb_cmc_roll", 1.39, "thumb_opposition", 1), + _joint("thumb_cmc_pitch", 0.99, "thumb_flex", 0), + _joint("thumb_ip", 1.22, "thumb_flex", 0), +) + +_L6_ACTION_JOINTS = ( + "left_hand_thunb_cmc_roll", + "left_hand_index_mcp_pitch", + "left_hand_middle_mcp_pitch", + "left_hand_ring_mcp_pitch", + "left_hand_pinky_mcp_pitch", + "right_hand_thunb_cmc_roll", + "right_hand_index_mcp_pitch", + "right_hand_middle_mcp_pitch", + "right_hand_ring_mcp_pitch", + "right_hand_pinky_mcp_pitch", + "left_hand_thumb_cmc_pitch", + "left_hand_index_dip", + "left_hand_middle_dip", + "left_hand_ring_dip", + "left_hand_pinky_dip", + "right_hand_thumb_cmc_pitch", + "right_hand_index_dip", + "right_hand_middle_dip", + "right_hand_ring_dip", + "right_hand_pinky_dip", + "left_hand_thumb_dip", + "right_hand_thumb_ip", +) + + +def _o6_joints(side: str) -> tuple[RealHandJointSpec, ...]: + thumb_yaw_upper = 1.30 if side == "left" else 1.36 + return ( + _joint("thumb_cmc_yaw", thumb_yaw_upper, "thumb_opposition", 1), + _joint("thumb_cmc_pitch", 0.58, "thumb_flex", 0), + _joint("index_mcp_pitch", 1.60, "index_curl", 2), + _joint("middle_mcp_pitch", 1.60, "middle_curl", 3), + _joint("ring_mcp_pitch", 1.60, "ring_curl", 4), + _joint("pinky_mcp_pitch", 1.60, "pinky_curl", 5), + ) + + +def _l20_joints(side: str) -> tuple[RealHandJointSpec, ...]: + thumb_roll_upper = 1.40 if side == "left" else 1.39 + thumb_pitch_upper = 0.84 if side == "left" else 0.83 + thumb_mcp_upper = 1.26 if side == "left" else 1.25 + pip_upper = 1.74 if side == "left" else 1.75 + joints = [ + _joint("thumb_cmc_roll", thumb_roll_upper, "thumb_rotation", 5, closed=0.50), + _joint("thumb_cmc_yaw", 1.57, "thumb_opposition", 10, closed=1.50), + _joint("thumb_cmc_pitch", thumb_pitch_upper, "thumb_base_flex", 0, closed=0.60), + _joint("thumb_mcp", thumb_mcp_upper, "thumb_distal_flex", 15, closed=1.20), + ] + for finger, root_motor, spread_motor, tip_motor in ( + ("index", 1, 6, 16), + ("middle", 2, 7, 17), + ("ring", 3, 8, 18), + ("pinky", 4, 9, 19), + ): + joints.extend( + ( + _joint( + f"{finger}_mcp_roll", + 0.23, + f"{finger}_spread", + spread_motor, + lower=-0.23, + closed=0.0, + command_lower=0.0, + command_upper=255.0, + ), + _joint( + f"{finger}_mcp_pitch", + 1.22, + f"{finger}_base_flex", + root_motor, + closed=1.20, + ), + _joint( + f"{finger}_pip", + pip_upper, + f"{finger}_distal_flex", + tip_motor, + closed=1.70, + ), + ) + ) + return tuple(joints) + + +def _o6_action_joints() -> tuple[str, ...]: + first_level = ( + "thumb_cmc_yaw", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", + ) + return tuple( + [f"left_hand_{name}" for name in first_level] + + [f"right_hand_{name}" for name in first_level] + + ["left_hand_thumb_cmc_pitch", "right_hand_thumb_cmc_pitch"] + ) + + +def _l20_action_joints() -> tuple[str, ...]: + levels = ( + ( + "thumb_cmc_roll", + "index_mcp_roll", + "middle_mcp_roll", + "ring_mcp_roll", + "pinky_mcp_roll", + ), + ( + "thumb_cmc_yaw", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", + ), + ("thumb_cmc_pitch", "index_pip", "middle_pip", "ring_pip", "pinky_pip"), + ("thumb_mcp",), + ) + return tuple( + f"{side}_hand_{name}" + for level in levels + for side in ("left", "right") + for name in level + ) + + +def _mimic_joint_names() -> tuple[str, ...]: + return tuple( + f"{side}_hand_{name}" + for side in ("left", "right") + for name in ("thumb_ip", "index_dip", "middle_dip", "ring_dip", "pinky_dip") + ) + + +L6_PROFILE = RealHandProfile( + model="l6", + display_name="L6", + asset_dir_name="p7_l6", + urdf_name="P7_l6_bimanual.urdf", + left_joints=_L6_LEFT, + right_joints=_L6_RIGHT, + action_joint_names=_L6_ACTION_JOINTS, +) + +O6_PROFILE = RealHandProfile( + model="o6", + display_name="O6", + asset_dir_name="p7_o6", + urdf_name="P7_o6_bimanual.urdf", + left_joints=_o6_joints("left"), + right_joints=_o6_joints("right"), + action_joint_names=_o6_action_joints(), + mimic_joint_names=_mimic_joint_names(), +) + +L20_PROFILE = RealHandProfile( + model="l20", + display_name="L20", + asset_dir_name="p7_l20", + urdf_name="P7_L20_bimanual.urdf", + left_joints=_l20_joints("left"), + right_joints=_l20_joints("right"), + action_joint_names=_l20_action_joints(), + mimic_joint_names=_mimic_joint_names(), +) + +REALHAND_PROFILES = { + L6_PROFILE.model: L6_PROFILE, + O6_PROFILE.model: O6_PROFILE, + L20_PROFILE.model: L20_PROFILE, +} + + +def get_realhand_profile(model: str) -> RealHandProfile: + key = model.strip().lower() + try: + return REALHAND_PROFILES[key] + except KeyError as exc: + supported = ", ".join(sorted(REALHAND_PROFILES)) + raise ValueError( + f"unsupported RealHand model {model!r}; expected one of: {supported}" + ) from exc + + +__all__ = [ + "L6_PROFILE", + "L20_PROFILE", + "O6_PROFILE", + "REALHAND_PROFILES", + "RealHandJointSpec", + "RealHandProfile", + "get_realhand_profile", +] diff --git a/src/python/isaaccapture/retargeters/realhand/realhand_ffg_glove.py b/src/python/isaaccapture/retargeters/realhand/realhand_ffg_glove.py new file mode 100644 index 0000000000..2f54ed7e42 --- /dev/null +++ b/src/python/isaaccapture/retargeters/realhand/realhand_ffg_glove.py @@ -0,0 +1,425 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Natural RealHand FFG Glove sensor-angle retargeting for L6, O6, and L20. + +USB transport and hand-side discovery live in the RealHand FFG Glove C++ device plugin. This +module consumes one standard ``JointStateSource`` and maps only that glove's 21 sensors to one +mechanical hand. Thumb outputs depend only on thumb sensors, and each non-thumb digit depends only +on that digit's sensors; there is no contact snapping or cross-finger pose activation. +""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import dataclass +from pathlib import Path + +import numpy as np +import yaml + +from isaaccapture.retargeting_engine.interface import BaseRetargeter +from isaaccapture.retargeting_engine.interface.retargeter_core_types import ( + RetargeterIO, + RetargeterIOType, +) +from isaaccapture.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaaccapture.retargeting_engine.tensor_types import FloatType + +from .profiles import RealHandJointSpec, get_realhand_profile + +REALHAND_FFG_GLOVE_SENSOR_NAMES = tuple(f"sensor_{index}" for index in range(21)) + +_SENSOR_CHANNELS = { + "thumb_rotation": (1,), + "thumb_opposition": (1, 2), + "thumb_flex": (2,), + "thumb_base_flex": (2,), + "thumb_distal_flex": (2,), + "index_curl": (6, 8), + "index_spread": (5,), + "index_base_flex": (6, 8), + "index_distal_flex": (6, 8), + "middle_curl": (10, 12), + "middle_spread": (9,), + "middle_base_flex": (10, 12), + "middle_distal_flex": (10, 12), + "ring_curl": (14, 16), + "ring_spread": (13,), + "ring_base_flex": (14, 16), + "ring_distal_flex": (14, 16), + "pinky_curl": (18, 20), + "pinky_spread": (17,), + "pinky_base_flex": (18, 20), + "pinky_distal_flex": (18, 20), +} + +_L20_TOUCH_TARGETS = { + ("left", "index"): (0.483, 1.534, 0.407, 0.706), + ("left", "middle"): (0.777, 1.116, 0.413, 0.684), + ("left", "ring"): (0.943, 1.175, 0.415, 0.674), + ("left", "pinky"): (1.191, 1.135, 0.495, 0.611), + ("right", "index"): (0.615, 1.015, 0.453, 0.655), + ("right", "middle"): (0.740, 1.232, 0.533, 1.222), + ("right", "ring"): (1.003, 1.084, 0.379, 0.738), + ("right", "pinky"): (1.116, 1.245, 0.459, 0.657), +} +_L20_THUMB_SEMANTICS = ( + "thumb_rotation", + "thumb_opposition", + "thumb_base_flex", + "thumb_distal_flex", +) + + +def load_realhand_ffg_glove_calibration( + path: str | Path, side: str +) -> dict[str, object]: + """Load one side from a RealHand FFG Glove raw-calibration YAML file.""" + if side not in ("left", "right"): + raise ValueError(f"side must be left or right, got {side!r}") + calibration_path = Path(path).expanduser() + with calibration_path.open("r", encoding="utf-8") as stream: + data = yaml.safe_load(stream) + if not isinstance(data, dict): + raise ValueError( + f"RealHand FFG Glove calibration must be a YAML mapping: {calibration_path}" + ) + + suffix = "l" if side == "left" else "r" + other_suffix = "r" if suffix == "l" else "l" + + def extract(current_suffix: str) -> dict[str, object]: + oposes: dict[str, Sequence[float]] = {} + legacy = data.get(f"jointangleopose_{current_suffix}") + for finger in ("index", "middle", "ring", "pinky"): + value = data.get(f"jointangleopose_{finger}_{current_suffix}") + if value is None and finger == "index": + value = legacy + if value is not None: + oposes[finger] = value + return { + "open": data.get(f"jointangleoriginal_{current_suffix}"), + "fist": data.get(f"jointanglefist_{current_suffix}"), + "thumb_curl": data.get(f"jointanglethumb_curl_{current_suffix}"), + "oposes": oposes, + } + + calibration = extract(suffix) + if calibration["open"] is None or calibration["fist"] is None: + calibration = extract(other_suffix) + if calibration["open"] is None or calibration["fist"] is None: + raise ValueError( + "RealHand FFG Glove calibration has no complete open/fist pair " + f"for {side}: {calibration_path}" + ) + return calibration + + +@dataclass +class RealHandFFGGloveRetargeterConfig: + """Configuration for one RealHand FFG Glove and one RealHand mechanical hand.""" + + input_device: str + joint_names: list[str] + side: str + hand_model: str = "l6" + calibration_path: str | None = None + smoothing_alpha: float = 0.65 + joint_deadband_rad: float = 0.01 + max_close_step_rad: float = 0.05 + max_open_step_rad: float = 0.20 + finger_close_gain: float = 1.0 + thumb_flex_gain: float = 1.0 + thumb_roll_gain: float = 1.0 + spread_gain: float = 1.0 + hold_last_on_missing: bool = True + + +class _NaturalRealHandFFGGloveMapper: + def __init__(self, config: RealHandFFGGloveRetargeterConfig) -> None: + self.profile = get_realhand_profile(config.hand_model) + self.side = config.side + self.config = config + self.open: np.ndarray | None = None + self.fist: np.ndarray | None = None + self.thumb_curl: np.ndarray | None = None + self.oposes: dict[str, np.ndarray] = {} + self.baseline: np.ndarray | None = None + if config.calibration_path: + calibration = load_realhand_ffg_glove_calibration( + config.calibration_path, config.side + ) + self.open = self._coerce(calibration["open"]) + self.fist = self._coerce(calibration["fist"]) + if calibration["thumb_curl"] is not None: + self.thumb_curl = self._coerce(calibration["thumb_curl"]) + oposes = calibration["oposes"] + if isinstance(oposes, Mapping): + self.oposes = { + str(name): self._coerce(value) + for name, value in oposes.items() + if value is not None + } + + @staticmethod + def _coerce(values: object) -> np.ndarray: + array = np.asarray(values, dtype=np.float64).reshape(-1) + if array.size != len(REALHAND_FFG_GLOVE_SENSOR_NAMES) or not np.all( + np.isfinite(array) + ): + raise ValueError( + "RealHand FFG Glove samples must contain 21 finite sensor angles" + ) + return array + + def reset(self) -> None: + self.baseline = None + + def map(self, sensors: Sequence[float], joint_names: Sequence[str]) -> np.ndarray: + values = self._coerce(sensors) + if self.open is None or self.fist is None: + if self.baseline is None: + self.baseline = values.copy() + return np.zeros(len(joint_names), dtype=np.float64) + return self._uncalibrated(values - self.baseline, joint_names) + return np.asarray( + [ + self._joint_target(values, self.profile.joint_spec(self.side, name)) + for name in joint_names + ], + dtype=np.float64, + ) + + def _gain(self, spec: RealHandJointSpec) -> float: + if not spec.semantic.startswith("thumb"): + return self.config.finger_close_gain + if spec.semantic in ("thumb_rotation", "thumb_opposition"): + return self.config.thumb_roll_gain + return self.config.thumb_flex_gain + + def _closed_target(self, spec: RealHandJointSpec) -> float: + return float( + np.clip(spec.trigger_closed * self._gain(spec), spec.lower, spec.upper) + ) + + def _uncalibrated( + self, delta: np.ndarray, joint_names: Sequence[str] + ) -> np.ndarray: + targets = [] + for name in joint_names: + spec = self.profile.joint_spec(self.side, name) + channels = _SENSOR_CHANNELS.get(spec.semantic, ()) + response = max( + (abs(float(delta[index])) for index in channels), default=0.0 + ) + targets.append( + np.clip( + response * 2.0 * self._closed_target(spec), spec.lower, spec.upper + ) + ) + return np.asarray(targets, dtype=np.float64) + + def _joint_target(self, values: np.ndarray, spec: RealHandJointSpec) -> float: + assert self.open is not None and self.fist is not None + if self.profile.model == "l20" and spec.semantic.startswith("thumb"): + return self._l20_thumb_target(values, spec) + + channels = _SENSOR_CHANNELS.get(spec.semantic, ()) + if spec.semantic.endswith("_spread"): + response = self._signed_response(values, self.fist, channels) + magnitude = spec.upper if response >= 0.0 else abs(spec.lower) + return float( + np.clip( + response * magnitude * self.config.spread_gain, + spec.lower, + spec.upper, + ) + ) + response = np.clip(self._response(values, self.fist, channels), 0.0, 1.0) + return float( + np.clip(response * self._closed_target(spec), spec.lower, spec.upper) + ) + + def _response( + self, values: np.ndarray, target: np.ndarray, channels: Sequence[int] + ) -> float: + assert self.open is not None + ratios = [] + weights = [] + for channel in channels: + span = float(target[channel] - self.open[channel]) + if abs(span) < 0.05: + continue + ratios.append(float((values[channel] - self.open[channel]) / span)) + weights.append(abs(span)) + if not ratios: + return 0.0 + positive = [ratio for ratio in ratios if ratio > 0.0] + if positive: + return max(positive) + return float(np.average(ratios, weights=weights)) + + def _signed_response( + self, values: np.ndarray, target: np.ndarray, channels: Sequence[int] + ) -> float: + return float(np.clip(self._response(values, target, channels), -1.0, 1.0)) + + def _l20_thumb_target(self, values: np.ndarray, spec: RealHandJointSpec) -> float: + assert self.open is not None and self.fist is not None + raw_anchors = [self.open, self.fist] + target_anchors = [0.0, self._closed_target(spec)] + if self.thumb_curl is not None: + raw_anchors.append(self.thumb_curl) + target_anchors.append(self._closed_target(spec)) + + semantic_index = _L20_THUMB_SEMANTICS.index(spec.semantic) + for finger in ("index", "middle", "ring", "pinky"): + pose = self.oposes.get(finger) + targets = _L20_TOUCH_TARGETS.get((self.side, finger)) + if pose is not None and targets is not None: + raw_anchors.append(pose) + target_anchors.append( + float(np.clip(targets[semantic_index], spec.lower, spec.upper)) + ) + + # Only thumb sensors participate. Moving any other finger cannot alter a thumb target. + channels = np.asarray((0, 1, 2, 3, 4), dtype=np.int64) + points = np.asarray(raw_anchors, dtype=np.float64)[:, channels] + query = values[channels] + scale = np.ptp(points, axis=0) + valid = scale >= 0.05 + if not np.any(valid): + return 0.0 + endpoint = self._outside_calibrated_endpoint( + points[:, valid], query[valid], scale[valid] + ) + if endpoint is not None: + return target_anchors[endpoint] + distances = np.sqrt( + np.mean(((points[:, valid] - query[valid]) / scale[valid]) ** 2, axis=1) + ) + nearest = int(np.argmin(distances)) + if distances[nearest] < 1.0e-5: + return target_anchors[nearest] + weights = 1.0 / np.maximum(distances, 1.0e-4) ** 2 + target = float(np.average(np.asarray(target_anchors), weights=weights)) + return float(np.clip(target, spec.lower, spec.upper)) + + @staticmethod + def _outside_calibrated_endpoint( + points: np.ndarray, query: np.ndarray, scale: np.ndarray + ) -> int | None: + """Return an endpoint when the sample continues beyond its calibrated ray.""" + normalized = (points - points[0]) / scale + normalized_query = (query - points[0]) / scale + candidates: list[tuple[float, int]] = [] + for index in range(1, len(normalized)): + direction = normalized[index] + denominator = float(np.dot(direction, direction)) + if denominator < 1.0e-8: + continue + progress = float(np.dot(normalized_query, direction) / denominator) + if progress < 1.0: + continue + perpendicular = normalized_query - progress * direction + distance = float(np.sqrt(np.mean(perpendicular**2))) + if distance <= 0.20: + candidates.append((distance, index)) + return min(candidates)[1] if candidates else None + + +class RealHandFFGGloveRetargeter(BaseRetargeter): + """Map one RealHand FFG Glove ``JointStateSource`` to an L6, O6, or L20.""" + + JOINTS = "joints" + OUTPUT = "hand_joints" + + def __init__(self, config: RealHandFFGGloveRetargeterConfig, name: str) -> None: + if config.side not in ("left", "right"): + raise ValueError(f"side must be left or right, got {config.side!r}") + self._config = config + self._profile = get_realhand_profile(config.hand_model) + expected = set(self._profile.joint_names(config.side)) + if set(config.joint_names) != expected: + raise ValueError( + f"{self._profile.display_name} {config.side} joint names do not match its profile" + ) + self._specs = [ + self._profile.joint_spec(config.side, name) for name in config.joint_names + ] + self._mapper = _NaturalRealHandFFGGloveMapper(config) + self._last = np.zeros(len(config.joint_names), dtype=np.float64) + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return { + self._config.input_device: OptionalType( + TensorGroupType( + self.JOINTS, + [FloatType(name) for name in REALHAND_FFG_GLOVE_SENSOR_NAMES], + ) + ) + } + + def output_spec(self) -> RetargeterIOType: + return { + self.OUTPUT: TensorGroupType( + f"realhand_ffg_glove_{self._profile.model}_{self._config.side}_hand_joints", + [FloatType(name) for name in self._config.joint_names], + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + source = inputs[self._config.input_device] + if source.is_none: + target = ( + self._last.copy() + if self._config.hold_last_on_missing + else np.zeros_like(self._last) + ) + else: + sensors = [ + float(source[index]) + for index in range(len(REALHAND_FFG_GLOVE_SENSOR_NAMES)) + ] + target = self._mapper.map(sensors, self._config.joint_names) + + if context.execution_events.reset: + self._mapper.reset() + self._last = target + else: + target = self._stabilize(target) + alpha = float(np.clip(self._config.smoothing_alpha, 0.0, 1.0)) + self._last = alpha * target + (1.0 - alpha) * self._last + for index, value in enumerate(self._last): + outputs[self.OUTPUT][index] = float(value) + + def _stabilize(self, target: np.ndarray) -> np.ndarray: + target = np.asarray(target, dtype=np.float64).copy() + delta = target - self._last + deadband = max(float(self._config.joint_deadband_rad), 0.0) + target[np.abs(delta) < deadband] = self._last[np.abs(delta) < deadband] + for index, spec in enumerate(self._specs): + closing = abs(target[index]) >= abs(self._last[index]) + limit = ( + self._config.max_close_step_rad + if closing + else self._config.max_open_step_rad + ) + target[index] = self._last[index] + np.clip( + target[index] - self._last[index], -limit, limit + ) + target[index] = np.clip(target[index], spec.lower, spec.upper) + return target + + +__all__ = [ + "RealHandFFGGloveRetargeter", + "RealHandFFGGloveRetargeterConfig", + "REALHAND_FFG_GLOVE_SENSOR_NAMES", + "load_realhand_ffg_glove_calibration", +] diff --git a/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index 6ad14ea6e3..b6f58712c8 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -6,3 +6,7 @@ add_subdirectory(plugin_utils) if(BUILD_PLUGIN_OGLO) add_subdirectory(oglo_tactile) endif() + +if(BUILD_PLUGIN_REALHAND_FFG_GLOVE) + add_subdirectory(realhand_ffg_glove) +endif() diff --git a/tests/cpp/plugins/realhand_ffg_glove/CMakeLists.txt b/tests/cpp/plugins/realhand_ffg_glove/CMakeLists.txt new file mode 100644 index 0000000000..f667faf084 --- /dev/null +++ b/tests/cpp/plugins/realhand_ffg_glove/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(test_realhand_ffg_glove_protocol + test_realhand_ffg_glove_protocol.cpp + ${CMAKE_SOURCE_DIR}/src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.cpp +) +target_include_directories(test_realhand_ffg_glove_protocol PRIVATE + ${CMAKE_SOURCE_DIR}/src/plugins/realhand_ffg_glove +) +add_test(NAME realhand_ffg_glove_protocol COMMAND test_realhand_ffg_glove_protocol) diff --git a/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_protocol.cpp b/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_protocol.cpp new file mode 100644 index 0000000000..e682d90fc3 --- /dev/null +++ b/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_protocol.cpp @@ -0,0 +1,125 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "realhand_ffg_glove_protocol.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::realhand_ffg_glove; + +namespace +{ + +int g_checks = 0; + +void check(bool condition, const char* message) +{ + ++g_checks; + if (!condition) + { + std::fprintf(stderr, "FAIL: %s\n", message); + std::abort(); + } +} + +template +void append_little_endian(std::vector& bytes, T value) +{ + std::array encoded{}; + std::memcpy(encoded.data(), &value, sizeof(T)); + bytes.insert(bytes.end(), encoded.begin(), encoded.end()); +} + +std::optional parse_all(const std::vector& bytes) +{ + FrameParser parser; + std::optional result; + for (uint8_t byte : bytes) + { + if (auto frame = parser.process(byte)) + { + result = std::move(frame); + } + } + return result; +} + +void test_pack_and_parse() +{ + const auto encoded = pack_frame(Command::Position, { 1, 2, 3 }); + check(encoded == std::vector({ 0x5D, 0x03, 0x03, 0x01, 0x02, 0x03, 0x69 }), "packed frame"); + const auto frame = parse_all(encoded); + check(frame.has_value(), "parse packed frame"); + check(frame->command == Command::Position, "parsed command"); + check(frame->payload == std::vector({ 1, 2, 3 }), "parsed payload"); +} + +void test_bad_checksum_and_resync() +{ + auto bad = pack_frame(Command::Version); + bad.back() ^= 1; + // Decimal 20104 is formatted by the SDK as major=2, minor=01, patch=04. + const auto good = pack_frame(Command::Version, { 0x88, 0x4E, 0x00, 0x00, 0x01 }); + bad.insert(bad.end(), good.begin(), good.end()); + const auto frame = parse_all(bad); + check(frame.has_value(), "parser resynchronizes after bad checksum"); + const auto version = decode_version(*frame); + check(version.has_value(), "decode version"); + check(version->version == "2.1.4", "version formatting matches Python SDK"); + check(version->side == HandSide::Right, "right side decode"); +} + +void test_float_positions() +{ + std::vector payload; + for (std::size_t i = 0; i < kNumSensors; ++i) + { + append_little_endian(payload, static_cast(i * 10)); + } + const auto frame = parse_all(pack_frame(Command::Position, payload)); + check(frame.has_value(), "parse float position frame"); + const auto positions = decode_positions(*frame); + check(positions.has_value(), "decode 21 float positions"); + check(std::abs((*positions)[9] - 1.5707963268f) < 1e-5f, "degrees converted to radians"); + + payload.resize(payload.size() - sizeof(float)); + const auto short_frame = parse_all(pack_frame(Command::Position, payload)); + check(!decode_positions(*short_frame).has_value(), "reject wrong float sensor count"); +} + +void test_a6_positions() +{ + std::vector payload; + for (std::size_t i = 0; i < kNumSensors; ++i) + { + append_little_endian(payload, static_cast(i == 0 ? -9000 : 0)); + } + const auto frame = parse_all(pack_frame(Command::A6Position, payload)); + const auto positions = decode_positions(*frame); + check(positions.has_value(), "decode A6 int16 positions"); + check(std::abs((*positions)[0] + 1.5707963268f) < 1e-5f, "A6 hundredth-degrees converted to radians"); + + const auto response = pack_frame(Command::A7Force, std::vector(5 * sizeof(float), 0)); + const auto response_frame = parse_all(response); + check(response_frame.has_value(), "parse A7 continuation frame"); + check(response_frame->command == Command::A7Force, "A7 continuation command"); + check(response_frame->payload.size() == 5 * sizeof(float), "A7 continuation has five force channels"); +} + +} // namespace + +int main() +{ + test_pack_and_parse(); + test_bad_checksum_and_resync(); + test_float_positions(); + test_a6_positions(); + std::printf("OK: all %d checks passed\n", g_checks); + return 0; +} diff --git a/tests/python/core/retargeting_engine/conftest.py b/tests/python/core/retargeting_engine/conftest.py index 1b41a3a31d..63674cbd29 100644 --- a/tests/python/core/retargeting_engine/conftest.py +++ b/tests/python/core/retargeting_engine/conftest.py @@ -1,10 +1,44 @@ # SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. # SPDX-License-Identifier: Apache-2.0 """Pytest configuration and fixtures for isaaccapture.retargeting_engine tests.""" import pytest import numpy as np +import yaml + + +@pytest.fixture +def realhand_calibration_dir(tmp_path): + """Create deterministic synthetic FFG calibration files for RealHand tests.""" + calibration_dir = tmp_path / "realhand_ffg_glove" + calibration_dir.mkdir() + opened = np.zeros(21, dtype=np.float64) + fist = np.linspace(0.8, 1.2, 21, dtype=np.float64) + thumb_curl = opened.copy() + thumb_curl[:5] = (0.25, 0.55, 0.85, 1.05, 0.70) + touch_anchors = { + "index": (0.30, 0.45, 0.60, 0.75, 0.90), + "middle": (0.45, 0.60, 0.75, 0.90, 0.55), + "ring": (0.60, 0.75, 0.90, 0.55, 0.70), + "pinky": (0.75, 0.90, 0.55, 0.70, 0.85), + } + + for model in ("l6", "o6", "l20"): + data = {"model": model, "format": "realhand-ffg-glove-raw-calibration-v2"} + for suffix in ("l", "r"): + data[f"jointangleoriginal_{suffix}"] = opened.tolist() + data[f"jointanglefist_{suffix}"] = fist.tolist() + data[f"jointanglethumb_curl_{suffix}"] = thumb_curl.tolist() + for finger, values in touch_anchors.items(): + pose = opened.copy() + pose[:5] = values + data[f"jointangleopose_{finger}_{suffix}"] = pose.tolist() + with (calibration_dir / f"{model}.yml").open("w", encoding="utf-8") as stream: + yaml.safe_dump(data, stream, sort_keys=False) + + return calibration_dir @pytest.fixture diff --git a/tests/python/core/retargeting_engine/pyproject.toml b/tests/python/core/retargeting_engine/pyproject.toml index e4bf13ac68..c1056ccfc2 100644 --- a/tests/python/core/retargeting_engine/pyproject.toml +++ b/tests/python/core/retargeting_engine/pyproject.toml @@ -12,6 +12,7 @@ dev = [ "pytest", "mypy", "numpy", + "pyyaml", "scipy", "types-PyYAML", "scipy-stubs", @@ -19,6 +20,7 @@ dev = [ test = [ "pytest", "numpy", + "pyyaml", "scipy", ] # Mirrors src/core/python/requirements-grounding.txt. The Sharpa unit test diff --git a/tests/python/core/retargeting_engine/test_realhand_asset_fetcher.py b/tests/python/core/retargeting_engine/test_realhand_asset_fetcher.py new file mode 100644 index 0000000000..1bde58d9ae --- /dev/null +++ b/tests/python/core/retargeting_engine/test_realhand_asset_fetcher.py @@ -0,0 +1,103 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Checks for the pinned RealHand robot-asset fetcher.""" + +import hashlib +import importlib.util +import sys +from pathlib import Path + +import pytest + +_REPO_ROOT = Path(__file__).resolve().parents[4] +_FETCHER_PATH = ( + _REPO_ROOT + / "examples" + / "teleop" + / "python" + / "scripts" + / "fetch_realhand_assets.py" +) +_SPEC = importlib.util.spec_from_file_location("fetch_realhand_assets", _FETCHER_PATH) +assert _SPEC is not None and _SPEC.loader is not None +_FETCHER = importlib.util.module_from_spec(_SPEC) +sys.modules[_SPEC.name] = _FETCHER +_SPEC.loader.exec_module(_FETCHER) + + +def _sibling(path: str, size: int = 4, sha256: str | None = None) -> dict: + sibling = {"rfilename": path, "size": size} + if sha256 is not None: + sibling["lfs"] = {"sha256": sha256, "size": size} + return sibling + + +def test_selected_files_include_only_requested_model_and_licenses(): + root = _FETCHER.REMOTE_ASSET_ROOT + metadata = { + "siblings": [ + *[_sibling(path) for path in sorted(_FETCHER.LICENSE_FILES)], + _sibling(f"{root}/p7_l6/P7_l6_bimanual.urdf"), + _sibling(f"{root}/p7_l6/meshes/link.stl", sha256="a" * 64), + _sibling(f"{root}/p7_l20/P7_L20_bimanual.urdf"), + _sibling("examples/isaac_lab/calibrations/operator.yml"), + _sibling("hardware/controller_mount.3mf"), + ] + } + + selected = _FETCHER._selected_files(metadata, {"l6"}) + selected_paths = {item.path for item in selected} + + assert _FETCHER.LICENSE_FILES <= selected_paths + assert f"{root}/p7_l6/P7_l6_bimanual.urdf" in selected_paths + assert f"{root}/p7_l6/meshes/link.stl" in selected_paths + assert not any("p7_l20" in path for path in selected_paths) + assert not any(path.endswith((".yml", ".3mf")) for path in selected_paths) + + +def test_repository_metadata_rejects_a_moved_revision(monkeypatch): + monkeypatch.setattr(_FETCHER, "_request_json", lambda _url: {"sha": "0" * 40}) + + with pytest.raises(RuntimeError, match="refusing an unpinned download"): + _FETCHER._repository_metadata() + + +@pytest.mark.parametrize("path", ("../secret", "/absolute/path")) +def test_remote_path_rejects_traversal(path): + with pytest.raises(ValueError, match="Unsafe repository path"): + _FETCHER._normalize_remote_path(path) + + +def test_local_paths_preserve_models_and_license_layout(tmp_path): + root = _FETCHER.REMOTE_ASSET_ROOT + + model = _FETCHER.RemoteFile(f"{root}/p7_o6/mesh.stl", 1, None) + license_file = _FETCHER.RemoteFile("LICENSES/Apache-2.0.txt", 1, None) + + assert _FETCHER._local_path(model, tmp_path) == tmp_path / "p7_o6" / "mesh.stl" + assert _FETCHER._local_path(license_file, tmp_path) == ( + tmp_path / "LICENSES" / "Apache-2.0.txt" + ) + + +def test_validate_model_checks_urdf_hash_and_meshes(tmp_path, monkeypatch): + asset_dir = tmp_path / "p7_l6" + mesh_path = asset_dir / "meshes" / "link.stl" + mesh_path.parent.mkdir(parents=True) + mesh_path.write_bytes(b"mesh") + urdf_path = asset_dir / "P7_l6_bimanual.urdf" + urdf_path.write_text( + '' + '' + "", + encoding="utf-8", + ) + expected_hash = hashlib.sha256(urdf_path.read_bytes()).hexdigest() + monkeypatch.setitem(_FETCHER.URDF_SHA256, "l6", expected_hash) + + assert _FETCHER._validate_model(tmp_path, "l6") == urdf_path + + mesh_path.unlink() + with pytest.raises(FileNotFoundError, match="references missing meshes"): + _FETCHER._validate_model(tmp_path, "l6") diff --git a/tests/python/core/retargeting_engine/test_realhand_example.py b/tests/python/core/retargeting_engine/test_realhand_example.py new file mode 100644 index 0000000000..2ce0d0bca7 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_realhand_example.py @@ -0,0 +1,61 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Graph-construction checks for every supported P7 and RealHand combination.""" + +import importlib.util +from pathlib import Path + +import pytest + +_REPO_ROOT = Path(__file__).resolve().parents[4] +_EXAMPLE_PATH = ( + _REPO_ROOT / "examples" / "teleop" / "python" / "p7_realhand_bimanual_example.py" +) +_SPEC = importlib.util.spec_from_file_location( + "p7_realhand_bimanual_example", _EXAMPLE_PATH +) +assert _SPEC is not None and _SPEC.loader is not None +_EXAMPLE = importlib.util.module_from_spec(_SPEC) +_SPEC.loader.exec_module(_EXAMPLE) + + +@pytest.mark.parametrize( + ("model", "expected_width"), (("l6", 36), ("o6", 26), ("l20", 46)) +) +@pytest.mark.parametrize("mode", ("controller", "handtracking", "ffg")) +def test_pipeline_builds_for_every_input_mode( + mode, model, expected_width, realhand_calibration_dir +): + calibration = realhand_calibration_dir / f"{model}.yml" + pipeline, action_order = _EXAMPLE.build_pipeline(mode, model, calibration) + + assert len(action_order) == expected_width + assert list(pipeline.output_types()) == ["action"] + assert pipeline.output_types()["action"].types[0].shape == (expected_width,) + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +def test_ffg_pipeline_requires_user_calibration(model): + with pytest.raises(ValueError, match="requires a calibration YAML"): + _EXAMPLE.build_pipeline("ffg", model, None) + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +@pytest.mark.parametrize("side", ("left", "right")) +def test_home_pose_has_hand_back_forward_and_fingers_down(model, side): + rotation = _EXAMPLE.Rotation.from_quat(_EXAMPLE._HOME_POSES[model][side][1]) + + hand_back = rotation.apply((-1.0, 0.0, 0.0)) + finger_direction = rotation.apply((0.0, 0.0, 1.0)) + + assert hand_back[1] > 0.99 + assert finger_direction[2] < -0.99 + + +def test_each_handtracking_model_uses_its_own_home_rotation(): + for side in ("left", "right"): + actual = _EXAMPLE._pose_config("l20", side, "handtracking") + expected = _EXAMPLE._home_rpy("l20", side) + + assert actual.rotation_offset_rpy_deg == pytest.approx(expected) diff --git a/tests/python/core/retargeting_engine/test_realhand_retargeters.py b/tests/python/core/retargeting_engine/test_realhand_retargeters.py new file mode 100644 index 0000000000..2d4555ebfe --- /dev/null +++ b/tests/python/core/retargeting_engine/test_realhand_retargeters.py @@ -0,0 +1,424 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Simulation-free checks for P7 and RealHand retargeters.""" + +import numpy as np +import pytest + +from isaaccapture.retargeters.realhand.arm import ( + P7ControllerPoseRetargeter, + P7WorkspacePoseConfig, +) +from isaaccapture.retargeters.realhand.realhand_ffg_glove import ( + RealHandFFGGloveRetargeterConfig, + _NaturalRealHandFFGGloveMapper, + load_realhand_ffg_glove_calibration, +) +from isaaccapture.retargeters.realhand.hand import ( + ControllerTriggerRealHandRetargeter, + ControllerTriggerRealHandRetargeterConfig, + RealHandHandTrackingRetargeter, + RealHandHandTrackingRetargeterConfig, +) +from isaaccapture.retargeters.realhand.profiles import get_realhand_profile +from isaaccapture.retargeting_engine.interface import ( + ComputeContext, + ExecutionEvents, + ExecutionState, + OptionalTensorGroup, + TensorGroup, +) +from isaaccapture.retargeting_engine.interface.retargeter_core_types import GraphTime +from isaaccapture.retargeting_engine.interface.tensor_group_type import ( + OptionalTensorGroupType, +) +from isaaccapture.retargeting_engine.tensor_types import ( + ControllerInputIndex, + HandInputIndex, + HandJointIndex, +) + +_DIGIT_SENSOR_CHANNELS = { + "thumb": (0, 1, 2, 3, 4), + "index": (5, 6, 8), + "middle": (9, 10, 12), + "ring": (13, 14, 16), + "pinky": (17, 18, 20), +} + + +def _context(*, reset: bool = False) -> ComputeContext: + return ComputeContext( + graph_time=GraphTime(sim_time_ns=0, real_time_ns=0), + execution_events=ExecutionEvents( + reset=reset, execution_state=ExecutionState.RUNNING + ), + ) + + +def _build_io(retargeter): + def make_group(group_type): + if isinstance(group_type, OptionalTensorGroupType): + return OptionalTensorGroup(group_type) + return TensorGroup(group_type) + + return ( + {name: make_group(spec) for name, spec in retargeter.input_spec().items()}, + {name: make_group(spec) for name, spec in retargeter.output_spec().items()}, + ) + + +def _fill_controller(group, *, position=(0.0, 0.0, 0.0), trigger=0.0, squeeze=0.0): + group[ControllerInputIndex.GRIP_POSITION] = np.asarray(position, dtype=np.float32) + group[ControllerInputIndex.GRIP_ORIENTATION] = np.asarray( + (0.0, 0.0, 0.0, 1.0), dtype=np.float32 + ) + group[ControllerInputIndex.GRIP_IS_VALID] = True + group[ControllerInputIndex.TRIGGER_VALUE] = trigger + group[ControllerInputIndex.SQUEEZE_VALUE] = squeeze + + +_FINGER_INDICES = { + "index": ( + HandJointIndex.INDEX_METACARPAL, + HandJointIndex.INDEX_PROXIMAL, + HandJointIndex.INDEX_INTERMEDIATE, + HandJointIndex.INDEX_DISTAL, + HandJointIndex.INDEX_TIP, + ), + "middle": ( + HandJointIndex.MIDDLE_METACARPAL, + HandJointIndex.MIDDLE_PROXIMAL, + HandJointIndex.MIDDLE_INTERMEDIATE, + HandJointIndex.MIDDLE_DISTAL, + HandJointIndex.MIDDLE_TIP, + ), + "ring": ( + HandJointIndex.RING_METACARPAL, + HandJointIndex.RING_PROXIMAL, + HandJointIndex.RING_INTERMEDIATE, + HandJointIndex.RING_DISTAL, + HandJointIndex.RING_TIP, + ), + "pinky": ( + HandJointIndex.LITTLE_METACARPAL, + HandJointIndex.LITTLE_PROXIMAL, + HandJointIndex.LITTLE_INTERMEDIATE, + HandJointIndex.LITTLE_DISTAL, + HandJointIndex.LITTLE_TIP, + ), +} + + +def _direction(flexion: float) -> np.ndarray: + return np.asarray((0.0, np.cos(flexion), -np.sin(flexion))) + + +def _synthetic_hand(close: float, *, mirror_x: bool) -> np.ndarray: + positions = np.zeros((26, 3), dtype=np.float32) + positions[HandJointIndex.PALM] = (0.0, 0.04, 0.0) + bases = { + "index": (0.031, 0.031, 0.0), + "middle": (0.010, 0.036, 0.0), + "ring": (-0.012, 0.033, 0.0), + "pinky": (-0.034, 0.026, 0.0), + } + lengths = (0.026, 0.035, 0.025, 0.019) + for finger, indices in _FINGER_INDICES.items(): + point = np.asarray(bases[finger], dtype=np.float64) + positions[indices[0]] = point + for segment, (index, length) in enumerate(zip(indices[1:], lengths)): + point = point + length * _direction(close * (1.15 + 0.65 * segment)) + positions[index] = point + + thumb_open = np.asarray( + ( + (0.045, 0.020, -0.002), + (0.065, 0.030, -0.001), + (0.083, 0.038, 0.000), + (0.099, 0.044, 0.001), + ) + ) + index_tip = positions[HandJointIndex.INDEX_TIP] + thumb_closed = np.asarray( + ( + thumb_open[0], + 0.70 * thumb_open[0] + 0.30 * index_tip + (0.008, -0.004, 0.014), + 0.35 * thumb_open[0] + 0.65 * index_tip + (0.004, -0.009, 0.010), + index_tip + (0.002, -0.001, 0.001), + ) + ) + thumb = (1.0 - close) * thumb_open + close * thumb_closed + for index, point in zip( + ( + HandJointIndex.THUMB_METACARPAL, + HandJointIndex.THUMB_PROXIMAL, + HandJointIndex.THUMB_DISTAL, + HandJointIndex.THUMB_TIP, + ), + thumb, + ): + positions[index] = point + if mirror_x: + positions[:, 0] *= -1.0 + return positions + + +def _fill_hand(group, positions: np.ndarray) -> None: + group[HandInputIndex.JOINT_POSITIONS] = positions + group[HandInputIndex.JOINT_ORIENTATIONS] = np.tile( + np.asarray((0.0, 0.0, 0.0, 1.0), dtype=np.float32), (26, 1) + ) + group[HandInputIndex.JOINT_RADII] = np.full(26, 0.008, dtype=np.float32) + group[HandInputIndex.JOINT_VALID] = np.ones(26, dtype=np.uint8) + + +def _mapper(model: str, calibration_dir, side: str = "left"): + profile = get_realhand_profile(model) + config = RealHandFFGGloveRetargeterConfig( + input_device="glove", + joint_names=profile.joint_names(side), + side=side, + hand_model=model, + calibration_path=str(calibration_dir / f"{model}.yml"), + ) + return profile, _NaturalRealHandFFGGloveMapper(config) + + +@pytest.mark.parametrize("model", ["l6", "o6", "l20"]) +def test_profile_resolves_downloaded_urdf(model, tmp_path): + profile = get_realhand_profile(model) + urdf = tmp_path / profile.asset_dir_name / profile.urdf_name + urdf.parent.mkdir(parents=True) + urdf.touch() + + assert profile.resolve_urdf(tmp_path) == urdf.resolve() + + +def test_profile_reports_asset_fetch_command_when_urdf_is_missing(tmp_path): + with pytest.raises(FileNotFoundError, match=r"fetch_realhand_assets\.py"): + get_realhand_profile("l6").resolve_urdf(tmp_path) + + +@pytest.mark.parametrize("model", ["l6", "o6", "l20"]) +@pytest.mark.parametrize("side", ["left", "right"]) +def test_open_and_fist_cover_joint_ranges(model, side, realhand_calibration_dir): + profile, mapper = _mapper(model, realhand_calibration_dir, side) + names = profile.joint_names(side) + calibration = load_realhand_ffg_glove_calibration( + realhand_calibration_dir / f"{model}.yml", side + ) + opened = mapper.map(calibration["open"], names) + closed = mapper.map(calibration["fist"], names) + + assert np.all(np.isfinite(opened)) + assert np.all(np.isfinite(closed)) + assert np.max(np.abs(opened)) < 1.0e-8 + for index, spec in enumerate(profile.joints(side)): + if not spec.semantic.endswith("_spread"): + assert closed[index] >= 0.85 * min(spec.trigger_closed, spec.upper) + + +@pytest.mark.parametrize("model", ["l6", "o6", "l20"]) +@pytest.mark.parametrize("side", ["left", "right"]) +def test_motion_beyond_fist_does_not_reopen_flexion_joints( + model, side, realhand_calibration_dir +): + profile, mapper = _mapper(model, realhand_calibration_dir, side) + names = profile.joint_names(side) + calibration = load_realhand_ffg_glove_calibration( + realhand_calibration_dir / f"{model}.yml", side + ) + opened = np.asarray(calibration["open"], dtype=np.float64) + fist = np.asarray(calibration["fist"], dtype=np.float64) + beyond_fist = opened + 1.5 * (fist - opened) + + closed = mapper.map(fist, names) + beyond = mapper.map(beyond_fist, names) + flexion = [ + index + for index, spec in enumerate(profile.joints(side)) + if not spec.semantic.endswith("_spread") + ] + assert np.all(beyond[flexion] >= closed[flexion] - 1.0e-8) + + +@pytest.mark.parametrize("model", ["l6", "o6", "l20"]) +@pytest.mark.parametrize("side", ["left", "right"]) +@pytest.mark.parametrize("digit", ["thumb", "index", "middle", "ring", "pinky"]) +def test_each_digit_ignores_other_digit_sensors( + model, side, digit, realhand_calibration_dir +): + profile, mapper = _mapper(model, realhand_calibration_dir, side) + names = profile.joint_names(side) + calibration = load_realhand_ffg_glove_calibration( + realhand_calibration_dir / f"{model}.yml", side + ) + baseline = np.asarray(calibration["open"], dtype=np.float64) + changed = baseline.copy() + for other_digit, channels in _DIGIT_SENSOR_CHANNELS.items(): + if other_digit != digit: + changed[list(channels)] += 0.75 + + baseline_target = mapper.map(baseline, names) + changed_target = mapper.map(changed, names) + own_joints = [ + index + for index, spec in enumerate(profile.joints(side)) + if spec.semantic.startswith(digit) + ] + np.testing.assert_allclose( + changed_target[own_joints], baseline_target[own_joints], atol=1.0e-12 + ) + + +@pytest.mark.parametrize("side", ["left", "right"]) +def test_l20_thumb_is_independent_of_non_thumb_sensors(side, realhand_calibration_dir): + profile, mapper = _mapper("l20", realhand_calibration_dir, side) + names = profile.joint_names(side) + calibration = load_realhand_ffg_glove_calibration( + realhand_calibration_dir / "l20.yml", side + ) + baseline = np.asarray(calibration["open"], dtype=np.float64) + changed = baseline.copy() + changed[5:] += np.linspace(-1.0, 1.0, len(changed) - 5) + + baseline_target = mapper.map(baseline, names) + changed_target = mapper.map(changed, names) + thumb = [ + index + for index, spec in enumerate(profile.joints(side)) + if spec.semantic.startswith("thumb") + ] + np.testing.assert_allclose( + changed_target[thumb], baseline_target[thumb], atol=1.0e-12 + ) + + +@pytest.mark.parametrize("side", ["left", "right"]) +def test_l20_each_finger_ignores_other_finger_sensors(side, realhand_calibration_dir): + profile, mapper = _mapper("l20", realhand_calibration_dir, side) + names = profile.joint_names(side) + calibration = load_realhand_ffg_glove_calibration( + realhand_calibration_dir / "l20.yml", side + ) + baseline = np.asarray(calibration["open"], dtype=np.float64) + changed = baseline.copy() + changed[14] += 1.0 + changed[16] += 1.0 + + baseline_target = mapper.map(baseline, names) + changed_target = mapper.map(changed, names) + index_joints = [ + index + for index, spec in enumerate(profile.joints(side)) + if spec.semantic.startswith("index_") + ] + np.testing.assert_allclose( + changed_target[index_joints], baseline_target[index_joints], atol=1.0e-12 + ) + + +@pytest.mark.parametrize("side", ["left", "right"]) +@pytest.mark.parametrize("finger", ["index", "middle", "ring", "pinky"]) +def test_l20_thumb_calibration_pose_is_an_exact_curve_anchor( + side, finger, realhand_calibration_dir +): + profile, mapper = _mapper("l20", realhand_calibration_dir, side) + names = profile.joint_names(side) + calibration = load_realhand_ffg_glove_calibration( + realhand_calibration_dir / "l20.yml", side + ) + pose = calibration["oposes"][finger] + target = mapper.map(pose, names) + assert np.all(np.isfinite(target)) + assert np.linalg.norm(target[:4]) > 0.2 + + +def test_profiles_have_unique_action_joint_names(): + for model in ("l6", "o6", "l20"): + profile = get_realhand_profile(model) + assert len(profile.action_joint_names) == len(set(profile.action_joint_names)) + + +def test_p7_controller_pose_maps_absolute_workspace_and_clamps(): + retargeter = P7ControllerPoseRetargeter( + P7WorkspacePoseConfig( + input_device="controller_left", + fallback_position=(0.0, 0.0, 0.0), + fallback_rotation=(0.0, 0.0, 0.0, 1.0), + input_center=(0.0, 0.0, 0.0), + workspace_center=(1.0, 2.0, 3.0), + position_scale=(2.0, 2.0, 2.0), + max_delta=(0.25, 0.25, 0.25), + ), + name="p7_left", + ) + inputs, outputs = _build_io(retargeter) + _fill_controller(inputs["controller_left"], position=(0.1, -0.2, 0.5)) + retargeter.compute(inputs, outputs, _context(reset=True)) + + pose = np.from_dlpack(outputs["ee_pose"][0]) + np.testing.assert_allclose(pose[:3], (1.2, 1.75, 3.25), atol=1.0e-6) + np.testing.assert_allclose(pose[3:], (0.0, 0.0, 0.0, 1.0), atol=1.0e-6) + + +@pytest.mark.parametrize("model", ["l6", "o6", "l20"]) +@pytest.mark.parametrize("side", ["left", "right"]) +def test_controller_trigger_closes_only_configured_hand(model, side): + profile = get_realhand_profile(model) + joint_names = profile.joint_names(side) + input_name = f"controller_{side}" + retargeter = ControllerTriggerRealHandRetargeter( + ControllerTriggerRealHandRetargeterConfig( + input_device=input_name, + joint_names=joint_names, + side=side, + hand_model=model, + smoothing_alpha=1.0, + ), + name=f"{model}_{side}", + ) + inputs, outputs = _build_io(retargeter) + _fill_controller(inputs[input_name], trigger=1.0) + retargeter.compute(inputs, outputs, _context(reset=False)) + + actual = np.asarray([float(value) for value in outputs["hand_joints"]]) + expected = np.asarray([joint.trigger_closed for joint in profile.joints(side)]) + np.testing.assert_allclose(actual, expected, atol=1.0e-6) + + +@pytest.mark.parametrize("model", ["l6", "o6", "l20"]) +@pytest.mark.parametrize("side", ["left", "right"]) +def test_handtracking_open_to_fist_drives_every_digit(model, side): + profile = get_realhand_profile(model) + input_name = f"hand_{side}" + retargeter = RealHandHandTrackingRetargeter( + RealHandHandTrackingRetargeterConfig( + input_device=input_name, + joint_names=profile.joint_names(side), + side=side, + hand_model=model, + smoothing_alpha=1.0, + ), + name=f"{model}_{side}_tracking", + ) + inputs, outputs = _build_io(retargeter) + _fill_hand(inputs[input_name], _synthetic_hand(0.0, mirror_x=side == "right")) + retargeter.compute(inputs, outputs, _context(reset=True)) + opened = np.asarray([float(value) for value in outputs["hand_joints"]]) + + _fill_hand(inputs[input_name], _synthetic_hand(1.0, mirror_x=side == "right")) + retargeter.compute(inputs, outputs, _context()) + closed = np.asarray([float(value) for value in outputs["hand_joints"]]) + + np.testing.assert_allclose(opened, 0.0, atol=1.0e-6) + for digit in ("thumb", "index", "middle", "ring", "pinky"): + indices = [ + index + for index, spec in enumerate(profile.joints(side)) + if spec.semantic.startswith(digit) and not spec.semantic.endswith("_spread") + ] + assert indices + assert np.max(closed[indices]) > 0.20