From 971d6ee6307f032b1110a8316bd876ad543af7ac Mon Sep 17 00:00:00 2001 From: Huadong Zhang Date: Thu, 17 Sep 2026 14:28:30 -0700 Subject: [PATCH 1/2] 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/hand_tracking/index.rst | 1 + docs/source/device/realhand_ffg_glove.rst | 119 +++++ 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 521ec03c30..5de9977a2c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,6 +71,13 @@ option(BUILD_PLUGIN_NOITOM_MOCAP "Build Noitom mocap plugin (downloads MocapApi 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) option(BUILD_PLUGIN_SHARPA_AVATAR "Build Sharpa AvatarExo Glove plugin (Linux; skips if the Avatar SDK is missing)" 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 92facf6c24..91ffc75d1e 100644 --- a/docs/source/_data/devices.yaml +++ b/docs/source/_data/devices.yaml @@ -225,6 +225,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/hand_tracking/index.rst b/docs/source/device/hand_tracking/index.rst index e792568c1c..a91aebe020 100644 --- a/docs/source/device/hand_tracking/index.rst +++ b/docs/source/device/hand_tracking/index.rst @@ -15,3 +15,4 @@ and hand exoskeletons. See :doc:`/device/trackers/index` for the common tracking wuji_glove sharpa_avatar haptikos + /device/realhand_ffg_glove 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/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 935996d6a0..c3744f51fd 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -26,6 +26,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 @@ -381,3 +389,4 @@ See the :doc:`Contributing Guide <../../development/contributing>` for details. 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 4e5fa231f1..bed77f529d 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -43,3 +43,6 @@ endif() if(BUILD_PLUGIN_SHARPA_AVATAR) add_subdirectory(sharpa_avatar) 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 518fb9b2bf..93a45dba8f 100644 --- a/src/python/isaaccapture/retargeters/__init__.py +++ b/src/python/isaaccapture/retargeters/__init__.py @@ -238,6 +238,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", @@ -410,6 +456,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 46847194f9..8010b15d54 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -16,3 +16,7 @@ endif() 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 f3f24c3c62..8147188c08 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 From 2c1ba4ed92004aabb1a737579de9e324930029ca Mon Sep 17 00:00:00 2001 From: Huadong Zhang Date: Mon, 2 Nov 2026 17:41:48 -0800 Subject: [PATCH 2/2] RealHand plugin: OpenXR glove output and official hand retargeting Publish calibrated 26-joint OpenXR hand skeletons from RealHand FFG gloves and use the official DexHandRetargeter for L6, O6, and L20. Unify calibration across all three hands and update examples, documentation, and tests for the current Isaac Capture structure. Signed-off-by: Huadong Zhang --- docs/source/_data/devices.yaml | 4 +- docs/source/device/realhand_ffg_glove.rst | 117 ++- docs/source/references/retargeting/index.rst | 7 +- .../references/retargeting/realhand.rst | 76 +- examples/teleop/README.md | 7 + .../{python => }/assets/realhand/.gitignore | 0 .../{python => }/assets/realhand/README.md | 14 +- .../teleop/config/realhand/README.md | 57 ++ .../teleop/config/realhand/l20/left.urdf | 212 +++++ .../teleop/config/realhand/l20/left.yml | 134 ++++ .../teleop/config/realhand/l20/right.urdf | 212 +++++ .../teleop/config/realhand/l20/right.yml | 134 ++++ .../teleop/config/realhand/l6/left.urdf | 127 +++ .../teleop/config/realhand/l6/left.yml | 105 +++ .../teleop/config/realhand/l6/right.urdf | 127 +++ .../teleop/config/realhand/l6/right.yml | 105 +++ .../teleop/config/realhand/o6/left.urdf | 132 +++ .../teleop/config/realhand/o6/left.yml | 100 +++ .../teleop/config/realhand/o6/right.urdf | 132 +++ .../teleop/config/realhand/o6/right.yml | 100 +++ .../teleop}/fetch_realhand_assets.py | 40 +- .../teleop}/p7_realhand_bimanual_example.py | 138 ++-- .../teleop/realhand_ffg_diagnostics.py | 216 +++++ .../teleop/realhand_ffg_glove_calibration.py | 382 +++++++++ .../teleop/realhand_ffg_hand_demo.env | 7 + .../teleop/realhand_ffg_hand_demo.py | 479 +++++++++++ .../teleop/realhand_ffg_hand_demo_bridge.py | 502 ++++++++++++ .../teleop/realhand_ffg_robot_scene.py | 112 +++ .../teleop/realhand_ffg_thumb_calibration.py | 424 ++++++++++ .../python/realhand_ffg_glove_calibration.py | 205 ----- examples/teleop/realhand_ffg_hand_demo.md | 116 +++ examples/teleop/realhand_ffg_robot_scenes.md | 91 +++ examples/teleop/run_realhand_ffg_hand_demo.sh | 30 + pyproject.toml | 1 + src/core/python/requirements-retargeters.txt | 2 + src/plugins/realhand_ffg_glove/CMakeLists.txt | 4 + src/plugins/realhand_ffg_glove/README.md | 60 +- src/plugins/realhand_ffg_glove/main.cpp | 79 +- src/plugins/realhand_ffg_glove/plugin.yaml | 12 +- .../realhand_ffg_glove_plugin.cpp | 260 +++--- .../realhand_ffg_glove_plugin.hpp | 51 +- .../realhand_ffg_glove_reader.cpp | 210 +++++ .../realhand_ffg_glove_reader.hpp | 63 ++ .../realhand_ffg_glove_serial.cpp | 118 ++- .../realhand_ffg_glove_serial.hpp | 1 - .../realhand_ffg_glove_skeleton.cpp | 398 ++++++++++ .../realhand_ffg_glove_skeleton.hpp | 65 ++ .../isaaccapture/retargeters/__init__.py | 24 - .../retargeters/realhand/__init__.py | 78 +- .../isaaccapture/retargeters/realhand/hand.py | 384 +-------- .../retargeters/realhand/profiles.py | 95 +-- .../realhand/realhand_ffg_glove.py | 425 ---------- .../plugins/realhand_ffg_glove/CMakeLists.txt | 6 + .../test_realhand_ffg_glove_protocol.cpp | 28 +- .../test_realhand_ffg_glove_reader.cpp | 198 +++++ .../test_realhand_ffg_glove_skeleton.cpp | 522 ++++++++++++ tests/python/CMakeLists.txt | 2 +- .../core/retargeting_engine/conftest.py | 34 - .../test_realhand_example.py | 61 -- .../test_realhand_retargeters.py | 326 +------- tests/python/examples/CMakeLists.txt | 13 +- tests/python/examples/realhand/CMakeLists.txt | 21 + tests/python/examples/realhand/conftest.py | 68 ++ tests/python/examples/realhand/pyproject.toml | 28 + .../realhand}/test_realhand_asset_fetcher.py | 61 +- .../realhand/test_realhand_dex_config.py | 367 +++++++++ .../realhand/test_realhand_example.py | 163 ++++ .../realhand/test_realhand_ffg_diagnostics.py | 72 ++ .../test_realhand_ffg_glove_calibration.py | 247 ++++++ .../test_realhand_ffg_glove_hand_input.py | 111 +++ .../realhand/test_realhand_ffg_hand_demo.py | 749 ++++++++++++++++++ .../realhand/test_realhand_ffg_robot_scene.py | 180 +++++ .../test_realhand_module_entrypoints.py | 67 ++ 73 files changed, 8407 insertions(+), 1891 deletions(-) rename examples/teleop/{python => }/assets/realhand/.gitignore (100%) rename examples/teleop/{python => }/assets/realhand/README.md (56%) create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/README.md create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/left.urdf create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/left.yml create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/right.urdf create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/right.yml create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/left.urdf create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/left.yml create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/right.urdf create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/right.yml create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/left.urdf create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/left.yml create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/right.urdf create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/right.yml rename examples/teleop/python/{scripts => isaaccapture_examples/teleop}/fetch_realhand_assets.py (87%) rename examples/teleop/python/{ => isaaccapture_examples/teleop}/p7_realhand_bimanual_example.py (77%) create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_diagnostics.py create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_glove_calibration.py create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo.env create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo.py create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo_bridge.py create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_robot_scene.py create mode 100644 examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_thumb_calibration.py delete mode 100644 examples/teleop/python/realhand_ffg_glove_calibration.py create mode 100644 examples/teleop/realhand_ffg_hand_demo.md create mode 100644 examples/teleop/realhand_ffg_robot_scenes.md create mode 100755 examples/teleop/run_realhand_ffg_hand_demo.sh create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.cpp create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.hpp create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.cpp create mode 100644 src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.hpp delete mode 100644 src/python/isaaccapture/retargeters/realhand/realhand_ffg_glove.py create mode 100644 tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_reader.cpp create mode 100644 tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_skeleton.cpp delete mode 100644 tests/python/core/retargeting_engine/test_realhand_example.py create mode 100644 tests/python/examples/realhand/CMakeLists.txt create mode 100644 tests/python/examples/realhand/conftest.py create mode 100644 tests/python/examples/realhand/pyproject.toml rename tests/python/{core/retargeting_engine => examples/realhand}/test_realhand_asset_fetcher.py (63%) create mode 100644 tests/python/examples/realhand/test_realhand_dex_config.py create mode 100644 tests/python/examples/realhand/test_realhand_example.py create mode 100644 tests/python/examples/realhand/test_realhand_ffg_diagnostics.py create mode 100644 tests/python/examples/realhand/test_realhand_ffg_glove_calibration.py create mode 100644 tests/python/examples/realhand/test_realhand_ffg_glove_hand_input.py create mode 100644 tests/python/examples/realhand/test_realhand_ffg_hand_demo.py create mode 100644 tests/python/examples/realhand/test_realhand_ffg_robot_scene.py create mode 100644 tests/python/examples/realhand/test_realhand_module_entrypoints.py diff --git a/docs/source/_data/devices.yaml b/docs/source/_data/devices.yaml index 91ffc75d1e..6da58f9c08 100644 --- a/docs/source/_data/devices.yaml +++ b/docs/source/_data/devices.yaml @@ -227,9 +227,9 @@ devices: - id: realhand-ffg-glove name: RealHand FFG Glove - url: https://github.com/RealHand-Robotics/FFG_realhand_pure_python + url: https://www.realhand.com/ group: peripheral - modes: Finger joint sensing over USB + modes: 26-joint human hand tracking from USB glove sensors since: main details: setup: diff --git a/docs/source/device/realhand_ffg_glove.rst b/docs/source/device/realhand_ffg_glove.rst index 65853e81ce..c595546571 100644 --- a/docs/source/device/realhand_ffg_glove.rst +++ b/docs/source/device/realhand_ffg_glove.rst @@ -5,7 +5,7 @@ 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 +glove as a standard 26-joint OpenXR ``hand_tracker``. 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. @@ -18,18 +18,24 @@ Data flow .. code-block:: text - RealHand FFG Glove USB ─► realhand_ffg_glove plugin ─► JointStateSource ─► RealHandFFGGloveRetargeter - side discovery 21 sensors L6 / O6 / L20 joints + FFG USB -> calibrated human skeleton -> HandInjector -> HandsSource -> HandInput retargeter + 21 angle sensors 26 OpenXR joints any robot hand 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. +The primary device paths are ``/hand/left`` and ``/hand/right``. They are consumed by +``HandsSource`` and the same ``HandInput`` retargeters used for optical hand tracking, +including ``DexHandRetargeter``, Sharpa, and Wuji. The device plugin contains +no robot-specific retargeting. Human joint poses are estimated from glove angles +and approximate hand dimensions. + Build ----- -The plugin is enabled by default on POSIX systems. Configure and build Isaac Teleop normally: +The plugin is enabled by default on POSIX systems. Configure and build IsaacCapture normally: .. code-block:: console @@ -50,7 +56,7 @@ The current user must be able to open the glove's serial device. On Ubuntu, add $ sudo usermod -aG dialout "$USER" -Do not run Isaac Teleop as root. Check permissions with ``ls -l /dev/ttyUSB*`` and membership with +Do not run IsaacCapture as root. Check permissions with ``ls -l /dev/ttyUSB*`` and membership with ``id -nG``. Run @@ -64,8 +70,10 @@ Start the CloudXR runtime, source its OpenXR environment, and launch the install $ 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: +Automatic discovery only probes ``/dev/serial/by-id/`` entries whose names contain +``RealHand``, ``RealForce``, or ``FFG`` (case insensitive). A generic USB UART identifier +does not distinguish a glove from other hardware. For these devices, explicitly +select the verified glove ports: .. code-block:: console @@ -73,47 +81,92 @@ By default the plugin scans ``/dev/ttyUSB*``, ``/dev/ttyACM*``, ``/dev/ttyXRUSB* --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``. +Use only verified glove ports and close other applications reading the same devices. + +When a Python example starts the plugin with ``--plugin-path``, pass +``--realhand-ffg-glove-ports=/dev/ttyUSB0,/dev/ttyUSB1`` to select its ports. +If the plugin is already running separately, omit ``--plugin-path`` instead of +starting another process on the same ports. + +The supplementary collections ``realhand_ffg_glove_left`` and +``realhand_ffg_glove_right`` publish ``sensor_0`` through ``sensor_20`` in radians. +These ``JointStateSource`` inputs are for calibration and diagnostics, not robot +control. Mechanical hands consume the standard human skeleton through ``HandsSource``. + +Wrist placement +--------------- + +The plugin uses the shared ``WristPoseSource`` utility, as in the Wuji Glove plugin. +``--wrist-source=auto`` (default) prefers native hand tracking, then falls back to the +controller aim pose. Use ``controller`` or ``handtracking`` to select one source explicitly. +The combined FFG example selects controller wrists; P7 arm targets still use controller +grip poses. + +The default offsets support PICO 4 Ultra controllers mounted upright on the back of +each FFG glove using the RealHand bracket, with mirrored left/right installation. +For a different mount, override them with +``--left-aim-to-wrist=px,py,pz,qx,qy,qz,qw`` and +``--right-aim-to-wrist=px,py,pz,qx,qy,qz,qw``. Translation is in meters in the aim frame; +quaternions use XYZW order. ``--hand-scale=1.0`` adjusts the estimated hand size +(supported range 0.5 to 2.0). + +In controller mode, the hand becomes inactive if wrist tracking is lost or jumps. +Disconnected gloves and stale sensor streams also become inactive. 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. +Human skeleton calibration describes the device, not a target mechanical hand. One calibration +can feed L6, O6, L20, or another manufacturer's ``HandInput`` retargeter. Wear the glove +in its final fit. The command-line workflow waits for Enter before every step, then gives +8 seconds to release the keyboard and prepare, followed by a 6-second hold. It requires +at least 120 samples per hold. Change ``--prepare-seconds``, ``--hold-seconds``, or +``--samples`` if needed. + +Capture these five endpoint poses for each connected side: -For L6 and O6, capture these poses: +* **Open palm:** wrist neutral, palm flat, four fingers straight and together; thumb points sideways. +* **Full fist:** curl the four fingers fully; keep the thumb outside the fist, without forcing it. +* **Thumb-only curl:** keep the four fingers straight; fully bend the thumb MCP and IP joints. +* **Thumb opposition:** keep the four fingers straight; sweep the thumb across the palm toward + the little-finger base, without forcing fingertip contact. +* **Finger spread:** keep the fingers straight and spread them sideways as far as comfortable. -* **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. +Then lightly touch the thumb tip to the **index, middle, ring, and little fingertip**, one +at a time. Keep the unused fingers relaxed, without squeezing. Release and repeat each +of these four contacts, then repeat the original **open palm** pose. This is one 14-step +calibration sequence; when both sides are selected, do the same pose with both hands. +The PC calibration window in the hand demos uses this same sequence, sample reduction, +and fitting. L6, O6, and L20 reuse one human calibration without a model-specific step. -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. +Calibration requires NumPy and SciPy in the Capture interpreter. Fingers remain +independently driven; contact poses are used for calibration, not runtime snapping. 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 \ + $ pip install -e ./examples/teleop + $ python -m isaaccapture_examples.teleop.realhand_ffg_glove_calibration \ + --side both \ --plugin-path install/plugins \ - --output /path/to/calibration_l20.yml + --realhand-ffg-glove-ports=/dev/ttyUSB0,/dev/ttyUSB1 \ + --output /path/to/ffg_human.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. +side updates the same YAML and preserves the existing side. Pass the resulting +path with ``--realhand-ffg-glove-calibration`` +when starting the plugin from the combined FFG example. +For a separately launched plugin, restart it with ``--calibration=/path/to/ffg_human.yml``. +Without calibration the plugin publishes raw sensors for capture but no active human hand. +Recalibrate after re-wearing, power-cycling, or materially shifting a glove. Failed +captures leave the previous calibration intact. Files use +``realhand-ffg-glove-human-calibration-v1``; robot-specific calibration files are incompatible. +The demo loads existing human calibrations, including those used before CTS, without +refitting or rewriting them. Calibration itself does not certify CTS conformance. .. seealso:: :doc:`/references/retargeting/realhand` describes the P7 and RealHand output pipelines. + For device verification, see the official `Hand CTS workflow + `_. diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index c3744f51fd..83673472ea 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -29,9 +29,10 @@ 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 + the calibrated P7 workspace. The official ``DexHandRetargeter`` + and ``ControllerTriggerRealHandRetargeter`` produce joint commands for L6, O6, and L20. + FFG and optical tracking use the same standard human-joint hand-tracking pipeline. + See :doc:`realhand` for the full input-mode matrix and pipeline example. .. dropdown:: Se3AbsRetargeter / Se3RelRetargeter diff --git a/docs/source/references/retargeting/realhand.rst b/docs/source/references/retargeting/realhand.rst index c033afb52b..43d000d6d9 100644 --- a/docs/source/references/retargeting/realhand.rst +++ b/docs/source/references/retargeting/realhand.rst @@ -4,8 +4,8 @@ Retargeters: P7 and RealHand ============================ -The RealHand integration separates device acquisition, robot-independent retargeting, and -simulation control along Isaac Teleop's existing boundaries. +The RealHand integration separates device acquisition, hand retargeting, and +simulation control along IsaacCapture's existing boundaries. Components ---------- @@ -26,11 +26,8 @@ Components * - ``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`` + * - Official ``DexHandRetargeter`` + - One 26-joint OpenXR hand (optical tracking or a glove plugin) - L6, O6, or L20 hand joints P7 arm control @@ -46,33 +43,69 @@ 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. +closed posture. Optical hand tracking and FFG both supply standard 26-joint +``HandsSource`` data to the same official ``DexHandRetargeter``. The mechanical +hand retargeter is independent of which device supplies the human joints. +FFG mode keeps P7 end-effector targets on the controller grip poses. + +The model-specific YAML and mesh-free hand URDF files are in +``examples/teleop/python/isaaccapture_examples/teleop/config/realhand/``. They configure the official +vector optimizer, joint order, mimic relationships, and coordinate transform. +There is no custom optical hand algorithm, FFG robot mapping, gesture anchor, +or fingertip snapping. These small kinematic files do not require downloading +the full assemblies to run the pipeline example. + +Human skeleton reconstruction and robot-independent glove calibration belong to +the device plugin. Robot control has no raw-sensor bypass or FFG-specific mapping. +Other compatible hand-tracking devices can drive RealHand hands through the same +``HandInput`` contract; FFG can feed other manufacturers' compatible retargeters. Run the combined example ------------------------ -Install the lightweight retargeting dependency and build the package first: +Build and install the package from this checkout, following +:doc:`/development/build_from_source/index`. Install the locally built wheel +so the example uses the retargeters in this branch: .. code-block:: console - $ pip install 'isaaccapture[retargeters-lite]' - $ cmake --build build --target python_package + $ cmake --build build + $ cmake --install build + $ pip install --force-reinstall --no-deps isaaccapture --find-links=./install/wheels/ --no-index + $ pip install 'isaaccapture[retargeters]' + $ pip install -e ./examples/teleop + +The wheel installation assumes the base runtime dependencies are already installed as +described in the build guide; ``--no-deps`` keeps the offline wheel lookup from +trying to reinstall those dependencies. The official hand solver requires the +``retargeters`` extra (including dex-retargeting and PyTorch); the P7 pose +nodes require SciPy. +Setting up a development install from this +checkout with ``pip install -e '.[retargeters]'`` is another supported path; +install ``./examples/teleop`` there as well. The example supports all three hand models and all three input modes: .. code-block:: console - $ python examples/teleop/python/p7_realhand_bimanual_example.py \ + $ python -m isaaccapture_examples.teleop.p7_realhand_bimanual_example \ --mode handtracking --hand-model l20 - $ python examples/teleop/python/p7_realhand_bimanual_example.py \ + $ python -m isaaccapture_examples.teleop.p7_realhand_bimanual_example \ --mode controller --hand-model l6 - $ python examples/teleop/python/p7_realhand_bimanual_example.py \ + $ python -m isaaccapture_examples.teleop.p7_realhand_bimanual_example \ --mode ffg --hand-model o6 \ --plugin-path install/plugins \ - --realhand-ffg-glove-calibration /path/to/calibration_o6.yml + --realhand-ffg-glove-ports=/dev/ttyUSB0,/dev/ttyUSB1 \ + --realhand-ffg-glove-calibration /path/to/ffg_human.yml + +Use only verified glove ports; omit ``--realhand-ffg-glove-ports`` for devices +identified as FFG hardware by their USB serial identifiers. +``--realhand-ffg-glove-calibration`` is independent of ``--hand-model`` and passed +only to a plugin started with ``--plugin-path``. For an already running calibrated +plugin, omit both options. Graph construction does not load a glove calibration; +one human calibration can be reused with any compatible robot-hand retargeter. 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 @@ -87,16 +120,18 @@ root with: .. code-block:: console - $ python3 examples/teleop/python/scripts/fetch_realhand_assets.py + $ python3 -m isaaccapture_examples.teleop.fetch_realhand_assets 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 \ + $ python3 -m isaaccapture_examples.teleop.fetch_realhand_assets \ --hand-model l20 \ --output-dir ~/.cache/isaaccapture/realhand +The default destination is ``~/.cache/isaaccapture/realhand`` (under ``XDG_CACHE_HOME`` +when set), so installed examples do not write into their package directory. 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 @@ -110,8 +145,11 @@ assets, and runnable controller, hand-tracking, and RealHand FFG Glove examples the `RealHand Teleop v0.2.0 release `_. -This package is optional and is not required to build or use the Isaac Teleop plugins and +This package is optional and is not required to build or use the IsaacCapture plugins and retargeters in this repository. +It is a standalone release. Its legacy robot-specific FFG calibration cannot be +used as this plugin's human-skeleton calibration; capture a device calibration +with the example in this checkout. Coordinate calibration ---------------------- diff --git a/examples/teleop/README.md b/examples/teleop/README.md index 12364979d7..6645b7d4c9 100644 --- a/examples/teleop/README.md +++ b/examples/teleop/README.md @@ -39,6 +39,13 @@ Every module runs the same way, as | `spacemouse_printer_example` | SpaceMouse axes and buttons. | | `keyboard_printer_example` | Keys from a small GLFW window. | | `keyboard_terminal_example` | Keys from the terminal, with no window. | +| `p7_realhand_bimanual_example` | P7 arms with L6, O6, or L20 hands from controllers, optical tracking, or the FFG plugin. | +| `realhand_ffg_glove_calibration` | Robot-independent FFG human-hand calibration. | +| `realhand_ffg_diagnostics` | Record raw glove sensors, controllers, and reconstructed hands. | +| `fetch_realhand_assets` | Download the pinned P7 and RealHand assemblies to the user cache. | + +The optional Isaac Sim [FFG hand viewer](realhand_ffg_hand_demo.md) includes a +PC calibration window and [L6/O6/L20 robot-hand scenes](realhand_ffg_robot_scenes.md). The bimanual examples default to the dex-retargeting YAML and URDF files in [`config/dex_retargeting/`](python/isaaccapture_examples/teleop/config/dex_retargeting/), diff --git a/examples/teleop/python/assets/realhand/.gitignore b/examples/teleop/assets/realhand/.gitignore similarity index 100% rename from examples/teleop/python/assets/realhand/.gitignore rename to examples/teleop/assets/realhand/.gitignore diff --git a/examples/teleop/python/assets/realhand/README.md b/examples/teleop/assets/realhand/README.md similarity index 56% rename from examples/teleop/python/assets/realhand/README.md rename to examples/teleop/assets/realhand/README.md index e9658f10dd..e3ad8df05a 100644 --- a/examples/teleop/python/assets/realhand/README.md +++ b/examples/teleop/assets/realhand/README.md @@ -5,26 +5,28 @@ SPDX-License-Identifier: Apache-2.0 # P7 and RealHand Robot Assets -This directory receives the P7 + L6, O6, and L20 URDF assemblies and their meshes. The binary +The P7 + L6, O6, and L20 URDF assemblies and their meshes are downloaded to +`~/.cache/isaaccapture/realhand` (or `$XDG_CACHE_HOME/isaaccapture/realhand`). 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. +in Isaac Capture. -From the Isaac Teleop repository root, download all three assemblies with: +From the Isaac Capture repository root, install the examples and download all three assemblies: ```bash -python3 examples/teleop/python/scripts/fetch_realhand_assets.py +pip install -e ./examples/teleop +python3 -m isaaccapture_examples.teleop.fetch_realhand_assets ``` Download one assembly or choose another destination with: ```bash -python3 examples/teleop/python/scripts/fetch_realhand_assets.py \ +python3 -m isaaccapture_examples.teleop.fetch_realhand_assets \ --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 +objects, Git blobs, and the main URDF, and checks that every referenced mesh 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/isaaccapture_examples/teleop/config/realhand/README.md b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/README.md new file mode 100644 index 0000000000..977c3e95e0 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/README.md @@ -0,0 +1,57 @@ + + +# RealHand Configuration for the Official Dex Hand Retargeter + +These L6, O6, and L20 configurations are consumed directly by IsaacCapture's +`DexHandRetargeter`. Both optical OpenXR hand tracking and the RealHand FFG +Glove plugin supply the same standard 26-joint `HandInput`. + +Each model has a left/right YAML vector objective and a mesh-free, hand-only +URDF. The URDF joint origins, axes, limits, and existing mimic relationships +come from the released P7 assemblies at immutable revision +`e20d1f7dfe27d84784270012f291ae16f4762b0c` of +[RealHand Teleop](https://huggingface.co/realhandinc/realhand-teleop). +Fixed fingertip frames use the mean of the distal visual mesh's terminal +0.5 mm cap. They are geometric reference points, not contact sensors. +Only kinematics are included; no meshes, CAD sources, or personal calibration. + +L6 and O6 fit wrist-to-tip, inter-tip, and finger segment vectors with a 0.75 +length scale. L20 uses inter-tip and finger segment vectors with a 0.9 scale; +its long base-to-knuckle offsets are not treated as human wrist-to-knuckle +lengths. Individual phalanx vectors constrain L20's additional degrees of +freedom without requiring a specific gesture. + +L6/O6 use `normal_delta: 0.001` and `huber_delta: 0.02`. L20 uses +`normal_delta: 0.016` and `huber_delta: 0.005` to reduce thumb solution changes +while retaining sensitivity to small position errors. All models use the +official joint-output filter `low_pass_alpha: 0.4`. It advances 40% toward each +solved target per update; its delay depends on the update rate. These settings +apply equally to FFG and optical tracking, not to controller trigger mapping. + +There is no gesture classifier, pinch projection, contact threshold, or pose +anchor. These are model-level starting configurations, not a guarantee of exact +contact: limited robot degrees of freedom and estimated glove geometry can +prevent exact reproduction of human finger positions. Validate stationary noise, +slow bidirectional human-space trajectories at several sampling densities, and +endpoint poses. Inspect unfiltered joint solutions, not just rendered output; +reachable-robot round trips alone do not establish glove accuracy or continuity. +Human calibration is independent of these robot settings and does not need to +be repeated when changing models. + +The example passes an explicit row-vector coordinate transform: OpenXR dorsal +`+Y` maps to robot `-X`, and OpenXR fingertip `-Z` maps to robot `+Z`. +Output order comes from `get_realhand_profile()`; the official optimizer owns +joint-limit handling and URDF mimic adaptation. + +Complete robot assemblies for simulation remain available through +`python -m isaaccapture_examples.teleop.fetch_realhand_assets`. + +To replay a local human calibration through the native skeleton reconstruction +and test L20 continuity, add `--ffg-human-calibration /path/to/human.yml` to the +RealHand pytest suite along with `--ffg-skeleton-executable /path/to/ffg_test`. +The additional tests use 1%, 0.5%, and 0.25% trajectory steps and synthetic noise. +They require no live glove. Recordings remain local and are not part of these +robot configurations; passing replay tests is not a guarantee for all motions. diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/left.urdf b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/left.urdf new file mode 100644 index 0000000000..d7be19c752 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/left.urdf @@ -0,0 +1,212 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/left.yml b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/left.yml new file mode 100644 index 0000000000..b3c7135a69 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/left.yml @@ -0,0 +1,134 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +retargeting: + type: vector + urdf_path: left.urdf + target_joint_names: + - left_hand_thumb_cmc_roll + - left_hand_index_mcp_roll + - left_hand_middle_mcp_roll + - left_hand_ring_mcp_roll + - left_hand_pinky_mcp_roll + - left_hand_thumb_cmc_yaw + - left_hand_index_mcp_pitch + - left_hand_middle_mcp_pitch + - left_hand_ring_mcp_pitch + - left_hand_pinky_mcp_pitch + - left_hand_thumb_cmc_pitch + - left_hand_index_pip + - left_hand_middle_pip + - left_hand_ring_pip + - left_hand_pinky_pip + - left_hand_thumb_mcp + target_origin_link_names: + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_index_tip + - left_hand_index_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_thumb_proximal + - left_hand_index_proximal + - left_hand_middle_proximal + - left_hand_ring_proximal + - left_hand_pinky_proximal + - left_hand_thumb_metacarpals + - left_hand_thumb_proximal + - left_hand_thumb_distal + - left_hand_index_proximal + - left_hand_index_middle + - left_hand_middle_proximal + - left_hand_middle_middle + - left_hand_ring_proximal + - left_hand_ring_middle + - left_hand_pinky_proximal + - left_hand_pinky_middle + target_task_link_names: + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_pinky_tip + - left_hand_thumb_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_thumb_proximal + - left_hand_thumb_distal + - left_hand_thumb_tip + - left_hand_index_middle + - left_hand_index_distal + - left_hand_middle_middle + - left_hand_middle_distal + - left_hand_ring_middle + - left_hand_ring_distal + - left_hand_pinky_middle + - left_hand_pinky_distal + target_link_human_indices: + - - 4 + - 4 + - 4 + - 4 + - 8 + - 8 + - 8 + - 12 + - 12 + - 16 + - 2 + - 5 + - 9 + - 13 + - 17 + - 1 + - 2 + - 3 + - 5 + - 6 + - 9 + - 10 + - 13 + - 14 + - 17 + - 18 + - - 8 + - 12 + - 16 + - 20 + - 12 + - 16 + - 20 + - 16 + - 20 + - 20 + - 4 + - 8 + - 12 + - 16 + - 20 + - 2 + - 3 + - 4 + - 6 + - 7 + - 10 + - 11 + - 14 + - 15 + - 18 + - 19 + scaling_factor: 0.9 + normal_delta: 0.016 + huber_delta: 0.005 + low_pass_alpha: 0.4 diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/right.urdf b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/right.urdf new file mode 100644 index 0000000000..f96beff601 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/right.urdf @@ -0,0 +1,212 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/right.yml b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/right.yml new file mode 100644 index 0000000000..3f3fb33b89 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l20/right.yml @@ -0,0 +1,134 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +retargeting: + type: vector + urdf_path: right.urdf + target_joint_names: + - right_hand_thumb_cmc_roll + - right_hand_index_mcp_roll + - right_hand_middle_mcp_roll + - right_hand_ring_mcp_roll + - right_hand_pinky_mcp_roll + - right_hand_thumb_cmc_yaw + - right_hand_index_mcp_pitch + - right_hand_middle_mcp_pitch + - right_hand_ring_mcp_pitch + - right_hand_pinky_mcp_pitch + - right_hand_thumb_cmc_pitch + - right_hand_index_pip + - right_hand_middle_pip + - right_hand_ring_pip + - right_hand_pinky_pip + - right_hand_thumb_mcp + target_origin_link_names: + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_index_tip + - right_hand_index_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_thumb_proximal + - right_hand_index_proximal + - right_hand_middle_proximal + - right_hand_ring_proximal + - right_hand_pinky_proximal + - right_hand_thumb_metacarpals + - right_hand_thumb_proximal + - right_hand_thumb_distal + - right_hand_index_proximal + - right_hand_index_middle + - right_hand_middle_proximal + - right_hand_middle_middle + - right_hand_ring_proximal + - right_hand_ring_middle + - right_hand_pinky_proximal + - right_hand_pinky_middle + target_task_link_names: + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_pinky_tip + - right_hand_thumb_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_thumb_proximal + - right_hand_thumb_distal + - right_hand_thumb_tip + - right_hand_index_middle + - right_hand_index_distal + - right_hand_middle_middle + - right_hand_middle_distal + - right_hand_ring_middle + - right_hand_ring_distal + - right_hand_pinky_middle + - right_hand_pinky_distal + target_link_human_indices: + - - 4 + - 4 + - 4 + - 4 + - 8 + - 8 + - 8 + - 12 + - 12 + - 16 + - 2 + - 5 + - 9 + - 13 + - 17 + - 1 + - 2 + - 3 + - 5 + - 6 + - 9 + - 10 + - 13 + - 14 + - 17 + - 18 + - - 8 + - 12 + - 16 + - 20 + - 12 + - 16 + - 20 + - 16 + - 20 + - 20 + - 4 + - 8 + - 12 + - 16 + - 20 + - 2 + - 3 + - 4 + - 6 + - 7 + - 10 + - 11 + - 14 + - 15 + - 18 + - 19 + scaling_factor: 0.9 + normal_delta: 0.016 + huber_delta: 0.005 + low_pass_alpha: 0.4 diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/left.urdf b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/left.urdf new file mode 100644 index 0000000000..1872745479 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/left.urdf @@ -0,0 +1,127 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/left.yml b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/left.yml new file mode 100644 index 0000000000..4a4835c8b1 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/left.yml @@ -0,0 +1,105 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +retargeting: + type: vector + urdf_path: left.urdf + target_joint_names: + - 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 + - left_hand_thumb_cmc_pitch + - left_hand_index_dip + - left_hand_middle_dip + - left_hand_ring_dip + - left_hand_pinky_dip + - left_hand_thumb_dip + target_origin_link_names: + - left_hand_hand_base_link + - left_hand_hand_base_link + - left_hand_hand_base_link + - left_hand_hand_base_link + - left_hand_hand_base_link + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_index_tip + - left_hand_index_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_thumb_metacarpals + - left_hand_index_proximal + - left_hand_middle_proximal + - left_hand_ring_proximal + - left_hand_pinky_proximal + target_task_link_names: + - left_hand_thumb_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_pinky_tip + - left_hand_thumb_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + target_link_human_indices: + - - 0 + - 0 + - 0 + - 0 + - 0 + - 4 + - 4 + - 4 + - 4 + - 8 + - 8 + - 8 + - 12 + - 12 + - 16 + - 2 + - 5 + - 9 + - 13 + - 17 + - - 4 + - 8 + - 12 + - 16 + - 20 + - 8 + - 12 + - 16 + - 20 + - 12 + - 16 + - 20 + - 16 + - 20 + - 20 + - 4 + - 8 + - 12 + - 16 + - 20 + scaling_factor: 0.75 + normal_delta: 0.001 + huber_delta: 0.02 + low_pass_alpha: 0.4 diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/right.urdf b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/right.urdf new file mode 100644 index 0000000000..fe5ea800ef --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/right.urdf @@ -0,0 +1,127 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/right.yml b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/right.yml new file mode 100644 index 0000000000..ebb25af74a --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/l6/right.yml @@ -0,0 +1,105 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +retargeting: + type: vector + urdf_path: right.urdf + target_joint_names: + - 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 + - right_hand_thumb_cmc_pitch + - right_hand_index_dip + - right_hand_middle_dip + - right_hand_ring_dip + - right_hand_pinky_dip + - right_hand_thumb_ip + target_origin_link_names: + - right_hand_hand_base_link + - right_hand_hand_base_link + - right_hand_hand_base_link + - right_hand_hand_base_link + - right_hand_hand_base_link + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_index_tip + - right_hand_index_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_thumb_metacarpals + - right_hand_index_proximal + - right_hand_middle_proximal + - right_hand_ring_proximal + - right_hand_pinky_proximal + target_task_link_names: + - right_hand_thumb_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_pinky_tip + - right_hand_thumb_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + target_link_human_indices: + - - 0 + - 0 + - 0 + - 0 + - 0 + - 4 + - 4 + - 4 + - 4 + - 8 + - 8 + - 8 + - 12 + - 12 + - 16 + - 2 + - 5 + - 9 + - 13 + - 17 + - - 4 + - 8 + - 12 + - 16 + - 20 + - 8 + - 12 + - 16 + - 20 + - 12 + - 16 + - 20 + - 16 + - 20 + - 20 + - 4 + - 8 + - 12 + - 16 + - 20 + scaling_factor: 0.75 + normal_delta: 0.001 + huber_delta: 0.02 + low_pass_alpha: 0.4 diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/left.urdf b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/left.urdf new file mode 100644 index 0000000000..489d02d52e --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/left.urdf @@ -0,0 +1,132 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/left.yml b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/left.yml new file mode 100644 index 0000000000..f182e5cbfa --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/left.yml @@ -0,0 +1,100 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +retargeting: + type: vector + urdf_path: left.urdf + target_joint_names: + - left_hand_thumb_cmc_yaw + - left_hand_index_mcp_pitch + - left_hand_middle_mcp_pitch + - left_hand_ring_mcp_pitch + - left_hand_pinky_mcp_pitch + - left_hand_thumb_cmc_pitch + target_origin_link_names: + - left_hand_hand_base_link + - left_hand_hand_base_link + - left_hand_hand_base_link + - left_hand_hand_base_link + - left_hand_hand_base_link + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_thumb_tip + - left_hand_index_tip + - left_hand_index_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_thumb_metacarpals + - left_hand_index_proximal + - left_hand_middle_proximal + - left_hand_ring_proximal + - left_hand_pinky_proximal + target_task_link_names: + - left_hand_thumb_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_ring_tip + - left_hand_pinky_tip + - left_hand_pinky_tip + - left_hand_thumb_tip + - left_hand_index_tip + - left_hand_middle_tip + - left_hand_ring_tip + - left_hand_pinky_tip + target_link_human_indices: + - - 0 + - 0 + - 0 + - 0 + - 0 + - 4 + - 4 + - 4 + - 4 + - 8 + - 8 + - 8 + - 12 + - 12 + - 16 + - 2 + - 5 + - 9 + - 13 + - 17 + - - 4 + - 8 + - 12 + - 16 + - 20 + - 8 + - 12 + - 16 + - 20 + - 12 + - 16 + - 20 + - 16 + - 20 + - 20 + - 4 + - 8 + - 12 + - 16 + - 20 + scaling_factor: 0.75 + normal_delta: 0.001 + huber_delta: 0.02 + low_pass_alpha: 0.4 diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/right.urdf b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/right.urdf new file mode 100644 index 0000000000..a58b8bade6 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/right.urdf @@ -0,0 +1,132 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/right.yml b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/right.yml new file mode 100644 index 0000000000..c0535d8a3e --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/config/realhand/o6/right.yml @@ -0,0 +1,100 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +retargeting: + type: vector + urdf_path: right.urdf + target_joint_names: + - right_hand_thumb_cmc_yaw + - right_hand_index_mcp_pitch + - right_hand_middle_mcp_pitch + - right_hand_ring_mcp_pitch + - right_hand_pinky_mcp_pitch + - right_hand_thumb_cmc_pitch + target_origin_link_names: + - right_hand_hand_base_link + - right_hand_hand_base_link + - right_hand_hand_base_link + - right_hand_hand_base_link + - right_hand_hand_base_link + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_thumb_tip + - right_hand_index_tip + - right_hand_index_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_thumb_metacarpals + - right_hand_index_proximal + - right_hand_middle_proximal + - right_hand_ring_proximal + - right_hand_pinky_proximal + target_task_link_names: + - right_hand_thumb_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_ring_tip + - right_hand_pinky_tip + - right_hand_pinky_tip + - right_hand_thumb_tip + - right_hand_index_tip + - right_hand_middle_tip + - right_hand_ring_tip + - right_hand_pinky_tip + target_link_human_indices: + - - 0 + - 0 + - 0 + - 0 + - 0 + - 4 + - 4 + - 4 + - 4 + - 8 + - 8 + - 8 + - 12 + - 12 + - 16 + - 2 + - 5 + - 9 + - 13 + - 17 + - - 4 + - 8 + - 12 + - 16 + - 20 + - 8 + - 12 + - 16 + - 20 + - 12 + - 16 + - 20 + - 16 + - 20 + - 20 + - 4 + - 8 + - 12 + - 16 + - 20 + scaling_factor: 0.75 + normal_delta: 0.001 + huber_delta: 0.02 + low_pass_alpha: 0.4 diff --git a/examples/teleop/python/scripts/fetch_realhand_assets.py b/examples/teleop/python/isaaccapture_examples/teleop/fetch_realhand_assets.py similarity index 87% rename from examples/teleop/python/scripts/fetch_realhand_assets.py rename to examples/teleop/python/isaaccapture_examples/teleop/fetch_realhand_assets.py index b2212241fb..fcda9bad54 100755 --- a/examples/teleop/python/scripts/fetch_realhand_assets.py +++ b/examples/teleop/python/isaaccapture_examples/teleop/fetch_realhand_assets.py @@ -49,14 +49,12 @@ class RemoteFile: path: str size: int sha256: str | None - - -def _example_root() -> Path: - return Path(__file__).resolve().parents[1] + blob_id: str | None = None def _default_output_dir() -> Path: - return _example_root() / "assets" / "realhand" + cache_root = Path(os.environ.get("XDG_CACHE_HOME", Path.home() / ".cache")) + return cache_root / "isaaccapture" / "realhand" def _request_json(url: str) -> dict[str, Any]: @@ -107,11 +105,24 @@ def _selected_files(metadata: dict[str, Any], models: set[str]) -> list[RemoteFi 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): + if sha256 is not None and ( + not isinstance(sha256, str) + or len(sha256) != 64 + or any(ch not in "0123456789abcdef" for ch in sha256) + ): raise RuntimeError( f"Invalid LFS SHA-256 in Hugging Face metadata for {path}" ) - selected.append(RemoteFile(path=path, size=size, sha256=sha256)) + blob_id = sibling.get("blobId") if sha256 is None else None + if sha256 is None and ( + not isinstance(blob_id, str) + or len(blob_id) != 40 + or any(ch not in "0123456789abcdef" for ch in blob_id) + ): + raise RuntimeError(f"Missing or invalid Git blob hash for {path}") + selected.append( + RemoteFile(path=path, size=size, sha256=sha256, blob_id=blob_id) + ) required_prefixes = { prefix @@ -150,7 +161,16 @@ def _sha256(path: Path) -> str: 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 + if remote.sha256 is not None: + return _sha256(path) == remote.sha256 + if remote.blob_id is None: + return False + # Git hashes the blob header and contents, not just the file bytes. + digest = hashlib.sha1(f"blob {remote.size}\0".encode(), usedforsecurity=False) + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() == remote.blob_id def _download(remote: RemoteFile, output_dir: Path, force: bool) -> tuple[Path, bool]: @@ -188,6 +208,10 @@ def _download(remote: RemoteFile, output_dir: Path, force: bool) -> tuple[Path, raise RuntimeError( f"Downloaded {remote.path} has SHA-256 {digest.hexdigest()}, expected {remote.sha256}" ) + if not _is_complete(temporary, remote): + raise RuntimeError( + f"Downloaded {remote.path} failed pinned content hash verification" + ) temporary.replace(destination) except (OSError, urllib.error.URLError) as exc: raise RuntimeError(f"Failed to download {remote.path}: {exc}") from exc diff --git a/examples/teleop/python/p7_realhand_bimanual_example.py b/examples/teleop/python/isaaccapture_examples/teleop/p7_realhand_bimanual_example.py similarity index 77% rename from examples/teleop/python/p7_realhand_bimanual_example.py rename to examples/teleop/python/isaaccapture_examples/teleop/p7_realhand_bimanual_example.py index 09bc486557..68e9dc8041 100644 --- a/examples/teleop/python/p7_realhand_bimanual_example.py +++ b/examples/teleop/python/isaaccapture_examples/teleop/p7_realhand_bimanual_example.py @@ -21,23 +21,15 @@ 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.retargeters.realhand import 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 ( @@ -166,8 +158,33 @@ def _pose_config(model: str, side: str, mode: str) -> P7WorkspacePoseConfig: ) -def build_pipeline(mode: str, model: str, calibration: Path | None): +def dex_hand_config(model: str, side: str): + """Configure the official solver with a mesh-free, hand-only robot model.""" + from isaaccapture.retargeters import DexHandRetargeterConfig + + profile = get_realhand_profile(model) + profile.joints(side) + directory = Path(__file__).parent / "config" / "realhand" / profile.model + return DexHandRetargeterConfig( + hand_retargeting_config=str(directory / f"{side}.yml"), + hand_urdf=str(directory / f"{side}.urdf"), + hand_joint_names=profile.joint_names(side), + hand_side=side, + # Row-vector convention: OpenXR dorsal +Y -> robot -X, tips -Z -> +Z. + handtracking_to_baselink_frame_transform=(0, -1, 0, -1, 0, 0, 0, 0, -1), + ) + + +def _dex_hand(model: str, side: str): + from isaaccapture.retargeters import DexHandRetargeter + + return DexHandRetargeter(dex_hand_config(model, side), name=f"{model}_{side}_hand") + + +def build_pipeline(mode: str, model: str): """Build the official DeviceIO -> retargeters -> flat P7 action graph.""" + if mode not in ("controller", "handtracking", "ffg"): + raise ValueError(f"Unknown input mode: {mode}") profile = get_realhand_profile(model) left_joint_names = profile.joint_names("left") right_joint_names = profile.joint_names("right") @@ -180,24 +197,8 @@ def build_pipeline(mode: str, model: str, calibration: Path | None): 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_hand = _dex_hand(model, "left") + right_hand = _dex_hand(model, "right") left_input = source.output(HandsSource.LEFT) right_input = source.output(HandsSource.RIGHT) left_arm_input_name = HandsSource.LEFT @@ -239,42 +240,15 @@ def build_pipeline(mode: str, model: str, calibration: Path | None): 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 + # Both glove and optical HandInput use the official DexHandRetargeter; + # model configuration must not introduce an alternate sensor mapping. + hands = HandsSource(name="ffg_hands") + left_hand = _dex_hand(model, "left") + right_hand = _dex_hand(model, "right") + left_input = hands.output(HandsSource.LEFT) + right_input = hands.output(HandsSource.RIGHT) + left_hand_input_name = HandsSource.LEFT + right_hand_input_name = HandsSource.RIGHT hand_output = "hand_joints" connected_left_arm = left_arm.connect( @@ -323,13 +297,17 @@ def main() -> None: parser.add_argument( "--realhand-ffg-glove-calibration", type=Path, - help="User-specific calibration YAML generated by realhand_ffg_glove_calibration.py", + help="Robot-independent human skeleton calibration passed to the FFG device plugin", ) parser.add_argument( "--plugin-path", type=Path, help="Directory containing realhand_ffg_glove/plugin.yaml", ) + parser.add_argument( + "--realhand-ffg-glove-ports", + help="Comma-separated verified glove ports; requires --mode ffg and --plugin-path", + ) parser.add_argument("--duration", type=float, default=360.0) parser.add_argument( "--dry-run", @@ -339,10 +317,21 @@ def main() -> None: CloudXRLauncher.add_launcher_arguments(parser) args = parser.parse_args() + if args.realhand_ffg_glove_ports and ( + args.mode != "ffg" or args.plugin_path is None + ): + parser.error("--realhand-ffg-glove-ports requires --mode ffg and --plugin-path") + 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) + if calibration is not None and (args.mode != "ffg" or args.plugin_path is None): + parser.error( + "--realhand-ffg-glove-calibration requires --mode ffg and --plugin-path" + ) + if args.mode == "ffg" and args.plugin_path is not None and calibration is None: + parser.error( + "--realhand-ffg-glove-calibration is required when starting the FFG plugin" + ) + pipeline, action_order = build_pipeline(args.mode, args.hand_model) print( f"P7 + {args.hand_model.upper()} mode={args.mode}; action_dim={len(action_order)}" ) @@ -357,6 +346,19 @@ def main() -> None: plugin_name="realhand_ffg_glove_plugin", plugin_root_id="realhand_ffg_glove", search_paths=[args.plugin_path], + plugin_args=[ + "--wrist-source=controller", + ] + + ( + [f"--calibration={calibration.resolve()}"] + if calibration is not None + else [] + ) + + ( + [f"--ports={args.realhand_ffg_glove_ports}"] + if args.realhand_ffg_glove_ports + else [] + ), required=True, ) ) diff --git a/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_diagnostics.py b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_diagnostics.py new file mode 100644 index 0000000000..946b0779ed --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_diagnostics.py @@ -0,0 +1,216 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Record raw FFG sensors, OpenXR hands and controllers without robot retargeting.""" + +from __future__ import annotations + +import argparse +import hashlib +import json +from pathlib import Path +import time + +import numpy as np + +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.deviceio import McapRecordingConfig +from isaaccapture.deviceio_trackers import PluginDeviceStatusTracker +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, +) + +from .realhand_ffg_glove_calibration import _POSES +from .realhand_ffg_hand_demo_bridge import ( + load_calibration_example, + sample_glove_inputs, +) + +SIDES = ("left", "right") +POSES = ( + *_POSES, + *( + (f"touch the thumb tip lightly to the {finger} fingertip", f"thumb_{finger}") + for finger in ("index", "middle", "ring", "little") + ), + (_POSES[0][0], "open_end"), +) + + +def build_pipeline(): + # Sources must remain in the output graph for their native MCAP channels to be recorded. + hands = HandsSource(name="diagnostic_hands") + controllers = ControllersSource(name="diagnostic_controllers") + outputs = {} + for side in SIDES: + outputs[f"hand_{side}"] = hands.output(getattr(HandsSource, side.upper())) + outputs[f"controller_{side}"] = controllers.output( + getattr(ControllersSource, side.upper()) + ) + raw = JointStateSource( + name=f"diagnostic_raw_{side}", + collection_id=f"realhand_ffg_glove_{side}", + joint_names=[f"sensor_{index}" for index in range(21)], + ) + outputs[f"raw_{side}"] = raw.output(JointStateSource.JOINTS) + return OutputCombiner(outputs) + + +def summarize_samples(samples): + values = np.asarray(samples, dtype=float) + if values.ndim != 2 or values.shape[1:] != (21,) or len(values) < 30: + raise ValueError("Need at least 30 complete raw frames for each pose and side") + if not np.all(np.isfinite(values)): + raise ValueError("Raw capture contains non-finite sensor angles") + values = np.unwrap(values, axis=0) + median = np.median(values, axis=0) + return { + "frames": len(values), + "median_rad": median.tolist(), + "robust_sigma_rad": ( + 1.4826 * np.median(np.abs(values - median), axis=0) + ).tolist(), + "p05_rad": np.percentile(values, 5, axis=0).tolist(), + "p95_rad": np.percentile(values, 95, axis=0).tolist(), + } + + +def pump(session, tracker, example, seconds, collect=False): + samples = {side: [] for side in SIDES} + missing = dict.fromkeys(SIDES, 0) + start = time.monotonic_ns() + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + result = session.step() + raw, _ = sample_glove_inputs(session, result, example, tracker) + if collect: + for side in SIDES: + if raw[side] is None: + missing[side] += 1 + else: + samples[side].append(raw[side].tolist()) + time.sleep(1 / 120) + return start, time.monotonic_ns(), samples, missing + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--calibration", type=Path, required=True) + parser.add_argument("--plugin-path", type=Path, required=True) + parser.add_argument( + "--ports", required=True, help="Previously identified glove serial ports" + ) + parser.add_argument("--cycles", type=int, default=2) + parser.add_argument("--prepare-seconds", type=float, default=3.0) + parser.add_argument("--hold-seconds", type=float, default=3.0) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args() + if args.cycles < 1 or not all( + np.isfinite(x) and x >= 1 for x in (args.prepare_seconds, args.hold_seconds) + ): + parser.error( + "Cycles must be positive; preparation and hold times must be finite and >= 1 s" + ) + if not args.calibration.is_file(): + parser.error("An existing human calibration file is required") + metadata = args.output.with_suffix(".diagnostics.json") + if args.output.exists() or metadata.exists(): + parser.error("Output already exists; select a new recording path") + args.output.parent.mkdir(parents=True, exist_ok=True) + tracker = PluginDeviceStatusTracker("realhand_ffg_glove/device_status") + config = TeleopSessionConfig( + app_name="RealHandFFGDiagnostics", + trackers=[tracker], + pipeline=build_pipeline(), + plugins=[ + PluginConfig( + plugin_name="realhand_ffg_glove_plugin", + plugin_root_id="realhand_ffg_glove", + search_paths=[args.plugin_path], + plugin_args=[ + f"--ports={args.ports}", + f"--calibration={args.calibration.resolve()}", + "--wrist-source=controller", + ], + required=True, + ) + ], + mcap_config=McapRecordingConfig(str(args.output)), + ) + report = { + "purpose": "Raw sensor diagnosis, not the official CTS or an acceptance result", + "calibration_sha256": hashlib.sha256(args.calibration.read_bytes()).hexdigest(), + "clock_domain": "system CLOCK_MONOTONIC, nanoseconds", + "completed": False, + "windows": [], + } + example = load_calibration_example() + try: + with CloudXRLauncher.launch_context(args): + input( + "Connect the headset and controllers. Press Enter to begin the guided diagnostic... " + ) + with TeleopSession(config) as session: + print("Waiting for current raw data from both gloves...", flush=True) + deadline = time.monotonic() + 20 + while True: + result = session.step() + raw, errors = sample_glove_inputs(session, result, example, tracker) + if all(raw[s] is not None for s in SIDES): + break + if time.monotonic() > deadline: + raise RuntimeError( + f"Both gloves must have current data: {errors}" + ) + time.sleep(0.01) + for cycle in range(args.cycles): + for instruction, label in POSES: + print( + f"\nCycle {cycle + 1}/{args.cycles}: {instruction}.", + flush=True, + ) + print( + f"Prepare for {args.prepare_seconds:g} seconds...", + flush=True, + ) + pump(session, tracker, example, args.prepare_seconds) + print(f"\aHOLD for {args.hold_seconds:g} seconds.", flush=True) + start, end, samples, missing = pump( + session, tracker, example, args.hold_seconds, True + ) + window = { + "cycle": cycle + 1, + "pose": label, + "start_ns": start, + "end_ns": end, + "missing_frames": missing, + "raw": {}, + } + report["windows"].append(window) + for side in SIDES: + window["raw"][side] = summarize_samples(samples[side]) + if any(missing.values()): + raise RuntimeError( + "Glove data was lost during a hold; the partial recording is retained" + ) + report["completed"] = True + except KeyboardInterrupt: + print("\nDiagnostic interrupted; partial data retained.", flush=True) + finally: + metadata.write_text(json.dumps(report, indent=2, allow_nan=False) + "\n") + print( + f"Diagnostic recording: {args.output}\nPose summary: {metadata}", flush=True + ) + + +if __name__ == "__main__": + main() diff --git a/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_glove_calibration.py b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_glove_calibration.py new file mode 100644 index 0000000000..b9203a3a29 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_glove_calibration.py @@ -0,0 +1,382 @@ +# 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 +from copy import deepcopy +import os +import tempfile +import time +from datetime import datetime, timezone +from pathlib import Path + +import numpy as np +import yaml + +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.deviceio_trackers import PluginDeviceStatusTracker +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"} +_SENSOR_NAMES = tuple(f"sensor_{index}" for index in range(21)) +_FORMAT = "realhand-ffg-glove-human-calibration-v1" +_DEFAULT_PREPARE_SECONDS = 8 +_DEFAULT_HOLD_SECONDS = 6.0 +_DEFAULT_SAMPLES = 120 +# CLI and PC previews share the same device calibration, independent of robot model. +_POSES = ( + ( + "open palm: straighten all digits with the four fingers together and thumb pointing sideways", + "open", + ), + ("full fist: curl the four fingers fully; keep the thumb outside the fist", "fist"), + ( + "thumb-only curl: bend the thumb MCP and IP fully; keep the other digits straight", + "thumb_curl", + ), + ( + "thumb opposition: sweep the thumb across the palm toward the little-finger base; keep fingers straight", + "thumb_opposition", + ), + ( + "finger spread: keep the fingers straight and spread them sideways as far as comfortable", + "spread", + ), +) +_CONTACTS = ("index", "middle", "ring", "little") +_CAPTURE_POSES = ( + *_POSES, + *( + (f"touch the thumb tip lightly to the {finger} fingertip", f"thumb_{finger}") + for finger in _CONTACTS + ), + *( + ( + f"release, then repeat thumb to {finger} fingertip contact", + f"repeat_thumb_{finger}", + ) + for finger in _CONTACTS + ), + (_POSES[0][0], "open_repeat"), +) + + +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(_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(_SENSOR_NAMES))]) + if not np.all(np.isfinite(values)): + return None + return values + + +def _wait_for_sides( + session: TeleopSession, sides: tuple[str, ...], timeout: float, status_tracker=None +) -> None: + pending = set(sides) + deadline = time.monotonic() + timeout + while pending and time.monotonic() < deadline: + result = session.step() + for side in tuple(pending): + if _current_sample(session, result, side, status_tracker) 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 _current_sample(session, result, side, status_tracker): + if status_tracker is not None: + report = status_tracker.get_device_status_snapshot(session.deviceio_session) + if ( + report is None + or report.schema_version != 1 + or not 0 <= time.monotonic_ns() - report.report_time_ns < 2_000_000_000 + or not any( + device.path == f"/glove/realhand_ffg_glove_{side}" + and device.state.name == "CONNECTED" + for device in report.devices + ) + ): + return None + return _sample_group(result, side) + + +def _capture_pose( + session: TeleopSession, + sides: tuple[str, ...], + sample_count: int, + timeout: float, + *, + hold_seconds: float | None = None, + status_tracker=None, + return_samples: bool = False, +) -> dict: + samples: dict[str, list[np.ndarray]] = {side: [] for side in sides} + deadline = time.monotonic() + (timeout if hold_seconds is None else hold_seconds) + printed = None + while time.monotonic() < deadline and ( + hold_seconds is not None + or any(len(samples[side]) < sample_count for side in sides) + ): + result = session.step() + for side in sides: + if hold_seconds is None and len(samples[side]) >= sample_count: + continue + values = _current_sample(session, result, side, status_tracker) + if values is None and hold_seconds is not None: + raise RuntimeError( + f"{side}: glove data lost during HOLD; calibration unchanged" + ) + if values is not None: + samples[side].append(values) + if hold_seconds is not None: + remaining = max(1, int(np.ceil(deadline - time.monotonic()))) + if remaining != printed: + print(f" HOLD: {remaining} s", flush=True) + printed = remaining + 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}") + if return_samples: + return samples + return { + # Median raw phases across 0/2pi can invent a nonexistent half-turn pose. + side: np.median(np.unwrap(np.asarray(values), axis=0), axis=0).tolist() + for side, values in samples.items() + } + + +def _prepare(session, seconds): + for remaining in range(seconds, 0, -1): + print(f" PREPARE: {remaining} s", flush=True) + deadline = time.monotonic() + 1 + while time.monotonic() < deadline: + session.step() + time.sleep(0.01) + + +def _store_capture(poses, key, samples): + values = np.asarray(samples, dtype=float) + if ( + values.ndim != 2 + or values.shape[1:] != (21,) + or len(values) < 30 + or not np.all(np.isfinite(values)) + ): + raise ValueError("Each hold needs at least 30 finite 21-angle samples") + poses[key] = np.median(np.unwrap(values, axis=0), axis=0).tolist() + canonical = key.removeprefix("repeat_") + if canonical == "open_repeat": + canonical = "open" + if canonical in {"open", "fist", "thumb_curl", *(f"thumb_{f}" for f in _CONTACTS)}: + indices = list(range(0, len(values), max(1, len(values) // 16)))[:16] + poses.setdefault("thumb_observations", []).extend( + {"pose": canonical, "values": values[i].tolist()} for i in indices + ) + + +def _fit_and_save(path, captured): + from .realhand_ffg_thumb_calibration import fit_thumb_mapping + + candidate = deepcopy(_load_output(path)) + for side, poses in captured.items(): + # Replace the selected side in full: old coefficients cannot survive fresh sensor zeros. + candidate["hands"][side] = deepcopy(poses) + mapping = fit_thumb_mapping(poses, side) + if ( + "open_repeat" in poses + and mapping["fit_report"]["repeat_open_training_tip_mm"] > 15 + ): + raise ValueError( + f"{side}: repeated-open thumb displacement exceeds 15 mm; repeat calibration" + ) + candidate["hands"][side]["thumb_mapping"] = mapping + _validate_calibration(candidate, tuple(captured)) + candidate["timestamp"] = datetime.now(timezone.utc).isoformat() + path.parent.mkdir(parents=True, exist_ok=True) + temporary = None + try: + with tempfile.NamedTemporaryFile( + "w", dir=path.parent, encoding="utf-8", delete=False + ) as stream: + temporary = Path(stream.name) + yaml.safe_dump(candidate, stream, sort_keys=False) + os.replace(temporary, path) + finally: + if temporary is not None: + temporary.unlink(missing_ok=True) + return candidate + + +def _validate_calibration(calibration: dict, sides: tuple[str, ...]) -> None: + for side in sides: + poses = calibration["hands"][side] + for _, key in _POSES: + values = np.asarray(poses[key], dtype=float) + if values.shape != (21,) or not np.all(np.isfinite(values)): + raise ValueError(f"{side}.{key} must contain 21 finite angles") + spans = [(0, "thumb_opposition"), (2, "thumb_curl"), (4, "thumb_curl")] + if "thumb_mapping" in poses: + mapping = poses["thumb_mapping"] + minimum = np.asarray(mapping.get("input_min"), dtype=float) + maximum = np.asarray(mapping.get("input_max"), dtype=float) + if ( + mapping.get("format") != "realhand-ffg-thumb-quadratic-v1" + or minimum.shape != (4,) + or maximum.shape != (4,) + or not np.all(np.isfinite((minimum, maximum))) + or np.any(minimum > 0) + or np.any(maximum < 0) + or np.any(maximum - minimum >= np.pi) + or np.max(maximum - minimum) < 0.05 + ): + raise ValueError(f"{side}: invalid thumb mapping input ranges") + # Full multivariate captures replace the legacy single-pose thumb span check. + spans = [] + spans.extend( + (sensor, "fist") + for finger in range(4) + for sensor in (6 + 4 * finger, 8 + 4 * finger) + ) + for sensor, key in spans: + delta = poses[key][sensor] - poses["open"][sensor] + delta = (delta + np.pi) % (2.0 * np.pi) - np.pi + if abs(delta) < 0.05: + raise ValueError( + f"{side} sensor_{sensor} has insufficient calibration motion; capture the poses again" + ) + + +def _load_output(path: Path) -> dict[str, object]: + if not path.exists(): + return {"format": _FORMAT, "hands": {}} + 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}") + if existing.get("format") != _FORMAT or not isinstance(existing.get("hands"), dict): + raise ValueError( + "Existing file is not a human device calibration; select a new output file" + ) + return existing + + +def main() -> None: + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + 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=_DEFAULT_SAMPLES) + parser.add_argument("--timeout", type=float, default=20.0) + parser.add_argument("--prepare-seconds", type=int, default=_DEFAULT_PREPARE_SECONDS) + parser.add_argument("--hold-seconds", type=float, default=_DEFAULT_HOLD_SECONDS) + parser.add_argument( + "--plugin-path", + type=Path, + help="Directory containing realhand_ffg_glove/plugin.yaml", + ) + parser.add_argument( + "--realhand-ffg-glove-ports", + help="Comma-separated verified glove ports; requires --plugin-path", + ) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args() + if args.realhand_ffg_glove_ports and args.plugin_path is None: + parser.error("--realhand-ffg-glove-ports requires --plugin-path") + if args.samples < 30: + parser.error("--samples must be at least 30") + if not np.isfinite(args.timeout) or args.timeout <= 0: + parser.error("--timeout must be finite and positive") + if ( + args.prepare_seconds < 0 + or not np.isfinite(args.hold_seconds) + or args.hold_seconds <= 0 + ): + parser.error( + "--prepare-seconds must be nonnegative and --hold-seconds finite and positive" + ) + + 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], + # Raw capture must not inherit a personal calibration from the manifest. + plugin_args=["--calibration="] + + ( + [f"--ports={args.realhand_ffg_glove_ports}"] + if args.realhand_ffg_glove_ports + else [] + ), + required=True, + ) + ) + tracker = PluginDeviceStatusTracker("realhand_ffg_glove/device_status") + config = TeleopSessionConfig( + app_name="FFGGloveCalibration", + trackers=[tracker], + pipeline=pipeline, + plugins=plugins, + ) + _load_output(args.output) + captured = {side: {} for side in sides} + + 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, tracker) + for index, (label, key) in enumerate(_CAPTURE_POSES, 1): + input( + f"[{index}/{len(_CAPTURE_POSES)}] {label}. Press Enter, then release the keyboard..." + ) + _prepare(session, args.prepare_seconds) + samples = _capture_pose( + session, + sides, + args.samples, + args.timeout, + hold_seconds=args.hold_seconds, + status_tracker=tracker, + return_samples=True, + ) + for side, values in samples.items(): + _store_capture(captured[side], key, values) + print(f"Captured {label}") + + _fit_and_save(args.output, captured) + print(f"Calibration saved to {args.output}") + + +if __name__ == "__main__": + main() diff --git a/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo.env b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo.env new file mode 100644 index 0000000000..f15922e797 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo.env @@ -0,0 +1,7 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Emulated standalone profile; external gloves use push-device injection. +NV_DEVICE_PROFILE=quest3 +NV_CXR_ENABLE_PUSH_DEVICES=1 +NV_ENABLE_POSE_WAIT=0 diff --git a/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo.py b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo.py new file mode 100644 index 0000000000..f5aef301f5 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo.py @@ -0,0 +1,479 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Display standard FFG skeletons or RealHand robot hands with PC calibration.""" + +from __future__ import annotations + +import argparse +import json +import os +import socket +import subprocess +import tempfile +import time +import traceback +from pathlib import Path + +import numpy as np + +from .fetch_realhand_assets import _default_output_dir + +_EDGES = [(1, 0), (1, 2), (2, 3), (3, 4), (4, 5)] +for _start in (6, 11, 16, 21): + _EDGES.append((1, _start)) + _EDGES.extend((index, index + 1) for index in range(_start, _start + 4)) + + +def display_positions(points, side): + points = np.asarray(points, dtype=float) + if points.shape != (26, 3) or not np.all(np.isfinite(points)): + raise ValueError("Expected 26 finite OpenXR joint positions") + # Rotate the display palm-up about the finger axis; never reflect or change calibration data. + result = points[:, (0, 2, 1)] * (-1.0, -1.0, -1.0) + result += (-0.15 if side == "left" else 0.15, -0.08, 0.20) + return result + + +def bone_segments(points): + points = np.asarray(points, dtype=float) + starts = points[[first for first, _ in _EDGES]] + ends = points[[second for _, second in _EDGES]] + vectors = ends - starts + return (starts + ends) * 0.5, vectors, np.linalg.norm(vectors, axis=1) + + +def parse_args(argv=None): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--capture-python", type=Path) + parser.add_argument("--capture-package", type=Path) + parser.add_argument("--plugin-path", type=Path) + parser.add_argument("--hand-model", choices=("l6", "o6", "l20")) + parser.add_argument( + "--asset-root", + type=Path, + default=_default_output_dir(), + ) + parser.add_argument( + "--ports", + help="Comma-separated verified glove ports; omit for identified-device discovery", + ) + parser.add_argument( + "--calibration", + type=Path, + default=Path.home() / ".config/realhand/ffg_human_demo.yml", + ) + parser.add_argument( + "--test-frames", + type=Path, + help="Replay C++ skeleton fixtures; no glove or runtime is used", + ) + parser.add_argument("--headless", action="store_true") + parser.add_argument("--frames", type=int, default=0) + parser.add_argument("--screenshot", type=Path) + parser.add_argument("--no-launch-cloudxr-runtime", action="store_true") + args = parser.parse_args(argv) + if ( + args.hand_model + and args.test_frames + and (args.capture_python is None or args.capture_package is None) + ): + parser.error( + "Robot fixture replay requires --capture-python and --capture-package" + ) + if args.test_frames is None and any( + value is None + for value in (args.capture_python, args.capture_package, args.plugin_path) + ): + parser.error( + "Live mode requires --capture-python, --capture-package, and --plugin-path" + ) + if args.capture_python and not args.capture_python.is_file(): + parser.error(f"Capture Python interpreter not found: {args.capture_python}") + return args + + +def worker_command(args, ipc_dir): + if args.hand_model and args.test_frames: + return [ + str(args.capture_python), + "-m", + "isaaccapture_examples.teleop.realhand_ffg_hand_demo_bridge", + "--hand-model", + args.hand_model, + "--test-frames", + str(args.test_frames), + "--replay-output", + str(Path(ipc_dir) / "robot-replay.json"), + ] + command = [ + str(args.capture_python), + "-m", + "isaaccapture_examples.teleop.realhand_ffg_hand_demo_bridge", + "--ipc-dir", + str(ipc_dir), + "--plugin-path", + str(args.plugin_path), + "--calibration", + str(args.calibration), + ] + if args.hand_model: + command.extend(("--hand-model", args.hand_model)) + # Omitted ports must reach native discovery, not silently select a single glove. + if args.ports: + command.extend(("--ports", args.ports)) + if args.no_launch_cloudxr_runtime: + command.append("--no-launch-cloudxr-runtime") + return command + + +def main(): + args = parse_args() + + from isaacsim import SimulationApp + + app = SimulationApp( + { + "headless": args.headless, + "width": 1440, + "height": 900, + "renderer": "RaytracedLighting", + "fast_shutdown": True, + } + ) + process = None + window = None + menu_items = None + exit_code = 0 + try: + import omni.ui as ui + import omni.usd + from pxr import Gf, UsdGeom, UsdLux + from omni.kit.viewport.utility import get_active_viewport + from omni.kit.menu.utils import ( + MenuItemDescription, + add_menu_items, + remove_menu_items, + ) + + stage = omni.usd.get_context().get_stage() + UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z) + UsdGeom.SetStageMetersPerUnit(stage, 1.0) + world = UsdGeom.Xform.Define(stage, "/World") + stage.SetDefaultPrim(world.GetPrim()) + dome = UsdLux.DomeLight.Define(stage, "/World/Light") + dome.CreateIntensityAttr(450.0) + key = UsdLux.DistantLight.Define(stage, "/World/Key") + key.CreateIntensityAttr(900.0) + UsdGeom.Xformable(key).AddRotateXYZOp().Set((25, -30, -20)) + floor = UsdGeom.Cube.Define(stage, "/World/Workbench") + floor.CreateSizeAttr(1.0) + floor.CreateDisplayColorAttr([(0.14, 0.16, 0.18)]) + xf = UsdGeom.Xformable(floor) + xf.AddTranslateOp().Set((0, 0, -0.02)) + xf.AddScaleOp().Set((0.78, 0.56, 0.03)) + camera = UsdGeom.Camera.Define(stage, "/World/Camera") + camera.CreateFocalLengthAttr(28.0) + camera.CreateClippingRangeAttr((0.01, 100.0)) + UsdGeom.Xformable(camera).AddTransformOp().Set( + Gf.Matrix4d() + .SetLookAt( + Gf.Vec3d(0, -0.65, 0.80), Gf.Vec3d(0, 0.01, 0.20), Gf.Vec3d(0, 0, 1) + ) + .GetInverse() + ) + viewport = get_active_viewport() + if viewport: + viewport.set_active_camera(str(camera.GetPath())) + robot_scene = None + if args.hand_model: + from .realhand_ffg_robot_scene import RobotHandScene + + robot_scene = RobotHandScene(stage, args.asset_root, args.hand_model) + skeletons = {} + for side, color in ( + ("left", (0.14, 0.58, 0.85)), + ("right", (0.90, 0.35, 0.27)), + ): + if robot_scene is not None: + continue + path = f"/World/{side.title()}Hand" + root = UsdGeom.Xform.Define(stage, path) + joints = [] + for index in range(26): + joint = UsdGeom.Sphere.Define(stage, f"{path}/Joint_{index}") + joint.CreateRadiusAttr(0.006 if index not in (0, 1) else 0.010) + joint.CreateDisplayColorAttr([color]) + joints.append(UsdGeom.Xformable(joint).AddTranslateOp()) + # Solid bones must render in live RTX viewports, not only curve-enabled captures. + bones = [] + for index in range(len(_EDGES)): + bone = UsdGeom.Cylinder.Define(stage, f"{path}/Bone_{index}") + bone.CreateAxisAttr(UsdGeom.Tokens.z) + bone.CreateRadiusAttr(0.003) + bone.CreateHeightAttr(1.0) + bone.CreateDisplayColorAttr([(0.72, 0.77, 0.80)]) + transform = UsdGeom.Xformable(bone) + bones.append( + ( + bone, + transform.AddTranslateOp(), + transform.AddOrientOp(), + transform.AddScaleOp(), + ) + ) + root.MakeInvisible() + skeletons[side] = (root, joints, bones) + + fixture = None + if args.test_frames: + import yaml + + fixture = yaml.safe_load(args.test_frames.read_text(encoding="utf-8")) + window = ui.Window("RealHand FFG Calibration", width=390, height=580) + with window.frame, ui.ScrollingFrame(): + with ui.VStack(spacing=10, margin=14): + ui.Label( + f"RealHand {args.hand_model.upper()} / FFG" + if args.hand_model + else "RealHand FFG Glove", + height=26, + ) + connection = ui.Label("Starting DeviceIO...", height=46, word_wrap=True) + ui.Separator(height=2) + side_choice = ui.ComboBox(2, "Left", "Right", "Both", height=28) + calibration_button = ui.Button( + "Start Calibration", + height=32, + clicked_fn=lambda: send( + { + "action": "begin", + "sides": (("left",), ("right",), ("left", "right"))[ + side_choice.model.get_item_value_model().as_int + ], + } + ), + ) + step_label = ui.Label("", height=24) + instruction = ui.Label("", height=92, word_wrap=True) + countdown = ui.Label("", height=28) + capture_button = ui.Button( + "Capture Pose", + height=32, + clicked_fn=lambda: send({"action": "capture"}), + ) + ui.Button( + "Cancel Capture", + height=28, + clicked_fn=lambda: send({"action": "cancel"}), + ) + message = ui.Label("", height=110, word_wrap=True) + window.deferred_dock_in("Property", ui.DockPolicy.CURRENT_WINDOW_IS_ACTIVE) + + def show_calibration(): + window.visible = True + window.focus() + + menu_items = [ + MenuItemDescription( + name="RealHand FFG Calibration", onclick_fn=show_calibration + ) + ] + add_menu_items(menu_items, "Window") + + rgb = None + if args.screenshot: + import omni.replicator.core as rep + + product = rep.create.render_product(str(camera.GetPath()), (1280, 800)) + rgb = rep.AnnotatorRegistry.get_annotator("rgb") + rgb.attach(product) + with tempfile.TemporaryDirectory(prefix="ffg-demo-") as temporary: + ipc = Path(temporary) + with socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) as channel: + channel.bind(str(ipc / "frames.sock")) + channel.setblocking(False) + + def send(command): + if fixture is not None: + message.text = "Fixture preview only. Launch live mode to calibrate a glove." + return + try: + channel.sendto( + json.dumps(command).encode(), str(ipc / "commands.sock") + ) + except OSError: + message.text = ( + "DeviceIO is not ready. Check the terminal output." + ) + + if fixture is None or robot_scene is not None: + environment = os.environ.copy() + environment["PYTHONPATH"] = os.pathsep.join( + ( + str(args.capture_package), + str(Path(__file__).resolve().parents[2]), + ) + ) + # Isaac Sim's bundled shared libraries must not leak into the standalone OpenXR worker. + for key in ( + "LD_LIBRARY_PATH", + "PYTHONHOME", + "CARB_APP_PATH", + "ISAAC_PATH", + ): + environment.pop(key, None) + if fixture is not None: + subprocess.run( + worker_command(args, temporary), env=environment, check=True + ) + fixture = json.loads( + (ipc / "robot-replay.json").read_text(encoding="utf-8") + ) + else: + process = subprocess.Popen( + worker_command(args, temporary), env=environment + ) + latest = None + received = 0.0 + frame = 0 + started = time.monotonic() + while app.is_running() and (not args.frames or frame < args.frames): + now = time.monotonic() + if fixture is not None: + pose_index = int((now - started) / 2.0) % 3 + latest = ( + fixture[int((now - started) * 20) % len(fixture)] + if robot_scene is not None + else { + "connected": {"left": True, "right": True}, + "hands": { + side: [ + joint["position"] + for joint in fixture[side][pose_index]["joints"] + ] + for side in ("left", "right") + }, + "calibration": { + "state": "idle", + "message": "C++ skeleton fixture preview; no live glove data.", + "instruction": "", + }, + } + ) + received = now + else: + while True: + try: + latest = json.loads(channel.recv(65536)) + received = now + except BlockingIOError: + break + fresh = latest is not None and now - received < 0.5 + if robot_scene is not None and fresh: + robot_scene.update(latest.get("robots", {})) + for side, (root, joints, bones) in skeletons.items(): + points = latest["hands"].get(side) if fresh else None + if points is None: + root.MakeInvisible() + continue + points = display_positions(points, side) + root.MakeVisible() + for index, joint in enumerate(joints): + joint.Set(Gf.Vec3d(*points[index])) + centers, vectors, lengths = bone_segments(points) + for ( + bone, + translate, + orient, + scale, + ), center, vector, length in zip( + bones, centers, vectors, lengths + ): + if length < 1e-6: + bone.MakeInvisible() + continue + bone.MakeVisible() + translate.Set(Gf.Vec3d(*center)) + rotation = Gf.Rotation( + Gf.Vec3d(0, 0, 1), Gf.Vec3d(*(vector / length)) + ) + orient.Set(Gf.Quatf(rotation.GetQuat())) + scale.Set(Gf.Vec3f(1, 1, length)) + if fresh: + connection.text = " | ".join( + f"{side.title()}: {'connected' if latest['connected'][side] else 'not found'}" + for side in ("left", "right") + ) + state = latest["calibration"] + step_label.text = ( + f"Calibration: {state['step']} / {state['total']}" + if state["state"] in ("ready", "preparing", "capturing") + else f"Calibration: {state['state'].title()}" + ) + instruction.text = state.get("instruction", "") + countdown.text = ( + f"PREPARE: {state['remaining']:.1f} s" + if state["state"] == "preparing" + else f"HOLD: {state['remaining']:.1f} s" + if state["state"] == "capturing" + else "" + ) + capture_button.enabled = state["state"] == "ready" + calibration_button.enabled = state["state"] not in ( + "ready", + "preparing", + "capturing", + ) + side_choice.enabled = state["state"] not in ( + "ready", + "preparing", + "capturing", + ) + errors = [ + f"{side.title()}: {error}" + for side, error in latest.get("device_errors", {}).items() + if error + ] + message.text = "\n".join([state.get("message", ""), *errors]) + elif process is not None and process.poll() is not None: + connection.text = ( + f"DeviceIO exited (code {process.returncode})." + ) + message.text = "Check the terminal for runtime, USB permission, or calibration errors." + elif now - started > 3.0: + connection.text = "Waiting for DeviceIO..." + app.update() + frame += 1 + time.sleep(max(0.0, 1.0 / 60.0 - (time.monotonic() - now))) + if rgb is not None: + from PIL import Image + + rep.orchestrator.step(rt_subframes=2) + image = rgb.get_data() + if not isinstance(image, np.ndarray) or image.size == 0: + raise RuntimeError("Isaac Sim produced no screenshot pixels") + args.screenshot.parent.mkdir(parents=True, exist_ok=True) + Image.fromarray(image).save(args.screenshot) + print(f"Screenshot saved: {args.screenshot}") + except Exception: + # Fast shutdown otherwise exits before Python can report a startup error. + traceback.print_exc() + exit_code = 1 + finally: + if process is not None and process.poll() is None: + process.terminate() + try: + process.wait(timeout=15) + except subprocess.TimeoutExpired: + process.kill() + process.wait() + if window is not None: + if menu_items is not None: + remove_menu_items(menu_items, "Window") + window.destroy() + app.close(exit_code=exit_code) + + +if __name__ == "__main__": + main() diff --git a/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo_bridge.py b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo_bridge.py new file mode 100644 index 0000000000..173d626ffc --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_hand_demo_bridge.py @@ -0,0 +1,502 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Run the standard FFG DeviceIO stream separately from Isaac Sim's Python ABI.""" + +from __future__ import annotations + +import argparse +import importlib +import json +import logging +import signal +import socket +import time +import xml.etree.ElementTree as ET +from functools import lru_cache +from pathlib import Path + +import numpy as np +import yaml + +_LOGGER = logging.getLogger("isaaccapture.examples.realhand_ffg_hand_demo_bridge") + + +def load_calibration_example(name="realhand_ffg_glove_calibration"): + return importlib.import_module(f".{name}", __package__) + + +def validate_saved_calibration(path, example): + # Previewing CTS calibration must never refit or rewrite the user's measurements. + calibration = example._load_output(path) + example._validate_calibration(calibration, tuple(calibration["hands"])) + return calibration + + +class CalibrationCapture: + def __init__(self, path, example): + self.path = Path(path) + self.example = example + self.poses = example._CAPTURE_POSES + self.prepare_seconds = example._DEFAULT_PREPARE_SECONDS + self.hold_seconds = example._DEFAULT_HOLD_SECONDS + self.sample_count = example._DEFAULT_SAMPLES + self.state = "idle" + self.sides = () + self.step = 0 + self.deadline = 0.0 + self.samples = {} + self.captured = {} + self.message = "Select the connected glove side, then start calibration." + + def command(self, command, connected, now): + action = command.get("action") + if action == "cancel": + self.state = "idle" + self.captured.clear() + self.samples.clear() + self.message = "Calibration cancelled. Existing calibration is unchanged." + elif action == "begin": + sides = command.get("sides") + if ( + not isinstance(sides, list) + or not sides + or any(side not in ("left", "right") for side in sides) + ): + self.message = "Choose Left, Right, or Both." + return + if not all(connected.get(side, False) for side in sides): + self.message = "Requested glove is not connected. Check its USB port and permissions." + return + self.sides = tuple(dict.fromkeys(sides)) + self.captured = {side: {} for side in self.sides} + self.step = 0 + self.state = "ready" + self.message = "Hold the requested pose, then click Capture Pose." + elif action == "capture" and self.state == "ready": + if not all(connected.get(side, False) for side in self.sides): + self.message = ( + "A requested glove is disconnected. Reconnect it before capturing." + ) + return + self.samples = {side: [] for side in self.sides} + self.deadline = now + self.prepare_seconds + self.state = "preparing" + self.message = "Release the mouse and prepare both hands before HOLD." + + def update(self, raw, now): + if self.state == "preparing": + if now < self.deadline: + return False + self.deadline = now + self.hold_seconds + self.state = "capturing" + self.message = "Hold still until the countdown finishes." + if self.state != "capturing": + return False + if any(raw.get(side) is None for side in self.sides): + self.state = "error" + self.message = "Glove data was lost during capture. Restart calibration." + return False + for side in self.sides: + values = np.asarray(raw[side], dtype=float) + if values.shape != (21,) or not np.all(np.isfinite(values)): + self.state = "error" + self.message = "Invalid glove data. Restart calibration." + return False + self.samples[side].append(values.copy()) + if now < self.deadline: + return False + if any(len(samples) < self.sample_count for samples in self.samples.values()): + self.state = "error" + self.message = "Too few valid samples. Restart calibration." + return False + key = self.poses[self.step][1] + for side, samples in self.samples.items(): + self.example._store_capture(self.captured[side], key, samples) + self.step += 1 + if self.step < len(self.poses): + self.state = "ready" + self.message = ( + "Pose captured. Prepare the next pose, then click Capture Pose." + ) + return False + try: + self.example._fit_and_save(self.path, self.captured) + except (OSError, ValueError, yaml.YAMLError) as error: + self.state = "error" + self.message = str(error) + return False + self.state = "complete" + self.message = "Calibration saved. Reloading the FFG hand plugin..." + return True + + def status(self, now): + step = min(self.step, len(self.poses) - 1) + return { + "state": self.state, + "step": step + 1, + "total": len(self.poses), + "instruction": self.poses[step][0] + if self.state in ("ready", "preparing", "capturing") + else "", + "remaining": max(0.0, self.deadline - now) + if self.state in ("preparing", "capturing") + else 0.0, + "message": self.message, + "file": str(self.path), + } + + +def build_pipeline(hand_model=None, retargeters=None): + from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + HandsSource, + JointStateSource, + ) + from isaaccapture.retargeting_engine.interface import OutputCombiner + + hands = HandsSource(name="ffg_demo_hands") + outputs = { + side: hands.output(getattr(HandsSource, side.upper())) + for side in ("left", "right") + } + if hand_model is not None: + from isaaccapture.retargeters import DexHandRetargeter + + example = load_calibration_example("p7_realhand_bimanual_example") + for side in ("left", "right"): + # Scene diagnostics must use the same official solver as the combined example. + retargeter = DexHandRetargeter( + example.dex_hand_config(hand_model, side), + name=f"demo_{hand_model}_{side}", + ) + connected = retargeter.connect( + {f"hand_{side}": hands.output(getattr(HandsSource, side.upper()))} + ) + outputs[f"robot_{side}"] = connected.output("hand_joints") + if retargeters is not None: + retargeters[side] = retargeter + for side in ("left", "right"): + raw = JointStateSource( + name=f"ffg_demo_raw_{side}", + collection_id=f"realhand_ffg_glove_{side}", + joint_names=[f"sensor_{sensor}" for sensor in range(21)], + ) + outputs[f"raw_{side}"] = raw.output(JointStateSource.JOINTS) + return OutputCombiner(outputs) + + +@lru_cache(maxsize=6) +def robot_link_names(urdf): + # Pinocchio exposes JOINT and BODY frames; only URDF links are drawable bodies. + return tuple(link.get("name") for link in ET.parse(urdf).getroot().findall("link")) + + +def robot_frame(retargeter, output): + """Render FK from the official solver's named joint output and mimic adaptor.""" + optimizer = retargeter._dex_hand.optimizer + robot = optimizer.robot + names = retargeter._config.hand_joint_names + values = ( + np.asarray([float(value) for value in output], dtype=float) + if output is not None + else np.zeros(len(names)) + ) + qpos = np.zeros(robot.dof) + for name, value in zip(names, values): + qpos[robot.get_joint_index(name)] = value + if optimizer.adaptor is not None: + qpos = optimizer.adaptor.forward_qpos(qpos) + robot.compute_forward_kinematics(qpos) + poses = { + name: robot.get_link_pose(robot.get_link_index(name)).tolist() + for name in robot_link_names(retargeter._config.hand_urdf) + } + return {"joint_names": names, "positions": values.tolist(), "link_poses": poses} + + +def replay_robot_frames(model, fixture): + """Replay native skeleton fixtures through the official solver without OpenXR.""" + from isaaccapture.retargeters import DexHandRetargeter + 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.tensor_types import HandInputIndex + + example = load_calibration_example("p7_realhand_bimanual_example") + retargeters = {} + ios = {} + for side in ("left", "right"): + retargeter = DexHandRetargeter( + example.dex_hand_config(model, side), name=f"replay_{side}" + ) + retargeters[side] = retargeter + ios[side] = ( + { + key: OptionalTensorGroup(spec) + for key, spec in retargeter.input_spec().items() + }, + {key: TensorGroup(spec) for key, spec in retargeter.output_spec().items()}, + ) + context = ComputeContext( + graph_time=GraphTime(sim_time_ns=0, real_time_ns=0), + execution_events=ExecutionEvents( + reset=False, execution_state=ExecutionState.RUNNING + ), + ) + count = min(len(fixture[side]) for side in ("left", "right")) + packets = [] + for start in range(count): + for alpha in np.linspace(0.0, 1.0, 20, endpoint=False): + packet = { + "connected": {"left": True, "right": True}, + "hands": {}, + "robots": {}, + "calibration": { + "state": "idle", + "instruction": "", + "message": "Fixture replay; no live glove or calibration capture.", + }, + } + for side, retargeter in retargeters.items(): + first = fixture[side][start]["joints"] + second = fixture[side][(start + 1) % count]["joints"] + inputs, outputs = ios[side] + group = inputs[f"hand_{side}"] + positions = (1 - alpha) * np.asarray( + [j["position"] for j in first] + ) + alpha * np.asarray([j["position"] for j in second]) + group[HandInputIndex.JOINT_POSITIONS] = positions.astype(np.float32) + group[HandInputIndex.JOINT_ORIENTATIONS] = np.asarray( + [j["orientation"] for j in first], dtype=np.float32 + ) + group[HandInputIndex.JOINT_RADII] = np.asarray( + [j["radius"] for j in first], dtype=np.float32 + ) + group[HandInputIndex.JOINT_VALID] = np.ones(26, dtype=np.uint8) + retargeter.compute(inputs, outputs, context) + packet["hands"][side] = hand_positions(group) + packet["robots"][side] = robot_frame(retargeter, outputs["hand_joints"]) + packets.append(packet) + return packets + + +def hand_positions(group): + from isaaccapture.retargeting_engine.tensor_types import HandInputIndex + + if group.is_none: + return None + valid = np.from_dlpack(group[HandInputIndex.JOINT_VALID]) + if not np.all(valid): + return None + positions = np.from_dlpack(group[HandInputIndex.JOINT_POSITIONS]).copy() + orientations = np.from_dlpack(group[HandInputIndex.JOINT_ORIENTATIONS]) + if not np.all(np.isfinite(positions)) or not np.all(np.isfinite(orientations)): + return None + q = orientations[1] + length = np.linalg.norm(q) + if length < 1e-6: + return None + # Fixed wrists in the viewer: remove the OpenXR wrist's global rigid transform. + xyz, w = -q[:3] / length, q[3] / length + local = positions - positions[1] + cross = 2.0 * np.cross(xyz, local) + local = local + w * cross + np.cross(xyz, cross) + return local.tolist() + + +def sample_glove_inputs(session, result, example, status_tracker): + report = status_tracker.get_device_status_snapshot(session.deviceio_session) + devices = {} + if ( + report is not None + and report.schema_version == 1 + and 0 <= time.monotonic_ns() - report.report_time_ns < 2_000_000_000 + ): + devices = {device.path: device for device in report.devices} + raw = {} + errors = {} + for side in ("left", "right"): + device = devices.get(f"/glove/realhand_ffg_glove_{side}") + # JointStateSource may retain its last tensor after USB disconnection. + raw[side] = ( + example._sample_group(result, f"raw_{side}") + if device is not None and device.state.name == "CONNECTED" + else None + ) + errors[side] = device.error if device is not None else "" + hand = devices.get(f"/hand/{side}") + if ( + raw[side] is not None + and hand is not None + and hand.state.name != "CONNECTED" + ): + errors[side] = hand.error + return raw, errors + + +def plugin_arguments(ports, calibration): + # Fixed-wrist previews accept local, untracked skeletons; CTS still requires a tracked wrist. + arguments = ["--wrist-source=auto"] + if ports: + arguments.append(f"--ports={ports}") + if calibration.is_file(): + arguments.append(f"--calibration={calibration.resolve()}") + return arguments + + +def main(): + from isaaccapture.cloudxr import CloudXRLauncher + from isaaccapture.deviceio_trackers import PluginDeviceStatusTracker + from isaaccapture.teleop_session_manager import ( + PluginConfig, + TeleopSession, + TeleopSessionConfig, + ) + + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--ipc-dir", type=Path) + parser.add_argument("--plugin-path", type=Path) + parser.add_argument("--hand-model", choices=("l6", "o6", "l20")) + parser.add_argument("--test-frames", type=Path) + parser.add_argument("--replay-output", type=Path) + parser.add_argument( + "--ports", + help="Comma-separated verified glove ports; omit for identified-device discovery", + ) + parser.add_argument("--calibration", type=Path) + parser.add_argument("--max-seconds", type=float, default=0.0) + CloudXRLauncher.add_launcher_arguments(parser) + parser.set_defaults( + cloudxr_device_profile="quest3", + cloudxr_env_config=str(Path(__file__).with_name("realhand_ffg_hand_demo.env")), + ) + args = parser.parse_args() + if args.test_frames is not None: + if args.hand_model is None or args.replay_output is None: + parser.error( + "Robot fixture replay requires --hand-model and --replay-output" + ) + fixture = yaml.safe_load(args.test_frames.read_text(encoding="utf-8")) + packets = replay_robot_frames(args.hand_model, fixture) + args.replay_output.write_text( + json.dumps(packets, allow_nan=False), encoding="utf-8" + ) + return + if any( + value is None for value in (args.ipc_dir, args.plugin_path, args.calibration) + ): + parser.error("Live mode requires --ipc-dir, --plugin-path, and --calibration") + example = load_calibration_example() + capture = CalibrationCapture(args.calibration, example) + try: + validate_saved_calibration(args.calibration, example) + except ValueError as error: + capture.message = ( + f"Existing calibration invalid: {error}. Start calibration to replace it." + ) + running = True + + def stop(signum, frame): + nonlocal running + running = False + + signal.signal(signal.SIGTERM, stop) + signal.signal(signal.SIGINT, stop) + frame_path = str(args.ipc_dir / "frames.sock") + command_path = args.ipc_dir / "commands.sock" + started = time.monotonic() + with socket.socket(socket.AF_UNIX, socket.SOCK_DGRAM) as channel: + channel.bind(str(command_path)) + channel.setblocking(False) + try: + with CloudXRLauncher.launch_context(args, host_client=False): + while running: + if args.calibration.is_file(): + example._load_output(args.calibration) + status_tracker = PluginDeviceStatusTracker( + "realhand_ffg_glove/device_status" + ) + retargeters = {} + config = TeleopSessionConfig( + app_name="RealHandFFGValidation", + trackers=[status_tracker], + pipeline=build_pipeline(args.hand_model, retargeters), + plugins=[ + PluginConfig( + plugin_name="realhand_ffg_glove_plugin", + plugin_root_id="realhand_ffg_glove", + search_paths=[args.plugin_path], + plugin_args=plugin_arguments( + args.ports, args.calibration + ), + required=True, + ) + ], + ) + restart = False + with TeleopSession(config) as session: + if capture.state == "complete": + capture.message = "Calibration active. The standard 26-joint hand stream is now displayed." + while running and not restart: + now = time.monotonic() + if args.max_seconds and now - started > args.max_seconds: + running = False + break + result = session.step() + raw, errors = sample_glove_inputs( + session, result, example, status_tracker + ) + connected = { + side: values is not None for side, values in raw.items() + } + hands = { + side: hand_positions(result[side]) + if connected[side] + else None + for side in ("left", "right") + } + while True: + try: + command = json.loads(channel.recv(4096)) + except BlockingIOError: + break + capture.command(command, connected, now) + restart = capture.update(raw, now) + packet = { + "time": now, + "connected": connected, + "device_errors": errors, + "hands": hands, + "calibration": capture.status(now), + } + if retargeters: + packet["robots"] = { + side: robot_frame( + retargeter, + result[f"robot_{side}"] + if connected[side] + else None, + ) + for side, retargeter in retargeters.items() + } + try: + channel.sendto( + json.dumps(packet, allow_nan=False).encode(), + frame_path, + ) + except (BlockingIOError, FileNotFoundError): + pass + time.sleep(max(0.0, 1.0 / 90.0 - (time.monotonic() - now))) + finally: + command_path.unlink(missing_ok=True) + + +if __name__ == "__main__": + main() diff --git a/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_robot_scene.py b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_robot_scene.py new file mode 100644 index 0000000000..d728c47572 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_robot_scene.py @@ -0,0 +1,112 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Mesh rendering for fixed-wrist robot-hand pose verification, without physics.""" + +from pathlib import Path +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from .fetch_realhand_assets import _validate_model + + +def visual_links(asset_root, model, side): + urdf = _validate_model(Path(asset_root), model) + root = ET.parse(urdf).getroot() + visuals = {} + for link in root.findall("link"): + name = link.get("name") + if not name.startswith(f"{side}_hand_"): + continue + parts = [] + for visual in link.findall("visual"): + mesh = visual.find("geometry/mesh") + if mesh is None: + continue + origin = visual.find("origin") + matrix = np.eye(4) + if origin is not None: + matrix[:3, :3] = Rotation.from_euler( + "xyz", np.fromstring(origin.get("rpy", "0 0 0"), sep=" ") + ).as_matrix() + matrix[:3, 3] = np.fromstring(origin.get("xyz", "0 0 0"), sep=" ") + matrix[:3, :3] *= np.fromstring(mesh.get("scale", "1 1 1"), sep=" ") + color = visual.find("material/color") + rgba = ( + tuple(np.fromstring(color.get("rgba"), sep=" ")) + if color is not None + else (0.82, 0.85, 0.89, 1.0) + ) + parts.append((urdf.parent / mesh.get("filename"), matrix, rgba)) + if parts: + visuals[name] = parts + if not visuals: + raise ValueError(f"No {side} hand visuals in {urdf}") + return visuals + + +def robot_display_transform(side): + matrix = np.eye(4) + # Robot palm +X points up; fingertips +Z point toward the back of the bench. + matrix[:3, :3] = ((0, 1, 0), (0, 0, 1), (1, 0, 0)) + matrix[:3, 3] = (-0.19 if side == "left" else 0.19, -0.09, 0.15) + return matrix + + +class RobotHandScene: + def __init__(self, stage, asset_root, model): + import trimesh + from pxr import Gf, UsdGeom, Vt + + self._gf = Gf + self._transforms = {} + self._bases = {} + self._roots = {} + for side in ("left", "right"): + path = f"/World/{side.title()}RobotHand" + root = UsdGeom.Xform.Define(stage, path) + root.MakeInvisible() + self._roots[side] = root + self._bases[side] = robot_display_transform(side) + UsdGeom.Xformable(root).AddTransformOp().Set( + Gf.Matrix4d(self._bases[side].T.tolist()) + ) + transforms = {} + for name, parts in visual_links(asset_root, model, side).items(): + link = UsdGeom.Xform.Define(stage, f"{path}/{name}") + transforms[name] = UsdGeom.Xformable(link).AddTransformOp() + for index, (filename, matrix, color) in enumerate(parts): + data = trimesh.load_mesh(filename, force="mesh", process=False) + geometry = UsdGeom.Mesh.Define( + stage, f"{path}/{name}/Visual_{index}" + ) + geometry.CreatePointsAttr( + Vt.Vec3fArray.FromNumpy( + np.asarray(data.vertices, dtype=np.float32) + ) + ) + geometry.CreateFaceVertexCountsAttr([3] * len(data.faces)) + geometry.CreateFaceVertexIndicesAttr( + np.asarray(data.faces).flatten().tolist() + ) + geometry.CreateSubdivisionSchemeAttr(UsdGeom.Tokens.none) + geometry.CreateDoubleSidedAttr(True) + geometry.CreateDisplayColorAttr([Gf.Vec3f(*color[:3])]) + UsdGeom.Xformable(geometry).AddTransformOp().Set( + Gf.Matrix4d(matrix.T.tolist()) + ) + self._transforms[side] = transforms + + def update(self, robots): + for side, transforms in self._transforms.items(): + poses = robots.get(side, {}).get("link_poses", {}) + if poses: + self._roots[side].MakeVisible() + for name, operation in transforms.items(): + if name in poses: + matrix = np.asarray(poses[name], dtype=float) + if matrix.shape != (4, 4) or not np.all(np.isfinite(matrix)): + raise ValueError(f"Invalid link transform for {name}") + operation.Set(self._gf.Matrix4d(matrix.T.tolist())) diff --git a/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_thumb_calibration.py b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_thumb_calibration.py new file mode 100644 index 0000000000..c422182863 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/realhand_ffg_thumb_calibration.py @@ -0,0 +1,424 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Fit a continuous thumb-only sensor map against human fingertip geometry.""" + +from __future__ import annotations + +import hashlib +import json + +import numpy as np +from scipy.optimize import least_squares, minimize +from scipy.spatial.transform import Rotation + +CHANNELS = (0, 1, 2, 4) +POSES = ( + "open", + "fist", + "thumb_curl", + "thumb_index", + "thumb_middle", + "thumb_ring", + "thumb_little", +) +FORMAT = "realhand-ffg-thumb-quadratic-v1" +FIT_MODEL = "noise-regularized-contact-surface-v6" +SENSOR_GAIN_SCALE = 2.0 +OPEN_REPEAT_TOLERANCE_M = 0.015 +CONTACT_MAX_DISTANCE_M = 0.035 +CURVATURE_SCALE = 0.3 +SENSOR_NOISE_RAD = np.deg2rad(1.0) +LOWER = np.array((-1.25, -1.2, 0.0, 0.0)) +UPPER = np.array((1.0, 1.2, 1.8, 1.8)) +BASE = np.array((0.025, -0.005, -0.020)) +LENGTHS = np.array((0.035, 0.030, 0.026)) +OPEN_ANGLES = np.array((-0.85, 0.0, 0.0, 0.0)) + + +class ThumbFitError(ValueError): + def __init__(self, message, report): + super().__init__(message) + self.report = report + + +def quadratic_features(inputs): + inputs = np.asarray(inputs) + products = [ + inputs[..., first] * inputs[..., second] + for first in range(4) + for second in range(first, 4) + ] + return np.concatenate((inputs, np.stack(products, axis=-1)), axis=-1) + + +def quadratic_feature_jacobian(inputs): + inputs = np.asarray(inputs, dtype=float) + linear = np.broadcast_to(np.eye(4), (*inputs.shape[:-1], 4, 4)) + products = [] + for first in range(4): + for second in range(first, 4): + gradient = np.zeros_like(inputs) + gradient[..., first] += inputs[..., second] + gradient[..., second] += inputs[..., first] + products.append(gradient) + return np.concatenate((linear, np.stack(products, axis=-2)), axis=-2) + + +def angle_delta(values, origin): + return (np.asarray(values) - origin + np.pi) % (2 * np.pi) - np.pi + + +def calibration_fingerprint(poses, side="left"): + payload = {key: poses[key] for key in (*POSES, "thumb_opposition", "spread")} + if "open_repeat" in poses: + payload["open_repeat"] = poses["open_repeat"] + if "thumb_observations" in poses: + payload["thumb_observations"] = poses["thumb_observations"] + payload["fit_model"] = FIT_MODEL + payload["sensor_gain_scale"] = SENSOR_GAIN_SCALE + payload["sensor_noise_rad"] = float(SENSOR_NOISE_RAD) + payload["curvature_scale"] = CURVATURE_SCALE + if side == "right": + payload["lateral_sensor_convention"] = "right-mirrored-v1" + return hashlib.sha256(json.dumps(payload, sort_keys=True).encode()).hexdigest() + + +def _rotation(angle, axis): + vector = np.zeros(3) + vector[axis] = angle + return Rotation.from_rotvec(vector).as_matrix() + + +def _response(value, origin, endpoint): + span = angle_delta(endpoint, origin) + center = span * 0.5 + delta = center + angle_delta(angle_delta(value, origin), center) + return np.clip(delta / np.where(np.abs(span) >= 0.05, span, 1), 0, 1) * ( + np.abs(span) >= 0.05 + ) + + +def finger_joints(poses, values, finger, side="left"): + """Canonical left-hand FK, kept equivalent to the native finger reconstruction.""" + sensor = 5 + 4 * finger + origin = np.asarray(poses["open"]) + fist = np.asarray(poses["fist"]) + span = abs(angle_delta(poses["spread"][sensor], origin[sensor])) + direction = 1 if side == "left" else -1 + spread = np.clip( + direction * angle_delta(values[sensor], origin[sensor]) / max(span, 0.10), -1, 1 + ) + spread *= (0.25, 0.08, 0.12, 0.30)[finger] + base = _response(values[sensor + 1], origin[sensor + 1], fist[sensor + 1]) * 1.5 + distal = _response(values[sensor + 3], origin[sensor + 3], fist[sensor + 3]) * 1.1 + point = np.array(((0.033, 0.011, -0.011, -0.031)[finger], 0, -0.015)) + rotation = _rotation(spread, 1) + point += rotation @ np.array((0, 0, -(0.070, 0.075, 0.070, 0.060)[finger] + 0.015)) + lengths = ( + (0.045, 0.026, 0.018), + (0.050, 0.030, 0.019), + (0.046, 0.027, 0.018), + (0.034, 0.020, 0.016), + )[finger] + points = [point.copy()] + for angle, length in zip((base, distal * 1.5, distal), lengths): + rotation = rotation @ _rotation(-angle, 0) + point += rotation @ np.array((0, 0, -length)) + points.append(point.copy()) + return np.asarray(points) + + +def finger_tip(poses, values, finger, side="left"): + return finger_joints(poses, values, finger, side)[-1] + + +def thumb_joints(angles, base=BASE, lengths=LENGTHS): + rotation = _rotation(angles[0], 1) @ _rotation(angles[1], 2) + point = np.asarray(base, dtype=float).copy() + points = [point.copy()] + for bend, length in zip((0, angles[2], angles[3]), lengths): + rotation = rotation @ _rotation(-bend, 0) + point += rotation @ np.array((0, 0, -length)) + points.append(point.copy()) + return np.asarray(points) + + +def thumb_tip_positions(angles, lengths=LENGTHS): + """Batched equivalent of thumb_joints(...)[-1].""" + a, b, c, d = np.asarray(angles).T + y = -lengths[1] * np.sin(c) - lengths[2] * np.sin(c + d) + z = -lengths[0] - lengths[1] * np.cos(c) - lengths[2] * np.cos(c + d) + x = -np.sin(b) * y + return BASE + np.column_stack( + (np.cos(a) * x + np.sin(a) * z, np.cos(b) * y, -np.sin(a) * x + np.cos(a) * z) + ) + + +def mapped_angles(mapping, values): + raw = np.asarray(values)[list(CHANNELS)] + minimum = np.asarray(mapping["input_min"]) + maximum = np.asarray(mapping["input_max"]) + center = (minimum + maximum) * 0.5 + delta = center + angle_delta( + angle_delta(raw, np.asarray(mapping["input_origin"])), center + ) + inputs = np.clip(delta, minimum, maximum) / mapping["input_scale"] + return np.clip( + np.asarray(mapping["coefficients"]) @ np.r_[1.0, quadratic_features(inputs)], + LOWER, + UPPER, + ) + + +def fit_thumb_mapping(poses, side="left"): + if side not in ("left", "right"): + raise ValueError("Thumb calibration side must be left or right") + values = np.asarray([poses[key] for key in POSES], dtype=float) + if values.shape != (7, 21) or not np.all(np.isfinite(values)): + raise ValueError("Thumb fitting requires seven finite 21-angle captures") + observations = [{"pose": key, "values": poses[key]} for key in POSES] + if "open_repeat" in poses: + observations.append({"pose": "open", "values": poses["open_repeat"]}) + observations.extend(poses.get("thumb_observations", ())) + if any(s.get("pose") not in POSES for s in observations): + raise ValueError("Thumb observations must use the calibration pose names") + for sample in observations: + row = np.asarray(sample["values"], dtype=float) + if row.shape != (21,) or not np.all(np.isfinite(row)): + raise ValueError("Thumb observations must contain 21 finite angles") + raw = np.asarray([s["values"] for s in observations], dtype=float) + if raw.shape != (len(observations), 21) or not np.all(np.isfinite(raw)): + raise ValueError("Thumb observations must contain 21 finite angles") + names = np.asarray([s["pose"] for s in observations]) + origin = values[0, list(CHANNELS)] + # Canonical relative inputs keep numerical optimizers invariant to constant sensor zero offsets. + # Twelve decimals are well below the native float32 sensor resolution. + deltas = np.round(angle_delta(raw[:, list(CHANNELS)], origin), 12) + if np.any(np.ptp(deltas, axis=0) >= np.pi): + raise ValueError( + "Thumb captures span an ambiguous angular arc; repeat calibration" + ) + minimum = np.min(deltas, axis=0) - 2 * SENSOR_NOISE_RAD + maximum = np.max(deltas, axis=0) + 2 * SENSOR_NOISE_RAD + if np.any(maximum - minimum >= np.pi): + raise ValueError("Thumb input uncertainty spans an ambiguous angular arc") + # Native loaders require the normalization scale to cover the full clipped arc, including uncertainty. + scales = np.maximum(np.maximum(np.abs(minimum), np.abs(maximum)), 0.10) + normalized = deltas / scales + features = quadratic_features(normalized) + anchor_features = features[3:7] + if ( + min( + np.linalg.norm(a - b) + for i, a in enumerate(anchor_features) + for b in anchor_features[i + 1 :] + ) + < 0.08 + ): + raise ValueError( + "Thumb contact captures are indistinguishable; distinct thumb readings are required" + ) + is_contact = np.isin(names, POSES[3:]) + contact_raw = raw[is_contact] + contact_names = names[is_contact] + targets = np.round( + np.asarray( + [ + finger_tip(poses, v, POSES[3:].index(n), side) + for v, n in zip(contact_raw, contact_names) + ] + ), + 12, + ) + directions = BASE - targets + weak_goals = ( + targets + directions / np.linalg.norm(directions, axis=1)[:, None] * 0.012 + ) + is_open = names == "open" + is_endpoint = np.isin(names, ("fist", "thumb_curl")) + prior = np.column_stack( + ( + -0.85 + 1.2 * _response(raw[:, 0], origin[0], poses["thumb_opposition"][0]), + -0.7 * _response(raw[:, 1], origin[1], poses["thumb_opposition"][1]), + 1.1 * _response(raw[:, 2], origin[2], poses["thumb_curl"][2]), + 1.2 * _response(raw[:, 4], origin[3], poses["thumb_curl"][4]), + ) + ) + prior[is_open] = OPEN_ANGLES + prior = np.round(prior, 12) + linear = np.linalg.lstsq(normalized, prior - OPEN_ANGLES, rcond=None)[0].T + initial = np.r_[ + np.column_stack((np.clip(linear, -1.7, 1.7), np.zeros((4, 10)))).ravel(), 1.1 + ] + n = int(np.sum(is_contact)) + noise = np.random.default_rng(29).normal(0, SENSOR_NOISE_RAD, (n, 4, 4)) + noisy_inputs = normalized[is_contact, None, :] + noise / scales + noisy_features = quadratic_features(noisy_inputs.reshape(-1, 4)) + sensor_jacobians = quadratic_feature_jacobian(normalized[::3]) / scales + # Weak curvature and cross-channel shrinkage limit correlated-sensor overfitting. + features[:, 4:] *= CURVATURE_SCALE + noisy_features[:, 4:] *= CURVATURE_SCALE + sensor_jacobians[:, 4:] *= CURVATURE_SCALE + + def unpack(p): + weights = p[:56].reshape(4, 14) + q = OPEN_ANGLES + features @ weights.T + return weights, q, LENGTHS * p[56] + + def residual(p): + weights, q, lengths = unpack(p) + tips = thumb_tip_positions(np.clip(q[is_contact], LOWER, UPPER), lengths) + noisy_q = np.clip(OPEN_ANGLES + noisy_features @ weights.T, LOWER, UPPER) + jac = np.einsum("ij,njk->nik", weights, sensor_jacobians) + return np.r_[ + # Contact fixes surface distance, not an unmeasured normal toward the thumb root. + ( + (np.linalg.norm(tips - targets, axis=1) - 0.012) / 0.0015 / np.sqrt(n) + ).ravel(), + ((tips - weak_goals) / 0.04 / np.sqrt(n)).ravel(), + ( + (thumb_tip_positions(noisy_q, lengths) - np.repeat(tips, 4, axis=0)) + / 0.003 + / np.sqrt(n * 4) + ).ravel(), + ((q[is_open] - prior[is_open]) / 0.12 / np.sqrt(np.sum(is_open))).ravel(), + ( + (q[is_endpoint] - prior[is_endpoint]) + / 0.10 + / np.sqrt(np.sum(is_endpoint)) + ).ravel(), + (weights / np.r_[np.full(4, 2.0), np.full(10, 0.6)] / np.sqrt(56)).ravel(), + (jac / SENSOR_GAIN_SCALE / np.sqrt(len(jac))).ravel(), + (jac * (1 - np.eye(4)) / np.sqrt(len(jac))).ravel(), + (np.maximum(LOWER - q, 0) / 0.05 / np.sqrt(len(q))).ravel(), + (np.maximum(q - UPPER, 0) / 0.05 / np.sqrt(len(q))).ravel(), + (p[56] - 1) / 0.2, + ] + + bounds = (np.r_[np.full(56, -1.75), 0.85], np.r_[np.full(56, 1.75), 1.15]) + result = least_squares(residual, initial, bounds=bounds, max_nfev=500, ftol=1e-6) + curl_features = quadratic_features( + np.round( + angle_delta(np.asarray(poses["thumb_curl"])[list(CHANNELS)], origin), 12 + ) + / scales + ) + curl_features[4:] *= CURVATURE_SCALE + + def curl_quality(p): + return (OPEN_ANGLES + curl_features @ p[:56].reshape(4, 14).T)[2:] - ( + 0.92, + 1.02, + ) + + repeat_features = None + if "open_repeat" in poses: + repeated = ( + np.round( + angle_delta(np.asarray(poses["open_repeat"])[list(CHANNELS)], origin), + 12, + ) + / scales + ) + repeat_features = quadratic_features(repeated) + repeat_features[4:] *= CURVATURE_SCALE + + def quality(p): + constraints = curl_quality(p).tolist() + if repeat_features is not None: + q = np.clip( + OPEN_ANGLES + repeat_features @ p[:56].reshape(4, 14).T, LOWER, UPPER + ) + length = LENGTHS * p[56] + shift = np.linalg.norm( + thumb_tip_positions(q[None], length)[0] + - thumb_tip_positions(OPEN_ANGLES[None], length)[0] + ) + constraints.append(1 - (shift / (OPEN_REPEAT_TOLERANCE_M * 0.95)) ** 2) + return np.asarray(constraints) + + if np.all(np.isfinite(result.x)) and np.min(quality(result.x)) < 0: + result = minimize( + lambda p: np.mean(residual(p) ** 2), + result.x, + method="SLSQP", + bounds=list(zip(*bounds)), + constraints={"type": "ineq", "fun": quality}, + options={"maxiter": 250, "ftol": 1e-8}, + ) + if not np.all(np.isfinite(result.x)): + raise ThumbFitError( + f"{side.upper()} thumb fit produced non-finite coefficients", {"side": side} + ) + weights, _, lengths = unpack(result.x) + weights = weights.copy() + weights[:, 4:] *= CURVATURE_SCALE + mapping = { + "format": FORMAT, + "fit_model": FIT_MODEL, + "sensor_gain_scale": SENSOR_GAIN_SCALE, + "calibration_sha256": calibration_fingerprint(poses, side), + "input_origin": origin.tolist(), + "input_scale": scales.tolist(), + "input_min": minimum.tolist(), + "input_max": maximum.tolist(), + "coefficients": np.column_stack((OPEN_ANGLES, weights)).tolist(), + "base_position": BASE.tolist(), + "lengths": lengths.tolist(), + } + gaps = np.linalg.norm( + thumb_tip_positions( + np.asarray([mapped_angles(mapping, v) for v in contact_raw]), lengths + ) + - targets, + axis=1, + ) + anchors = [] + for name in POSES[3:]: + v = poses[name] + anchors.append( + float( + np.linalg.norm( + thumb_joints(mapped_angles(mapping, v), BASE, lengths)[-1] + - finger_tip(poses, v, POSES[3:].index(name), side) + ) + ) + ) + report = { + "side": side, + "solver": "noise_regularized_contact_surface", + "solver_converged": bool(result.success), + "solver_message": str(result.message), + "observation_count": len(raw), + "tip_center_distances_mm": (np.asarray(anchors) * 1000).tolist(), + "contact_surface_error_mm": ( + np.abs(np.asarray(anchors) - 0.012) * 1000 + ).tolist(), + "contact_surface_error_before_mm": ( + np.abs( + np.linalg.norm(thumb_tip_positions(prior[3:7]) - targets[:4], axis=1) + - 0.012 + ) + * 1000 + ).tolist(), + "contact_gap_p95_mm": float(np.percentile(gaps, 95) * 1000), + "training_only": True, + } + if "open_repeat" in poses: + p = thumb_joints(mapped_angles(mapping, poses["open_repeat"]), BASE, lengths)[ + -1 + ] + report["repeat_open_training_tip_mm"] = float( + np.linalg.norm(p - thumb_joints(OPEN_ANGLES, BASE, lengths)[-1]) * 1000 + ) + mapping["fit_report"] = report + if ( + np.percentile(gaps, 95) > CONTACT_MAX_DISTANCE_M + or np.min(quality(result.x)) < -0.005 + ): + raise ThumbFitError( + f"{side.upper()} thumb fit fails repeated-contact/curl quality checks; existing calibration is unchanged", + report, + ) + return mapping diff --git a/examples/teleop/python/realhand_ffg_glove_calibration.py b/examples/teleop/python/realhand_ffg_glove_calibration.py deleted file mode 100644 index ee20ba4f4f..0000000000 --- a/examples/teleop/python/realhand_ffg_glove_calibration.py +++ /dev/null @@ -1,205 +0,0 @@ -# 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/realhand_ffg_hand_demo.md b/examples/teleop/realhand_ffg_hand_demo.md new file mode 100644 index 0000000000..682c728401 --- /dev/null +++ b/examples/teleop/realhand_ffg_hand_demo.md @@ -0,0 +1,116 @@ + + +# RealHand FFG Hand Validation + +By default, this optional Isaac Sim example displays the plugin's two **human 26-joint OpenXR +skeletons**, not a robot retargeter. Left is blue, right is coral. Wrists remain +fixed in the viewer; fingers follow the FFG stream directly. No P7, robot IK, +mechanical-hand collision, or headset connection is involved. Isaac Sim and +the local OpenXR runtime are required; they are not added as package dependencies. + +The skeleton is estimated from glove angles and approximate human dimensions. +This demo checks device reconstruction before introducing robot-hand retargeting. +It does not test physical contacts or mechanical-hand accuracy. + +To display actual L6, O6, or L20 mesh hands driven by the official +`DexHandRetargeter`, use `--hand-model l6|o6|l20`. See +[the three robot pose verification scenes](realhand_ffg_robot_scenes.md) for +setup, calibration, and individual launch commands. + +## Local checkout + +Activate your Isaac Sim Python environment first, then substitute your checkout path: + +```bash +cd /path/to/IsaacCapture +bash examples/teleop/run_realhand_ffg_hand_demo.sh --ports=/dev/ttyUSB0 +``` + +For two gloves use `--ports=/dev/ttyUSB0,/dev/ttyUSB1`. Firmware identifies left +and right; port numbering does not determine the hand side. Only verified FFG +ports should be passed. USB access still requires Linux serial permissions. +Omitting `--ports` enables the plugin's restricted discovery of USB serial +identifiers containing RealHand, RealForce, or FFG. Generic USB UART identifiers +require explicit verified ports; omission never defaults to `/dev/ttyUSB0`. + +The wrapper builds the current FFG plugin, stages it in a user cache, and uses +the locally built Python package. Its separate DeviceIO process supports a +different Python ABI from Isaac Sim. Set `REALHAND_CAPTURE_PYTHON` to another +matching Isaac Capture interpreter when needed; the default is the checkout's +`.venv/bin/python`, set up using the main build guide. Default build configuration is +`Release`; override `REALHAND_CAPTURE_BUILD_CONFIG` for a different configuration. + +The background CloudXR/OpenXR runtime starts automatically or attaches to an +existing compatible service. Do not launch another FFG reader on the same USB +port. The bundled `.env` uses the official [standalone CloudXR profile](https://isaac-sim.github.io/IsaacLab/develop/source/features/isaac_teleop.html#cloudxr-environment-profiles) +(`quest3`) with push-device injection enabled. No PICO client is needed. +If an existing runtime uses another profile, stop it before starting this demo; +the launcher cannot reconfigure a service that is already running. +If using a separately configured compatible +runtime, source its environment and pass `--no-launch-cloudxr-runtime`. +The CloudXR service outlives the viewer. Stop it when finished using the +Capture interpreter and the built package: + +```bash +PYTHONPATH=build/python_package/Release \ + .venv/bin/python -m isaaccapture.cloudxr.service stop +``` + +The Capture interpreter also needs the upstream CloudXR optional dependencies: +`cryptography>=3.0` and `websockets>=14.0`. +The offline calibration fitter requires NumPy and SciPy in that interpreter. + +## PC Calibration Window + +Use the **RealHand FFG Calibration** window in Isaac Sim. +If closed, reopen it from **Window > RealHand FFG Calibration**. + +1. Select Left, Right, or Both according to the connected gloves. +2. Click **Start Calibration**. +3. Click **Capture Pose**, release the mouse during the 8-second **PREPARE** countdown, + then keep the displayed pose still for the 6-second **HOLD**. +4. Complete the 14-step sequence. The complete file is validated, saved, and applied automatically. + +The PC window and `realhand_ffg_glove_calibration` command use the same human +calibration workflow used before CTS: open palm, fist, thumb-only curl, thumb +opposition, finger spread, four thumb-to-fingertip contacts, a second set of those +contacts, and repeated open palm. See the [device page](../../docs/source/device/realhand_ffg_glove.rst) +for pose instructions. Both interfaces preserve repeated samples for fitting and +require at least 120 samples per hold. Calibration is not a CTS test result. + +L6, O6, L20, and the human skeleton view share this workflow and file. There is no +model-specific calibration. For Both, hold the same pose on both hands simultaneously. +Fingers remain independently driven; contact poses train the calibration, not runtime snapping. + +**Cancel Capture** cancels the whole calibration without replacing the +saved file. Saving happens only after all captures and fitting succeed. + +The viewer uses solid cylindrical bones between joint spheres. Palms face up; +only the viewer orientation is changed, not the input or calibration coordinates. + +The default file is `~/.config/realhand/ffg_human_demo.yml`. Use `--calibration` +to select another **human-calibration-v1** file. Stable-release robot-specific +FFG calibration files are not compatible. Each side has its own measurements; +calibrating one side preserves the other. Invalid or cancelled captures do not +replace a working file. Existing calibrations, including files used for CTS, are +loaded without refitting or rewriting; switching robot models does not require +recalibration. Recalibrate after re-wearing or power-cycling the gloves. +A missing or stale hand stream is hidden, not frozen. + +## Rendering Test Without Hardware + +Generate fixtures from the production C++ skeleton and display them without +starting OpenXR or accessing a USB device: + +```bash +cmake --build build --target test_realhand_ffg_glove_protocol +build/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_protocol \ + --write-skeleton-fixtures /tmp/ffg_skeleton.yml +PYTHONPATH=examples/teleop/python python -m isaaccapture_examples.teleop.realhand_ffg_hand_demo \ + --test-frames /tmp/ffg_skeleton.yml +``` + +Live calibration is disabled in fixture mode. diff --git a/examples/teleop/realhand_ffg_robot_scenes.md b/examples/teleop/realhand_ffg_robot_scenes.md new file mode 100644 index 0000000000..86f87e454c --- /dev/null +++ b/examples/teleop/realhand_ffg_robot_scenes.md @@ -0,0 +1,91 @@ + + +# L6 / O6 / L20 FFG Pose Verification Scenes + +Each scene contains the selected model's left and right mesh hands above a small +workbench. Wrists are fixed and palms face upward. The displayed joints follow +FFG -> calibrated standard OpenXR hand -> official `DexHandRetargeter`. +There is no raw-glove robot mapping or gesture snapping. + +These are **kinematic pose tests**, not grasping or contact-physics tests. +No dynamics or self-collision response is simulated. This isolates retargeting +quality from actuator lag and contacts; the full robot environment is needed +to verify those effects. + +## Setup + +From the IsaacCapture checkout, use an Isaac Sim Python environment with +`numpy`, `scipy`, `trimesh`, `pyyaml`, and `Pillow`. +Set `REALHAND_CAPTURE_PYTHON` to a separate interpreter containing the locally +built IsaacCapture package and its `retargeters` extra. Its Python ABI must match +the CMake build. The launcher uses the checkout's `.venv/bin/python` by +default and builds/installs the FFG plugin automatically. + +Download the verified public meshes once: + +```bash +PYTHONPATH=examples/teleop/python \ +python -m isaaccapture_examples.teleop.fetch_realhand_assets +``` + +## Three Scenes + +Run one scene at a time, closing an existing FFG demo before launching another. +An existing demo may own the serial ports; never run two glove readers together. + +```bash +bash examples/teleop/run_realhand_ffg_hand_demo.sh --hand-model l6 +``` + +```bash +bash examples/teleop/run_realhand_ffg_hand_demo.sh --hand-model o6 +``` + +```bash +bash examples/teleop/run_realhand_ffg_hand_demo.sh --hand-model l20 +``` + +Native discovery is used by default. If your USB adapter has no recognized +FFG identifier, append your verified glove ports, for example +`--ports=/dev/ttyUSB0,/dev/ttyUSB1`. Do not select unrelated serial devices. +USB permission errors are shown in the PC window; the launcher does not use sudo. + +The emulated local OpenXR runtime is started by the existing launcher. A headset +and controller are not required for these fixed-wrist tests. +The demo uses `--wrist-source=auto` so a missing wrist tracker does not suppress +the local finger skeleton. This does not change the tracked-wrist requirements of CTS. + +## Calibration + +The **RealHand FFG Calibration** PC window is shared with the human skeleton demo. +All three models use the same 14-step human calibration used before CTS, also +available through `realhand_ffg_glove_calibration`. There is no L20-specific sequence. +Choose Left, Right, or Both; click **Start Calibration**, then **Capture Pose** for +each step. Each capture has 8 seconds to prepare and a 6-second hold. +See the [shared calibration instructions](realhand_ffg_hand_demo.md#pc-calibration-window). +Complete all steps even when the mechanical hand cannot reproduce every human contact. + +The default file is `~/.config/realhand/ffg_human_demo.yml`. An existing valid +human calibration can be reused across all three models; model-specific legacy +FFG files cannot. Append `--calibration /path/to/human.yml` to select another file. +An existing CTS human calibration is loaded unchanged, without automatic refitting. +Saving restarts only the device session and activates the new calibration. +Reopen a closed window from **Window > RealHand FFG Calibration**. + +A missing glove shows **not found** and its robot hand stays open. Connected +gloves are independent; testing only one glove is supported. + +## Offline Verification + +Generate native skeleton fixtures and render them without opening USB or CloudXR: + +```bash +build/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_protocol \ + --write-skeleton-fixtures /tmp/ffg-skeletons.yml +bash examples/teleop/run_realhand_ffg_hand_demo.sh \ + --hand-model l20 --test-frames /tmp/ffg-skeletons.yml \ + --headless --frames 90 --screenshot /tmp/l20-ffg.png +``` diff --git a/examples/teleop/run_realhand_ffg_hand_demo.sh b/examples/teleop/run_realhand_ffg_hand_demo.sh new file mode 100755 index 0000000000..0ea2214bd2 --- /dev/null +++ b/examples/teleop/run_realhand_ffg_hand_demo.sh @@ -0,0 +1,30 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail +# Small hand solves share the CPU with RTX; avoid unbounded OpenMP fan-out. +export OMP_NUM_THREADS="${OMP_NUM_THREADS:-1}" +SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)" +REPO="$(cd -- "$SCRIPT_DIR/../.." && pwd)" +BUILD="${REALHAND_CAPTURE_BUILD:-$REPO/build}" +DEMO_CACHE="${XDG_CACHE_HOME:-$HOME/.cache}/realhand_ffg_hand_demo" +CAPTURE_PYTHON="${REALHAND_CAPTURE_PYTHON:-$REPO/.venv/bin/python}" + +if [[ ! -x "$CAPTURE_PYTHON" ]]; then + printf 'Capture Python not found: %s\nSet REALHAND_CAPTURE_PYTHON to a matching IsaacCapture interpreter.\n' "$CAPTURE_PYTHON" >&2 + exit 1 +fi +if [[ ! -f "$BUILD/CMakeCache.txt" ]]; then + printf 'Configure the IsaacCapture build first: cmake -S "%s" -B "%s"\n' "$REPO" "$BUILD" >&2 + exit 1 +fi +cmake --build "$BUILD" --target realhand_ffg_glove_plugin python_package -j 4 +cmake --install "$BUILD" --component realhand_ffg_glove --prefix "$DEMO_CACHE/install" +BUILD_CONFIG="${REALHAND_CAPTURE_BUILD_CONFIG:-Release}" +export PYTHONPATH="$SCRIPT_DIR/python${PYTHONPATH:+:$PYTHONPATH}" +exec python -m isaaccapture_examples.teleop.realhand_ffg_hand_demo \ + --capture-python "$CAPTURE_PYTHON" \ + --capture-package "$BUILD/python_package/$BUILD_CONFIG" \ + --plugin-path "$DEMO_CACHE/install/plugins" \ + "$@" diff --git a/pyproject.toml b/pyproject.toml index 4b64e1f40d..4f13726860 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -68,6 +68,7 @@ desktop-mirror = [ ] retargeters = [ "dex-retargeting>=0.4.6,<0.6.0", + "six>=1.16", "scipy>=1.15.0", "pyyaml>=6.0.3", "torch>=2.7.0", diff --git a/src/core/python/requirements-retargeters.txt b/src/core/python/requirements-retargeters.txt index 27ed26b2f3..3b521c4106 100644 --- a/src/core/python/requirements-retargeters.txt +++ b/src/core/python/requirements-retargeters.txt @@ -2,6 +2,8 @@ # SPDX-License-Identifier: Apache-2.0 dex-retargeting>=0.4.6,<0.6.0 +# dex-retargeting's bundled yourdfpy imports six without declaring it. +six>=1.16 scipy>=1.15.0 pyyaml>=6.0.3 torch>=2.7.0 diff --git a/src/plugins/realhand_ffg_glove/CMakeLists.txt b/src/plugins/realhand_ffg_glove/CMakeLists.txt index 72d7aa5d04..df0fc52ca7 100644 --- a/src/plugins/realhand_ffg_glove/CMakeLists.txt +++ b/src/plugins/realhand_ffg_glove/CMakeLists.txt @@ -4,6 +4,8 @@ add_executable(realhand_ffg_glove_plugin main.cpp realhand_ffg_glove_plugin.cpp + realhand_ffg_glove_reader.cpp + realhand_ffg_glove_skeleton.cpp realhand_ffg_glove_protocol.cpp realhand_ffg_glove_serial.cpp ) @@ -13,6 +15,8 @@ target_link_libraries(realhand_ffg_glove_plugin PRIVATE pusherio::pusherio oxr::oxr_core isaacteleop_schema + log_bridge::log_bridge_core + yaml-cpp::yaml-cpp ) install(TARGETS realhand_ffg_glove_plugin diff --git a/src/plugins/realhand_ffg_glove/README.md b/src/plugins/realhand_ffg_glove/README.md index dbdd2d1f8a..5138421dbb 100644 --- a/src/plugins/realhand_ffg_glove/README.md +++ b/src/plugins/realhand_ffg_glove/README.md @@ -5,11 +5,12 @@ SPDX-License-Identifier: Apache-2.0 # 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. +Publishes one or two RealHand FFG Gloves as standard 26-joint OpenXR hands at +`/hand/left` and `/hand/right`, using the official `HandInjector` and `WristPoseSource` +utilities. `HandsSource` feeds compatible `HandInput` retargeters independently of +the target robot. Raw sensor collections are for calibration and diagnostics only. +The plugin queries each glove's firmware-reported hand side. +A single glove is supported; the absent side remains inactive. The serial protocol implementation follows the Apache-2.0 [RealHand FFG Glove pure-Python SDK](https://github.com/RealHand-Robotics/FFG_realhand_pure_python). @@ -28,13 +29,16 @@ cmake --build build --target 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: +Automatic discovery only considers `/dev/serial/by-id/` entries whose names identify +RealHand, RealForce, or FFG hardware. Generic USB UART names do not identify a glove; +explicitly select the glove ports in that case: ```bash realhand_ffg_glove_plugin --ports=/dev/ttyUSB0,/dev/ttyUSB1 --baudrates=2000000,460800 ``` +Use only verified glove ports and close other applications reading the same devices. + On Linux, the user must be able to open the serial device. Common distributions grant this via the `dialout` group: @@ -42,8 +46,42 @@ the `dialout` group: sudo usermod -aG dialout "$USER" ``` -Log out and back in after changing group membership. Do not run the Isaac Teleop process as root. +Log out and back in after changing group membership. Do not run the IsaacCapture process as root. + +## Human calibration and tracking + +Without human calibration, raw sensor collections remain available for calibration +capture and diagnostics, but the human-hand outputs remain inactive. +They use `JointStateSource`, IDs `realhand_ffg_glove_left` / `realhand_ffg_glove_right`, +and names `sensor_0` through `sensor_20` (radians). + +```bash +pip install -e ./examples/teleop +python -m isaaccapture_examples.teleop.realhand_ffg_glove_calibration \ + --side left --output /path/to/ffg_human.yml \ + --plugin-path install/plugins --realhand-ffg-glove-ports=/dev/ttyUSB0 +realhand_ffg_glove_plugin --ports=/dev/ttyUSB0 \ + --calibration=/path/to/ffg_human.yml --wrist-source=controller +``` + +Capture open palm, full fist, thumb-only curl, thumb opposition, and finger spread, +then the four thumb-to-fingertip contacts, a second set of those contacts, and a +repeated open palm. Each step waits for Enter, allows 8 seconds to prepare, and +records a 6-second hold. Calibration requires NumPy and SciPy. Failed captures +leave the previous file intact. +Calibration is side-specific and robot-independent. Files use +`realhand-ffg-glove-human-calibration-v1`; legacy robot calibration is rejected. +The PC hand demos share this workflow and reuse one calibration for L6, O6, and L20. +Recalibrate after re-wearing or power-cycling a glove. Human joint poses are +estimated from glove angles; `--hand-scale=1.0` adjusts the estimated hand size. + +`--wrist-source=auto` prefers native hand tracking, then controller aim pose; `controller` +and `handtracking` select a source explicitly. Default offsets support PICO 4 Ultra +controllers mounted upright on the RealHand FFG hand-back bracket, with mirrored +left/right installation. Override a different mount with +`--left-aim-to-wrist=px,py,pz,qx,qy,qz,qw` and the corresponding right option +(meters in the aim frame, XYZW quaternion). In controller mode, lost or jumping +wrist tracking makes the hand inactive until tracking recovers. -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. +See `docs/source/device/realhand_ffg_glove.rst` for calibration poses and coordinate details. +For verification, see the official [Hand CTS](https://nvidia.github.io/IsaacCapture/main/conformance/hand.html). diff --git a/src/plugins/realhand_ffg_glove/main.cpp b/src/plugins/realhand_ffg_glove/main.cpp index 9b46f315d0..a9c0191b2e 100644 --- a/src/plugins/realhand_ffg_glove/main.cpp +++ b/src/plugins/realhand_ffg_glove/main.cpp @@ -3,10 +3,12 @@ #include "realhand_ffg_glove_plugin.hpp" +#include + #include #include +#include #include -#include #include #include #include @@ -51,6 +53,31 @@ std::vector parse_baudrates(const std::string& value) return result; } +XrPosef parse_pose(const std::string& value) +{ + const auto fields = split_csv(value); + if (fields.size() != 7) + { + throw std::invalid_argument("wrist offset must be px,py,pz,qx,qy,qz,qw (meters, XYZW)"); + } + std::array v{}; + for (std::size_t i = 0; i < v.size(); ++i) + { + std::size_t consumed = 0; + v[i] = std::stof(fields[i], &consumed); + if (consumed != fields[i].size() || !std::isfinite(v[i])) + { + throw std::invalid_argument("wrist offset must contain finite numbers"); + } + } + const float norm = std::sqrt(v[3] * v[3] + v[4] * v[4] + v[5] * v[5] + v[6] * v[6]); + if (!std::isfinite(norm) || norm < 1e-6f) + { + throw std::invalid_argument("wrist offset quaternion must have nonzero finite norm"); + } + return { { v[3] / norm, v[4] / norm, v[5] / norm, v[6] / norm }, { v[0], v[1], v[2] } }; +} + } // namespace int main(int argc, char** argv) @@ -59,6 +86,7 @@ try std::vector ports; std::vector baudrates; std::string plugin_root_id = "realhand_ffg_glove"; + plugins::realhand_ffg_glove::RealHandFFGGloveConfig config; for (int i = 1; i < argc; ++i) { const std::string argument = argv[i]; @@ -74,6 +102,48 @@ try { plugin_root_id = argument.substr(17); } + else if (argument.starts_with("--calibration=")) + { + config.calibration_path = argument.substr(14); + } + else if (argument.starts_with("--hand-scale=")) + { + const auto value = argument.substr(13); + std::size_t consumed = 0; + config.hand_scale = std::stof(value, &consumed); + if (consumed != value.size()) + { + throw std::invalid_argument("hand scale must be a number"); + } + } + else if (argument.starts_with("--left-aim-to-wrist=")) + { + config.wrist.left_aim_to_wrist = parse_pose(argument.substr(20)); + } + else if (argument.starts_with("--right-aim-to-wrist=")) + { + config.wrist.right_aim_to_wrist = parse_pose(argument.substr(21)); + } + else if (argument.starts_with("--wrist-source=")) + { + const auto source = argument.substr(15); + if (source == "auto") + { + config.wrist.mode = plugin_utils::WristSourceMode::Auto; + } + else if (source == "controller") + { + config.wrist.mode = plugin_utils::WristSourceMode::Controller; + } + else if (source == "handtracking") + { + config.wrist.mode = plugin_utils::WristSourceMode::HandTracking; + } + else + { + throw std::invalid_argument("wrist source must be auto, controller, or handtracking"); + } + } else { throw std::invalid_argument("unknown argument: " + argument); @@ -82,8 +152,9 @@ try 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; + plugins::realhand_ffg_glove::RealHandFFGGlovePlugin plugin( + std::move(ports), std::move(baudrates), plugin_root_id, std::move(config)); + isaaccapture::Logger::get("isaaccapture.plugins.realhand_ffg_glove.main")->info("RealHand FFG Glove plugin running"); constexpr auto frame_period = std::chrono::nanoseconds(1'000'000'000 / 90); while (!g_stop_requested.load(std::memory_order_relaxed)) @@ -96,6 +167,6 @@ try } catch (const std::exception& error) { - std::cerr << argv[0] << ": " << error.what() << std::endl; + isaaccapture::Logger::get("isaaccapture.plugins.realhand_ffg_glove.main")->error("{}: {}", argv[0], error.what()); return 1; } diff --git a/src/plugins/realhand_ffg_glove/plugin.yaml b/src/plugins/realhand_ffg_glove/plugin.yaml index 3c79ee0449..80d114f392 100644 --- a/src/plugins/realhand_ffg_glove/plugin.yaml +++ b/src/plugins/realhand_ffg_glove/plugin.yaml @@ -2,13 +2,19 @@ # SPDX-License-Identifier: Apache-2.0 name: realhand_ffg_glove_plugin -description: "RealHand FFG Glove joint sensors" +description: "RealHand FFG Glove OpenXR hand tracking" command: "./realhand_ffg_glove_plugin" version: "1.0.0" devices: + - path: "/hand/left" + type: "hand_tracker" + description: "Left RealHand FFG Glove human skeleton (26 OpenXR joints)" + - path: "/hand/right" + type: "hand_tracker" + description: "Right RealHand FFG Glove human skeleton (26 OpenXR joints)" - path: "/glove/realhand_ffg_glove_left" type: "joint_state" - description: "Left RealHand FFG Glove (21 joint sensors over USB serial)" + description: "Left RealHand FFG Glove raw sensors for calibration and diagnostics" - path: "/glove/realhand_ffg_glove_right" type: "joint_state" - description: "Right RealHand FFG Glove (21 joint sensors over USB serial)" + description: "Right RealHand FFG Glove raw sensors for calibration and diagnostics" diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp index d9794ecee1..b599e7077b 100644 --- a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp @@ -3,8 +3,6 @@ #include "realhand_ffg_glove_plugin.hpp" -#include "realhand_ffg_glove_serial.hpp" - #include #include #include @@ -13,10 +11,7 @@ #include #include -#include #include -#include -#include #include namespace plugins::realhand_ffg_glove @@ -26,27 +21,35 @@ 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", +constexpr std::array kDevicePaths = { "/hand/left", "/hand/right" }; +constexpr std::array kSensorPaths = { "/glove/realhand_ffg_glove_left", "/glove/realhand_ffg_glove_right" }; - -std::size_t side_index(HandSide side) -{ - return side == HandSide::Left ? 0 : 1; -} +constexpr auto kSkeletonStaleThreshold = std::chrono::milliseconds(200); const char* side_name(std::size_t side) { return side == 0 ? "left" : "right"; } -std::vector required_extensions() +std::vector required_extensions(const plugin_utils::WristPoseSource::Requirements& wrist) { - auto extensions = core::SchemaPusher::get_required_extensions(); + auto extensions = core::DeviceIOSession::get_required_extensions(wrist.trackers); + extensions.push_back(XR_NVX1_DEVICE_INTERFACE_BASE_EXTENSION_NAME); + for (const auto* extension : wrist.extensions) + { + if (std::find(extensions.begin(), extensions.end(), extension) == extensions.end()) + { + extensions.push_back(extension); + } + } + for (const auto& extension : core::SchemaPusher::get_required_extensions()) + { + if (std::find(extensions.begin(), extensions.end(), extension) == extensions.end()) + { + extensions.push_back(extension); + } + } for (const auto& extension : plugin_utils::PluginDeviceStatusPublisher::get_required_extensions()) { if (std::find(extensions.begin(), extensions.end(), extension) == extensions.end()) @@ -61,14 +64,22 @@ std::vector required_extensions() 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())) + std::string plugin_root_id, + RealHandFFGGloveConfig config) + : reader_(std::move(configured_ports), std::move(baudrates)), skeleton_(config.calibration_path, config.hand_scale) { - if (baudrates_.empty()) + require_controller_wrist_ = config.wrist.mode == plugin_utils::WristSourceMode::Controller; + const auto wrist = plugin_utils::WristPoseSource::collect_requirements(config.wrist.mode); + session_ = std::make_shared("RealHandFFGGlovePlugin", required_extensions(wrist)); + const auto handles = session_->get_handles(); + deviceio_session_ = core::DeviceIOSession::run(wrist.trackers, handles); + time_converter_.emplace(handles); + wrist_source_ = std::make_unique( + config.wrist, handles, deviceio_session_.get(), wrist.controller_tracker); + if (config.calibration_path.empty()) { - baudrates_.assign(kDefaultBaudrates.begin(), kDefaultBaudrates.end()); + isaaccapture::Logger::get("isaaccapture.plugins.realhand_ffg_glove.main") + ->warn("No human calibration: raw sensors are available for calibration, but hands remain inactive"); } for (std::size_t side = 0; side < pushers_.size(); ++side) { @@ -82,133 +93,10 @@ RealHandFFGGlovePlugin::RealHandFFGGlovePlugin(std::vector configur } 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; @@ -221,7 +109,7 @@ void RealHandFFGGlovePlugin::push_state(std::size_t side) { auto joint = std::make_shared(); joint->name = "sensor_" + std::to_string(sensor); - joint->position = endpoints_[side]->positions[sensor]; + joint->position = reader_.endpoints()[side]->positions[sensor]; joint->valid = true; output.joints.push_back(std::move(joint)); } @@ -235,17 +123,17 @@ void RealHandFFGGlovePlugin::push_state(std::size_t side) void RealHandFFGGlovePlugin::publish_status() { std::vector entries; - entries.reserve(2); - for (std::size_t side = 0; side < endpoints_.size(); ++side) + entries.reserve(4); + for (std::size_t side = 0; side < reader_.endpoints().size(); ++side) { plugin_utils::PluginDeviceStatusEntry entry{ .path = kDevicePaths[side] }; - if (!endpoints_[side]) + if (!reader_.endpoints()[side]) { entry.state = core::PluginDeviceState_DISCONNECTED; entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; - entry.error = errors_[side]; + entry.error = reader_.errors()[side]; } - else if (!endpoints_[side]->has_positions) + else if (!reader_.endpoints()[side]->has_positions) { entry.state = core::PluginDeviceState_DEGRADED; entry.reason = core::PluginDeviceReason_NO_CURRENT_DATA; @@ -256,6 +144,19 @@ void RealHandFFGGlovePlugin::publish_status() entry.state = core::PluginDeviceState_CONNECTED; entry.reason = core::PluginDeviceReason_NONE; } + auto sensor = entry; + sensor.path = kSensorPaths[side]; + entries.push_back(std::move(sensor)); + if (entry.state == core::PluginDeviceState_CONNECTED && !injectors_[side]) + { + entry.state = core::PluginDeviceState_DEGRADED; + entry.reason = core::PluginDeviceReason_NO_CURRENT_DATA; + entry.error = wrist_rejected_[side] ? + "controller wrist unavailable or discontinuous; awaiting stable tracking" : + !skeleton_.calibrated(side == 0 ? HandSide::Left : HandSide::Right) ? + "missing human calibration for this hand" : + "no fresh usable skeleton"; + } entries.push_back(std::move(entry)); } status_publisher_->publish_if_changed(entries, core::os_monotonic_now_ns()); @@ -263,9 +164,58 @@ void RealHandFFGGlovePlugin::publish_status() void RealHandFFGGlovePlugin::update() { - discover(); - poll_endpoint(0); - poll_endpoint(1); + deviceio_session_->update(); + reader_.update(); + const auto now = GloveReader::Clock::now(); + const auto time = time_converter_->os_monotonic_now(); + for (std::size_t side = 0; side < reader_.endpoints().size(); ++side) + { + const auto& endpoint = reader_.endpoints()[side]; + if (endpoint && endpoint->has_positions && now - endpoint->last_receive < kSkeletonStaleThreshold) + { + push_state(side); + if (auto joints = skeleton_.reconstruct(endpoint->positions, endpoint->version.side)) + { + const auto wrist = wrist_source_->query(side == 0, time); + if (require_controller_wrist_) + { + const bool rejected = + !wrist_continuity_[side].accept(wrist.pose, core::os_monotonic_now_ns(), wrist.valid); + if (rejected != wrist_rejected_[side]) + { + auto logger = isaaccapture::Logger::get("isaaccapture.plugins.realhand_ffg_glove.wrist"); + if (rejected) + { + logger->warn("{} controller wrist tracking interrupted; hand inactive, raw sensors retained", + side_name(side)); + } + else + { + logger->info("{} controller wrist tracking reacquired", side_name(side)); + } + } + wrist_rejected_[side] = rejected; + if (rejected) + { + injectors_[side].reset(); + continue; + } + } + *joints = place_hand(*joints, wrist.pose, wrist.valid, wrist.tracked); + if (!injectors_[side]) + { + const auto handles = session_->get_handles(); + injectors_[side] = std::make_unique( + handles.instance, handles.session, side == 0 ? XR_HAND_LEFT_EXT : XR_HAND_RIGHT_EXT, + handles.space); + } + injectors_[side]->push(joints->data(), time); + continue; + } + } + // Destroying the injector makes isActive false; never leave a frozen hand behind. + injectors_[side].reset(); + } publish_status(); } diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp index 07bd52b4e2..087616210d 100644 --- a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp @@ -3,8 +3,12 @@ #pragma once -#include "realhand_ffg_glove_protocol.hpp" +#include "realhand_ffg_glove_reader.hpp" +#include "realhand_ffg_glove_skeleton.hpp" +#include +#include +#include #include #include @@ -27,44 +31,47 @@ class PluginDeviceStatusPublisher; namespace plugins::realhand_ffg_glove { -class SerialGlove; +struct RealHandFFGGloveConfig +{ + std::string calibration_path; + float hand_scale = 1.0f; + // Mirrored PICO 4 Ultra controllers mounted upright on the FFG hand-back bracket. + // Mount defaults are hardware configuration, separate from per-user finger calibration. + plugin_utils::WristSourceConfig wrist{ + .left_aim_to_wrist = { { -0.422424918198f, 0.014580024139f, 0.024960358040f, 0.905936858676f }, + { 0.020534770647f, -0.047368722071f, 0.192378854628f } }, + .right_aim_to_wrist = { { -0.422424918198f, -0.014580024139f, -0.024960358040f, 0.905936858676f }, + { -0.020534770647f, -0.047368722071f, 0.192378854628f } } + }; +}; class RealHandFFGGlovePlugin { public: explicit RealHandFFGGlovePlugin(std::vector configured_ports = {}, std::vector baudrates = {}, - std::string plugin_root_id = "realhand_ffg_glove"); + std::string plugin_root_id = "realhand_ffg_glove", + RealHandFFGGloveConfig config = {}); ~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_{}; + GloveReader reader_; + HumanHandSkeleton skeleton_; std::shared_ptr session_; + std::shared_ptr deviceio_session_; + std::unique_ptr wrist_source_; + std::optional time_converter_; + std::array, 2> injectors_; std::array, 2> pushers_; + std::array wrist_continuity_; + std::array wrist_rejected_{}; + bool require_controller_wrist_ = false; std::unique_ptr status_publisher_; }; diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.cpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.cpp new file mode 100644 index 0000000000..6e413a0fc4 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.cpp @@ -0,0 +1,210 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "realhand_ffg_glove_reader.hpp" + +#include + +#include +#include +#include + +namespace plugins::realhand_ffg_glove +{ +namespace +{ +constexpr auto kDiscoveryPeriod = std::chrono::seconds(2); +constexpr auto kProbeTimeout = std::chrono::milliseconds(250); +constexpr auto kProbeQueryPeriod = std::chrono::milliseconds(40); +constexpr auto kQueryPeriod = std::chrono::milliseconds(16); +constexpr auto kStaleTimeout = std::chrono::seconds(1); +auto logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.realhand_ffg_glove.reader"); +} +} // namespace + +GloveReader::GloveReader(std::vector ports, std::vector baudrates) + : configured_ports_(std::move(ports)), baudrates_(std::move(baudrates)) +{ + if (baudrates_.empty()) + { + baudrates_.assign(kDefaultBaudrates.begin(), kDefaultBaudrates.end()); + } + if (configured_ports_.empty() && discover_serial_ports().empty()) + { + logger()->warn("No identified FFG USB device; specify verified glove ports with --ports=/dev/ttyUSB0"); + } +} + +bool GloveReader::occupied(const std::string& port) const +{ + return std::any_of(endpoints_.begin(), endpoints_.end(), + [&port](const auto& endpoint) + { + std::error_code error; + return endpoint && std::filesystem::equivalent(port, endpoint->serial->port(), error); + }); +} + +void GloveReader::advance_probe() +{ + probe_.reset(); + if (++baud_index_ == baudrates_.size()) + { + baud_index_ = 0; + ++candidate_index_; + } +} + +void GloveReader::discover(Clock::time_point now) +{ + if (endpoints_[0] && endpoints_[1]) + { + probe_.reset(); + candidates_.clear(); + return; + } + if (!probe_ && candidate_index_ >= candidates_.size()) + { + if (last_discovery_ && now - *last_discovery_ < kDiscoveryPeriod) + { + return; + } + last_discovery_ = now; + candidates_ = configured_ports_.empty() ? discover_serial_ports() : configured_ports_; + candidate_index_ = 0; + baud_index_ = 0; + if (candidates_.empty()) + { + for (std::size_t side = 0; side < endpoints_.size(); ++side) + { + if (!endpoints_[side]) + { + errors_[side] = "No identified FFG USB device. Specify glove ports with --ports=/dev/ttyUSB0"; + } + } + return; + } + } + const auto& port = candidates_[candidate_index_]; + if (occupied(port)) + { + ++candidate_index_; + baud_index_ = 0; + return; + } + try + { + if (!probe_) + { + probe_ = std::make_unique(port, baudrates_[baud_index_]); + probe_started_ = now; + probe_->send(Command::Version); + probe_last_query_ = now; + } + // Probe incrementally; never wait for a silent port on the publish thread. + for (const auto& frame : probe_->read_available(0)) + { + if (auto version = decode_version(frame)) + { + const std::size_t side = version->side == HandSide::Left ? 0 : 1; + if (!endpoints_[side]) + { + auto endpoint = std::make_unique(); + endpoint->serial = std::move(probe_); + endpoint->version = *version; + endpoint->last_receive = now; + logger()->info("Connected {} glove {} on {} at {} baud", side == 0 ? "left" : "right", + version->version, port, endpoint->serial->baudrate()); + endpoints_[side] = std::move(endpoint); + errors_[side].clear(); + } + else + { + logger()->warn("Ignoring duplicate {} glove on {}", side == 0 ? "left" : "right", port); + probe_.reset(); + } + ++candidate_index_; + baud_index_ = 0; + return; + } + } + if (now - probe_started_ >= kProbeTimeout) + { + advance_probe(); + } + else if (now - probe_last_query_ >= kProbeQueryPeriod) + { + probe_->send(Command::Version); + probe_last_query_ = now; + } + } + catch (const std::exception& error) + { + for (std::size_t side = 0; side < endpoints_.size(); ++side) + { + if (!endpoints_[side]) + { + errors_[side] = error.what(); + } + } + advance_probe(); + } +} + +void GloveReader::drop_endpoint(std::size_t side, const std::string& error) +{ + logger()->warn("Disconnected {} glove: {}", side == 0 ? "left" : "right", error); + endpoints_[side].reset(); + errors_[side] = error; + last_discovery_.reset(); +} + +void GloveReader::poll_endpoint(std::size_t side, Clock::time_point now) +{ + auto& endpoint = endpoints_[side]; + if (!endpoint) + { + return; + } + try + { + // Empty optional timestamps mean "due now" without subtracting time_point::min(). + if (!endpoint->last_query || now - *endpoint->last_query >= kQueryPeriod) + { + endpoint->serial->send(Command::Position); + endpoint->last_query = now; + } + for (const auto& frame : endpoint->serial->read_available(0)) + { + if (auto positions = decode_positions(frame)) + { + endpoint->last_receive = now; + endpoint->positions = *positions; + endpoint->has_positions = true; + if (frame.command == Command::A6Position) + { + endpoint->serial->send(Command::A7Force, std::vector(5 * sizeof(float), 0)); + } + } + } + if (now - endpoint->last_receive > kStaleTimeout) + { + drop_endpoint(side, "no position response for one second"); + } + } + catch (const std::exception& error) + { + drop_endpoint(side, error.what()); + } +} + +void GloveReader::update() +{ + const auto now = Clock::now(); + poll_endpoint(0, now); + poll_endpoint(1, now); + discover(now); +} +} // namespace plugins::realhand_ffg_glove diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.hpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.hpp new file mode 100644 index 0000000000..45ac637628 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.hpp @@ -0,0 +1,63 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "realhand_ffg_glove_serial.hpp" + +#include +#include +#include +#include +#include +#include + +namespace plugins::realhand_ffg_glove +{ + +class GloveReader +{ +public: + using Clock = std::chrono::steady_clock; + struct Endpoint + { + std::unique_ptr serial; + VersionInfo version; + std::array positions{}; + bool has_positions = false; + Clock::time_point last_receive{}; + std::optional last_query; + }; + + explicit GloveReader(std::vector ports = {}, std::vector baudrates = {}); + void update(); + const std::array, 2>& endpoints() const + { + return endpoints_; + } + const std::array& errors() const + { + return errors_; + } + +private: + void discover(Clock::time_point now); + void poll_endpoint(std::size_t side, Clock::time_point now); + bool occupied(const std::string& port) const; + void drop_endpoint(std::size_t side, const std::string& error); + void advance_probe(); + + std::vector configured_ports_; + std::vector baudrates_; + std::vector candidates_; + std::size_t candidate_index_ = 0; + std::size_t baud_index_ = 0; + std::unique_ptr probe_; + Clock::time_point probe_started_{}; + Clock::time_point probe_last_query_{}; + std::optional last_discovery_; + std::array, 2> endpoints_; + std::array errors_; +}; + +} // 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 index f8f19361d3..1ed46592ef 100644 --- a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.cpp +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.cpp @@ -4,15 +4,21 @@ #include "realhand_ffg_glove_serial.hpp" #include +#include #include #include +#include #include #include #ifndef _WIN32 +# include +# include # include +# include # include +# include # include # include #endif @@ -62,6 +68,41 @@ void append_glob(const char* pattern, std::vector& paths) ::globfree(&matches); } +bool port_is_open(const std::string& port) +{ + struct stat device + { + }; + if (::stat(port.c_str(), &device) != 0 || !S_ISCHR(device.st_mode)) + { + throw std::runtime_error("not a serial character device: " + port); + } +# ifdef __linux__ + // TIOCEXCL blocks future opens, but does not detect existing non-exclusive users. + std::error_code error; + for (const auto& process : std::filesystem::directory_iterator("/proc", error)) + { + const auto name = process.path().filename().string(); + if (name.empty() || !std::all_of(name.begin(), name.end(), [](unsigned char ch) { return std::isdigit(ch); })) + { + continue; + } + std::error_code fd_error; + for (const auto& fd : std::filesystem::directory_iterator(process.path() / "fd", fd_error)) + { + struct stat opened + { + }; + if (::stat(fd.path().c_str(), &opened) == 0 && S_ISCHR(opened.st_mode) && opened.st_rdev == device.st_rdev) + { + return true; + } + } + } +# endif + return false; +} + } // namespace #endif @@ -70,21 +111,33 @@ SerialGlove::SerialGlove(const std::string& port, int baudrate) : port_(port), b #ifdef _WIN32 throw std::runtime_error("RealHand FFG Glove serial backend currently supports POSIX systems only"); #else + const speed_t speed = baud_to_termios(baudrate); + if (port_is_open(port)) + { + throw std::runtime_error("serial port is already in use: " + port); + } fd_ = ::open(port.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); if (fd_ < 0) { throw std::runtime_error("cannot open " + port + ": " + std::strerror(errno)); } + if (::flock(fd_, LOCK_EX | LOCK_NB) != 0 || ::ioctl(fd_, TIOCEXCL) != 0) + { + const std::string error = std::strerror(errno); + ::close(fd_); + fd_ = -1; + throw std::runtime_error("cannot exclusively claim " + port + ": " + error); + } termios tty{}; if (::tcgetattr(fd_, &tty) != 0) { const std::string error = std::strerror(errno); + ::ioctl(fd_, TIOCNXCL); ::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; @@ -100,6 +153,7 @@ SerialGlove::SerialGlove(const std::string& port, int baudrate) : port_(port), b if (::tcsetattr(fd_, TCSANOW, &tty) != 0) { const std::string error = std::strerror(errno); + ::ioctl(fd_, TIOCNXCL); ::close(fd_); fd_ = -1; throw std::runtime_error("tcsetattr failed for " + port + ": " + error); @@ -113,6 +167,7 @@ SerialGlove::~SerialGlove() #ifndef _WIN32 if (fd_ >= 0) { + ::ioctl(fd_, TIOCNXCL); ::close(fd_); } #endif @@ -122,18 +177,43 @@ void SerialGlove::send(Command command, const std::vector& payload) { #ifndef _WIN32 const auto frame = pack_frame(command, payload); + const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(20); std::size_t sent = 0; while (sent < frame.size()) { + if (std::chrono::steady_clock::now() >= deadline) + { + throw std::runtime_error("write timed out for " + port_); + } const ssize_t count = ::write(fd_, frame.data() + sent, frame.size() - sent); if (count < 0) { - if (errno == EINTR || errno == EAGAIN) + if (errno == EINTR) { continue; } + if (errno == EAGAIN || errno == EWOULDBLOCK) + { + const auto remaining = + std::chrono::duration_cast(deadline - std::chrono::steady_clock::now()); + pollfd descriptor{ fd_, POLLOUT, 0 }; + const int ready = ::poll(&descriptor, 1, static_cast(std::max(1, remaining.count()))); + if (ready < 0 && errno == EINTR) + { + continue; + } + if (ready <= 0 || (descriptor.revents & (POLLERR | POLLHUP | POLLNVAL))) + { + throw std::runtime_error("write unavailable or timed out for " + port_); + } + continue; + } throw std::runtime_error("write failed for " + port_ + ": " + std::strerror(errno)); } + if (count == 0) + { + throw std::runtime_error("write made no progress for " + port_); + } sent += static_cast(count); } #else @@ -194,31 +274,23 @@ 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); + // Generic USB UART IDs cannot distinguish a glove from a robot motor bus. + append_glob("/dev/serial/by-id/*", paths); + paths.erase(std::remove_if(paths.begin(), paths.end(), + [](const std::string& path) + { + auto name = std::filesystem::path(path).filename().string(); + std::transform(name.begin(), name.end(), name.begin(), + [](unsigned char ch) { return static_cast(std::tolower(ch)); }); + return name.find("realhand") == std::string::npos && + name.find("realforce") == std::string::npos && + name.find("ffg") == std::string::npos; + }), + paths.end()); 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 index 4fd4e761c3..6165272262 100644 --- a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.hpp +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.hpp @@ -42,6 +42,5 @@ class SerialGlove }; std::vector discover_serial_ports(); -std::optional probe_glove(SerialGlove& glove, int timeout_ms = 250); } // namespace plugins::realhand_ffg_glove diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.cpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.cpp new file mode 100644 index 0000000000..8c6c797f8a --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.cpp @@ -0,0 +1,398 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "realhand_ffg_glove_skeleton.hpp" + +#include + +#include +#include +#include +#include + +#include + +namespace plugins::realhand_ffg_glove +{ +namespace +{ +constexpr auto kValid = XR_SPACE_LOCATION_POSITION_VALID_BIT | XR_SPACE_LOCATION_ORIENTATION_VALID_BIT; +constexpr auto kTracked = XR_SPACE_LOCATION_POSITION_TRACKED_BIT | XR_SPACE_LOCATION_ORIENTATION_TRACKED_BIT; +constexpr float kPi = std::numbers::pi_v; + +template +std::array read_values(const YAML::Node& node, const std::string& label) +{ + if (!node.IsSequence() || node.size() != N) + { + throw std::invalid_argument(label + " must contain " + std::to_string(N) + " finite values"); + } + std::array angles{}; + for (std::size_t i = 0; i < angles.size(); ++i) + { + angles[i] = node[i].as(); + if (!std::isfinite(angles[i])) + { + throw std::invalid_argument(label + " contains a non-finite angle"); + } + } + return angles; +} + +std::array read_angles(const YAML::Node& node, const std::string& label) +{ + return read_values(node, label); +} + +ThumbSensorMapping read_thumb_mapping(const YAML::Node& node) +{ + if (!node.IsMap() || node["format"].as("") != "realhand-ffg-thumb-quadratic-v1") + { + throw std::invalid_argument("Invalid thumb sensor mapping format"); + } + ThumbSensorMapping mapping{ read_values<4>(node["input_origin"], "thumb origin"), + read_values<4>(node["input_scale"], "thumb scale"), + read_values<4>(node["input_min"], "thumb minimum"), + read_values<4>(node["input_max"], "thumb maximum"), + {}, + read_values<3>(node["base_position"], "thumb base"), + read_values<3>(node["lengths"], "thumb lengths") }; + const auto coefficients = node["coefficients"]; + if (!coefficients.IsSequence() || coefficients.size() != 4) + { + throw std::invalid_argument("Thumb mapping must have four coefficient rows"); + } + bool has_motion = false; + for (std::size_t i = 0; i < 4; ++i) + { + mapping.coefficients[i] = read_values<15>(coefficients[i], "thumb coefficients"); + if (mapping.scale[i] < 0.10f - 1e-6f || mapping.scale[i] > kPi || mapping.minimum[i] > 0.0f || + mapping.maximum[i] < 0.0f || mapping.maximum[i] - mapping.minimum[i] >= kPi || + std::max(std::abs(mapping.minimum[i]), std::abs(mapping.maximum[i])) > mapping.scale[i] + 1e-5f) + { + throw std::invalid_argument("Thumb mapping has invalid input ranges"); + } + has_motion = has_motion || mapping.maximum[i] - mapping.minimum[i] >= 0.05f; + for (float coefficient : mapping.coefficients[i]) + { + if (std::abs(coefficient) > 1.76f) + { + throw std::invalid_argument("Thumb coefficient exceeds the bounded fit range"); + } + } + } + if (!has_motion) + { + throw std::invalid_argument("Thumb mapping has insufficient sensor motion"); + } + constexpr float base[3] = { 0.025f, -0.005f, -0.020f }; + constexpr float lengths[3] = { 0.035f, 0.030f, 0.026f }; + for (std::size_t i = 0; i < 3; ++i) + { + if (std::abs(mapping.base[i] - base[i]) > 0.0081f || mapping.lengths[i] < lengths[i] * 0.849f || + mapping.lengths[i] > lengths[i] * 1.151f) + { + throw std::invalid_argument("Thumb geometry exceeds the bounded fit range"); + } + } + return mapping; +} + +float angle_delta(float value, float origin) +{ + return std::remainder(std::remainder(value, 2.0f * kPi) - std::remainder(origin, 2.0f * kPi), 2.0f * kPi); +} + +float response(float value, float origin, float endpoint) +{ + const float span = angle_delta(endpoint, origin); + if (std::abs(span) < 0.05f) + { + return 0.0f; + } + // Unwrap around the middle of the calibrated arc, not the open boundary. + // Otherwise continuing past a >pi/2 span can wrap back to "open". + const float center = span * 0.5f; + const float delta = center + std::remainder(angle_delta(value, origin) - center, 2.0f * kPi); + return std::clamp(delta / span, 0.0f, 1.0f); +} + +XrPosef rotate(float angle, int axis) +{ + XrPosef pose{ { 0.0f, 0.0f, 0.0f, std::cos(angle * 0.5f) }, {} }; + if (axis == 0) + { + pose.orientation.x = std::sin(angle * 0.5f); + } + else if (axis == 1) + { + pose.orientation.y = std::sin(angle * 0.5f); + } + else + { + pose.orientation.z = std::sin(angle * 0.5f); + } + return pose; +} + +XrPosef advance(const XrPosef& pose, float length) +{ + return oxr_utils::multiply_poses(pose, { { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, -length } }); +} + +void set_joint(HandJoints& joints, std::size_t index, const XrPosef& pose, float radius) +{ + joints[index] = { kValid, pose, radius }; +} +} // namespace + +HumanHandSkeleton::HumanHandSkeleton(const std::string& calibration_path, float hand_scale) : hand_scale_(hand_scale) +{ + if (!std::isfinite(hand_scale_) || hand_scale_ < 0.5f || hand_scale_ > 2.0f) + { + throw std::invalid_argument("hand scale must be finite and between 0.5 and 2.0"); + } + if (calibration_path.empty()) + { + return; + } + const auto data = YAML::LoadFile(calibration_path); + if (!data.IsMap() || data["format"].as("") != "realhand-ffg-glove-human-calibration-v1") + { + throw std::invalid_argument("Expected device-side human calibration; run realhand_ffg_glove_calibration.py"); + } + const auto hands = data["hands"]; + if (!hands.IsMap()) + { + throw std::invalid_argument("human calibration must contain a hands mapping"); + } + for (std::size_t side = 0; side < 2; ++side) + { + const std::string name = side == 0 ? "left" : "right"; + const auto node = hands[name]; + if (!node) + { + continue; + } + calibrations_[side] = + HumanHandCalibration{ read_angles(node["open"], name + ".open"), read_angles(node["fist"], name + ".fist"), + read_angles(node["thumb_curl"], name + ".thumb_curl"), + read_angles(node["thumb_opposition"], name + ".thumb_opposition"), + read_angles(node["spread"], name + ".spread") }; + if (node["thumb_mapping"]) + { + calibrations_[side]->thumb_mapping = read_thumb_mapping(node["thumb_mapping"]); + } + const auto& calibration = *calibrations_[side]; + const auto require_span = [&](std::size_t sensor, const auto& endpoint) + { + if (std::abs(angle_delta(endpoint[sensor], calibration.open[sensor])) < 0.05f) + { + throw std::invalid_argument(name + " sensor_" + std::to_string(sensor) + + " has insufficient calibration motion; capture the poses again"); + } + }; + // A multivariate fit uses the full captured ranges, not one legacy pose/channel pair. + if (!calibration.thumb_mapping) + { + require_span(0, calibration.thumb_opposition); + require_span(2, calibration.thumb_curl); + require_span(4, calibration.thumb_curl); + } + for (std::size_t finger = 0; finger < 4; ++finger) + { + require_span(6 + 4 * finger, calibration.fist); + require_span(8 + 4 * finger, calibration.fist); + } + } + if (!calibrations_[0] && !calibrations_[1]) + { + throw std::invalid_argument("human calibration has neither left nor right hand measurements"); + } +} + +bool HumanHandSkeleton::calibrated(HandSide side) const +{ + return calibrations_[side == HandSide::Left ? 0 : 1].has_value(); +} + +std::optional HumanHandSkeleton::reconstruct(const std::array& sensors, HandSide side) const +{ + const auto& calibration = calibrations_[side == HandSide::Left ? 0 : 1]; + if (!calibration || !std::all_of(sensors.begin(), sensors.end(), [](float x) { return std::isfinite(x); })) + { + return std::nullopt; + } + const auto& c = *calibration; + const float mirror = side == HandSide::Left ? 1.0f : -1.0f; + const float scale = hand_scale_; + HandJoints joints{}; + const XrPosef identity{ { 0.0f, 0.0f, 0.0f, 1.0f }, {} }; + set_joint(joints, XR_HAND_JOINT_WRIST_EXT, identity, 0.020f * scale); + set_joint(joints, XR_HAND_JOINT_PALM_EXT, { identity.orientation, { 0.0f, 0.0f, -0.045f * scale } }, 0.030f * scale); + + // SDK l20_config.py: 5+4*f is spread, 6+4*f MCP flexion, 8+4*f distal flexion. + // It does not independently resolve PIP; infer it from DIP (1.5:1). + constexpr float lateral[4] = { 0.033f, 0.011f, -0.011f, -0.031f }; + constexpr float knuckle[4] = { 0.070f, 0.075f, 0.070f, 0.060f }; + constexpr float lengths[4][3] = { + { 0.045f, 0.026f, 0.018f }, { 0.050f, 0.030f, 0.019f }, { 0.046f, 0.027f, 0.018f }, { 0.034f, 0.020f, 0.016f } + }; + constexpr float spread_limits[4] = { 0.25f, 0.08f, 0.12f, 0.30f }; + for (std::size_t finger = 0; finger < 4; ++finger) + { + const std::size_t sensor = 5 + 4 * finger; + const std::size_t joint = XR_HAND_JOINT_INDEX_METACARPAL_EXT + 5 * finger; + const float spread_span = angle_delta(c.spread[sensor], c.open[sensor]); + // A contact can reach either lateral extreme: use its magnitude, never its sign. + // Raw lateral deltas are mirrored between gloves, separately from skeleton anatomy. + // Normalize them before applying the anatomical mirror; thumb/flexion channels are unchanged. + const float spread = + std::clamp(mirror * angle_delta(sensors[sensor], c.open[sensor]) / std::max(std::abs(spread_span), 0.10f), + -1.0f, 1.0f) * + spread_limits[finger]; + const float base_flex = response(sensors[sensor + 1], c.open[sensor + 1], c.fist[sensor + 1]) * 1.5f; + const float distal_flex = response(sensors[sensor + 3], c.open[sensor + 3], c.fist[sensor + 3]) * 1.1f; + XrPosef pose{ identity.orientation, { mirror * lateral[finger] * scale, 0.0f, -0.015f * scale } }; + pose = oxr_utils::multiply_poses(pose, rotate(mirror * spread, 1)); + pose = advance(pose, (knuckle[finger] - 0.015f) * scale); + // Fan the neutral metacarpals toward the wrist; preserve knuckle poses and bone lengths. + const float fan_angle = std::atan2(-mirror * lateral[finger], knuckle[finger]); + auto metacarpal = oxr_utils::multiply_poses(pose, rotate(fan_angle, 1)); + metacarpal = advance(metacarpal, -(knuckle[finger] - 0.015f) * scale); + set_joint(joints, joint, metacarpal, 0.010f * scale); + pose = oxr_utils::multiply_poses(pose, rotate(-base_flex, 0)); + set_joint(joints, joint + 1, pose, 0.009f * scale); + pose = advance(pose, lengths[finger][0] * scale); + pose = oxr_utils::multiply_poses(pose, rotate(-1.5f * distal_flex, 0)); + set_joint(joints, joint + 2, pose, 0.008f * scale); + pose = advance(pose, lengths[finger][1] * scale); + pose = oxr_utils::multiply_poses(pose, rotate(-distal_flex, 0)); + set_joint(joints, joint + 3, pose, 0.007f * scale); + pose = advance(pose, lengths[finger][2] * scale); + set_joint(joints, joint + 4, pose, 0.006f * scale); + } + + // Thumb abduction/roll/MCP/IP use SDK channels 0/1/2/4. No other digit is consulted. + const float opposition = response(sensors[0], c.open[0], c.thumb_opposition[0]); + const float roll = response(sensors[1], c.open[1], c.thumb_opposition[1]); + std::array angles{ -0.85f + 1.20f * opposition, -0.70f * roll, + response(sensors[2], c.open[2], c.thumb_curl[2]) * 1.1f, + response(sensors[4], c.open[4], c.thumb_curl[4]) * 1.2f }; + std::array base{ 0.025f, -0.005f, -0.020f }, thumb_lengths{ 0.035f, 0.030f, 0.026f }; + if (c.thumb_mapping) + { + const auto& mapping = *c.thumb_mapping; + constexpr std::size_t channels[4] = { 0, 1, 2, 4 }; + std::array features{ 1.0f }; + for (std::size_t i = 0; i < 4; ++i) + { + const float center = (mapping.minimum[i] + mapping.maximum[i]) * 0.5f; + const float delta = + center + std::remainder(angle_delta(sensors[channels[i]], mapping.origin[i]) - center, 2.0f * kPi); + features[i + 1] = std::clamp(delta, mapping.minimum[i], mapping.maximum[i]) / mapping.scale[i]; + } + std::size_t index = 5; + for (std::size_t first = 1; first <= 4; ++first) + { + for (std::size_t second = first; second <= 4; ++second) + { + features[index++] = features[first] * features[second]; + } + } + // One continuous thumb-only polynomial: no pose lookup, live contact target, or other-digit input. + constexpr float lower[4] = { -1.25f, -1.2f, 0.0f, 0.0f }, upper[4] = { 1.0f, 1.2f, 1.8f, 1.8f }; + for (std::size_t axis = 0; axis < 4; ++axis) + { + float angle = 0.0f; + for (std::size_t feature = 0; feature < features.size(); ++feature) + { + angle += mapping.coefficients[axis][feature] * features[feature]; + } + angles[axis] = std::clamp(angle, lower[axis], upper[axis]); + } + base = mapping.base; + thumb_lengths = mapping.lengths; + } + XrPosef thumb{ identity.orientation, { mirror * base[0] * scale, base[1] * scale, base[2] * scale } }; + thumb = oxr_utils::multiply_poses(thumb, rotate(mirror * angles[0], 1)); + thumb = oxr_utils::multiply_poses(thumb, rotate(mirror * angles[1], 2)); + set_joint(joints, XR_HAND_JOINT_THUMB_METACARPAL_EXT, thumb, 0.012f * scale); + thumb = advance(thumb, thumb_lengths[0] * scale); + thumb = oxr_utils::multiply_poses(thumb, rotate(-angles[2], 0)); + set_joint(joints, XR_HAND_JOINT_THUMB_PROXIMAL_EXT, thumb, 0.010f * scale); + thumb = advance(thumb, thumb_lengths[1] * scale); + thumb = oxr_utils::multiply_poses(thumb, rotate(-angles[3], 0)); + set_joint(joints, XR_HAND_JOINT_THUMB_DISTAL_EXT, thumb, 0.009f * scale); + thumb = advance(thumb, thumb_lengths[2] * scale); + set_joint(joints, XR_HAND_JOINT_THUMB_TIP_EXT, thumb, 0.007f * scale); + return joints; +} + +bool WristPoseContinuity::accept(const XrPosef& pose, std::int64_t time_ns, bool valid) +{ + const auto& p = pose.position; + const auto& q = pose.orientation; + const float norm = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; + if (!valid || !std::isfinite(p.x) || !std::isfinite(p.y) || !std::isfinite(p.z) || !std::isfinite(norm) || + std::abs(norm - 1.0f) > 0.01f) + { + previous_.reset(); + stable_samples_ = 0; + recovering_ = true; + return false; + } + if (!previous_) + { + previous_ = pose; + previous_time_ns_ = time_ns; + return !recovering_; + } + const auto before = *previous_; + const double dt = (static_cast(time_ns) - previous_time_ns_) / 1e9; + previous_ = pose; + previous_time_ns_ = time_ns; + const float x = p.x - before.position.x, y = p.y - before.position.y, z = p.z - before.position.z; + const float distance = std::sqrt(x * x + y * y + z * z); + const auto& b = before.orientation; + const float dot = std::abs(q.x * b.x + q.y * b.y + q.z * b.z + q.w * b.w); + const float angle = 2.0f * std::acos(std::clamp(dot, 0.0f, 1.0f)); + const bool discontinuity = + dt <= 0.0 || dt > 0.2 || + (dt < 0.05 && ((distance > 0.10f && distance / dt > 8.0) || (angle > 1.0f && angle / dt > 20.0))); + if (discontinuity) + { + recovering_ = true; + stable_samples_ = 0; + return false; + } + if (recovering_) + { + stable_samples_ = distance / dt < 3.0 && angle / dt < 10.0 ? stable_samples_ + 1 : 0; + if (stable_samples_ < 3) + { + return false; + } + recovering_ = false; + } + // Never smooth, clamp, or relabel a discontinuity as a tracked pose. + return true; +} + +HandJoints place_hand(HandJoints joints, const XrPosef& wrist, bool wrist_valid, bool wrist_tracked) +{ + for (auto& joint : joints) + { + joint.locationFlags &= ~kTracked; + if (wrist_valid) + { + joint.pose = oxr_utils::multiply_poses(wrist, joint.pose); + if (wrist_tracked) + { + joint.locationFlags |= kTracked; + } + } + } + return joints; +} +} // namespace plugins::realhand_ffg_glove diff --git a/src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.hpp b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.hpp new file mode 100644 index 0000000000..5a2f67b580 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.hpp @@ -0,0 +1,65 @@ +// 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 +{ + +struct ThumbSensorMapping +{ + std::array origin, scale, minimum, maximum; + std::array, 4> coefficients; + std::array base, lengths; +}; + +struct HumanHandCalibration +{ + std::array open; + std::array fist; + std::array thumb_curl; + std::array thumb_opposition; + std::array spread; + std::optional thumb_mapping = std::nullopt; +}; + +using HandJoints = std::array; + +// Device-side human FK only: no robot joint names, URDFs, limits, or target poses. +class HumanHandSkeleton +{ +public: + explicit HumanHandSkeleton(const std::string& calibration_path = {}, float hand_scale = 1.0f); + bool calibrated(HandSide side) const; + std::optional reconstruct(const std::array& sensors, HandSide side) const; + +private: + std::array, 2> calibrations_; + float hand_scale_; +}; + +// VALID describes usable reconstruction; TRACKED is added only with a live wrist source. +HandJoints place_hand(HandJoints joints, const XrPosef& wrist, bool wrist_valid, bool wrist_tracked); + +class WristPoseContinuity +{ +public: + bool accept(const XrPosef& pose, std::int64_t time_ns, bool valid); + +private: + std::optional previous_; + std::int64_t previous_time_ns_ = 0; + unsigned stable_samples_ = 0; + bool recovering_ = false; +}; + +} // namespace plugins::realhand_ffg_glove diff --git a/src/python/isaaccapture/retargeters/__init__.py b/src/python/isaaccapture/retargeters/__init__.py index 93a45dba8f..d230cd0173 100644 --- a/src/python/isaaccapture/retargeters/__init__.py +++ b/src/python/isaaccapture/retargeters/__init__.py @@ -254,16 +254,6 @@ "P7WorkspacePoseConfig", "retargeters-lite", ), - "RealHandHandTrackingRetargeter": ( - ".realhand.hand", - "RealHandHandTrackingRetargeter", - None, - ), - "RealHandHandTrackingRetargeterConfig": ( - ".realhand.hand", - "RealHandHandTrackingRetargeterConfig", - None, - ), "ControllerTriggerRealHandRetargeter": ( ".realhand.hand", "ControllerTriggerRealHandRetargeter", @@ -274,16 +264,6 @@ "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", @@ -460,12 +440,8 @@ def __getattr__(name: str): "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 index 64acacac81..81648d4e52 100644 --- a/src/python/isaaccapture/retargeters/realhand/__init__.py +++ b/src/python/isaaccapture/retargeters/realhand/__init__.py @@ -3,40 +3,54 @@ """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, -) +import importlib + +_EXPORT_MODULES = { + "arm": ( + "P7ControllerPoseRetargeter", + "P7HandPoseRetargeter", + "P7WorkspacePoseConfig", + ), + "hand": ( + "ControllerTriggerRealHandRetargeter", + "ControllerTriggerRealHandRetargeterConfig", + ), + "profiles": ( + "L6_PROFILE", + "L20_PROFILE", + "O6_PROFILE", + "REALHAND_PROFILES", + "RealHandJointSpec", + "RealHandProfile", + "get_realhand_profile", + ), +} + + +def __getattr__(name: str): + for module_name, exports in _EXPORT_MODULES.items(): + if name in exports: + try: + module = importlib.import_module(f".{module_name}", __name__) + except ModuleNotFoundError as error: + if ( + module_name == "arm" + and error.name + and error.name.startswith("scipy") + ): + raise ModuleNotFoundError( + f"{name} requires scipy. Install it with: pip install 'isaaccapture[retargeters-lite]'" + ) from error + raise + value = getattr(module, name) + globals()[name] = value + return value + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + __all__ = [ "ControllerTriggerRealHandRetargeter", "ControllerTriggerRealHandRetargeterConfig", - "RealHandFFGGloveRetargeter", - "RealHandFFGGloveRetargeterConfig", - "REALHAND_FFG_GLOVE_SENSOR_NAMES", "L6_PROFILE", "L20_PROFILE", "O6_PROFILE", @@ -44,11 +58,7 @@ "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/hand.py b/src/python/isaaccapture/retargeters/realhand/hand.py index 19edb4d00f..557802024c 100644 --- a/src/python/isaaccapture/retargeters/realhand/hand.py +++ b/src/python/isaaccapture/retargeters/realhand/hand.py @@ -1,11 +1,11 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""Model-aware OpenXR hand and controller-trigger retargeters.""" +"""Controller-trigger commands for RealHand simulation models.""" from __future__ import annotations -import os +import logging from dataclasses import dataclass import numpy as np @@ -22,364 +22,11 @@ ControllerInput, ControllerInputIndex, FloatType, - HandInput, - HandInputIndex, - HandJointIndex, ) -from .profiles import RealHandJointSpec, get_realhand_profile +from .profiles import 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 +_LOGGER = logging.getLogger(__name__) @dataclass @@ -416,9 +63,6 @@ def __init__( ] 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: @@ -479,15 +123,14 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N 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, - ) + _LOGGER.debug( + "%s controller side=%s trigger=%.3f squeeze=%.3f close=%.3f", + self._profile.display_name, + self._config.side, + trigger_value, + squeeze_value, + self._last_close_amount, + ) def _write_output(self, output) -> None: for index, value in enumerate(self._last_output): @@ -497,7 +140,4 @@ def _write_output(self, output) -> None: __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 index 7731b7a81e..b9b8828d46 100644 --- a/src/python/isaaccapture/retargeters/realhand/profiles.py +++ b/src/python/isaaccapture/retargeters/realhand/profiles.py @@ -11,16 +11,13 @@ @dataclass(frozen=True) class RealHandJointSpec: - """Description of one actively controlled hand joint.""" + """Robot joint data; glove sensor mappings belong to the device, not this profile.""" 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) @@ -61,27 +58,18 @@ def resolve_urdf(self, asset_root: str | Path) -> Path: 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." + "python -m isaaccapture_examples.teleop.fetch_realhand_assets 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, @@ -89,38 +77,35 @@ def _joint( 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), + _joint("index_mcp_pitch", 1.26, "index_curl"), + _joint("index_dip", 1.14, "index_curl"), + _joint("middle_mcp_pitch", 1.26, "middle_curl"), + _joint("middle_dip", 1.14, "middle_curl"), + _joint("pinky_mcp_pitch", 1.26, "pinky_curl"), + _joint("pinky_dip", 1.14, "pinky_curl"), + _joint("ring_mcp_pitch", 1.26, "ring_curl"), + _joint("ring_dip", 1.14, "ring_curl"), + _joint("thunb_cmc_roll", 1.39, "thumb_opposition"), + _joint("thumb_cmc_pitch", 0.99, "thumb_flex"), + _joint("thumb_dip", 1.22, "thumb_flex"), ) _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), + _joint("index_mcp_pitch", 1.26, "index_curl"), + _joint("index_dip", 1.14, "index_curl"), + _joint("middle_mcp_pitch", 1.26, "middle_curl"), + _joint("middle_dip", 1.14, "middle_curl"), + _joint("pinky_mcp_pitch", 1.26, "pinky_curl"), + _joint("pinky_dip", 1.14, "pinky_curl"), + _joint("ring_mcp_pitch", 1.26, "ring_curl"), + _joint("ring_dip", 1.14, "ring_curl"), + _joint("thunb_cmc_roll", 1.39, "thumb_opposition"), + _joint("thumb_cmc_pitch", 0.99, "thumb_flex"), + _joint("thumb_ip", 1.22, "thumb_flex"), ) _L6_ACTION_JOINTS = ( @@ -152,12 +137,12 @@ def _joint( 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), + _joint("thumb_cmc_yaw", thumb_yaw_upper, "thumb_opposition"), + _joint("thumb_cmc_pitch", 0.58, "thumb_flex"), + _joint("index_mcp_pitch", 1.60, "index_curl"), + _joint("middle_mcp_pitch", 1.60, "middle_curl"), + _joint("ring_mcp_pitch", 1.60, "ring_curl"), + _joint("pinky_mcp_pitch", 1.60, "pinky_curl"), ) @@ -167,41 +152,31 @@ def _l20_joints(side: str) -> tuple[RealHandJointSpec, ...]: 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), + _joint("thumb_cmc_roll", thumb_roll_upper, "thumb_rotation", closed=0.50), + _joint("thumb_cmc_yaw", 1.57, "thumb_opposition", closed=1.50), + _joint("thumb_cmc_pitch", thumb_pitch_upper, "thumb_base_flex", closed=0.60), + _joint("thumb_mcp", thumb_mcp_upper, "thumb_distal_flex", 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), - ): + for finger in ("index", "middle", "ring", "pinky"): 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, ), ) diff --git a/src/python/isaaccapture/retargeters/realhand/realhand_ffg_glove.py b/src/python/isaaccapture/retargeters/realhand/realhand_ffg_glove.py deleted file mode 100644 index 2f54ed7e42..0000000000 --- a/src/python/isaaccapture/retargeters/realhand/realhand_ffg_glove.py +++ /dev/null @@ -1,425 +0,0 @@ -# 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/realhand_ffg_glove/CMakeLists.txt b/tests/cpp/plugins/realhand_ffg_glove/CMakeLists.txt index f667faf084..792e3014c2 100644 --- a/tests/cpp/plugins/realhand_ffg_glove/CMakeLists.txt +++ b/tests/cpp/plugins/realhand_ffg_glove/CMakeLists.txt @@ -3,8 +3,14 @@ add_executable(test_realhand_ffg_glove_protocol test_realhand_ffg_glove_protocol.cpp + test_realhand_ffg_glove_reader.cpp + test_realhand_ffg_glove_skeleton.cpp + ${CMAKE_SOURCE_DIR}/src/plugins/realhand_ffg_glove/realhand_ffg_glove_skeleton.cpp ${CMAKE_SOURCE_DIR}/src/plugins/realhand_ffg_glove/realhand_ffg_glove_protocol.cpp + ${CMAKE_SOURCE_DIR}/src/plugins/realhand_ffg_glove/realhand_ffg_glove_reader.cpp + ${CMAKE_SOURCE_DIR}/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.cpp ) +target_link_libraries(test_realhand_ffg_glove_protocol PRIVATE Teleop::plugin_utils yaml-cpp::yaml-cpp) target_include_directories(test_realhand_ffg_glove_protocol PRIVATE ${CMAKE_SOURCE_DIR}/src/plugins/realhand_ffg_glove ) 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 index e682d90fc3..6b4a525838 100644 --- 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 @@ -9,10 +9,16 @@ #include #include #include +#include #include using namespace plugins::realhand_ffg_glove; +void run_reader_tests(); +void run_skeleton_tests(); +void write_skeleton_fixtures(const char* path); +void write_calibrated_skeleton_fixtures(const char* calibration_path, const char* output_path); + namespace { @@ -114,12 +120,32 @@ void test_a6_positions() } // namespace -int main() +int main(int argc, char** argv) { + if (argc == 4 && std::strcmp(argv[1], "--write-calibrated-skeleton-fixtures") == 0) + { + try + { + write_calibrated_skeleton_fixtures(argv[2], argv[3]); + return 0; + } + catch (const std::exception& error) + { + std::fprintf(stderr, "%s\n", error.what()); + return 1; + } + } + if (argc == 3 && std::strcmp(argv[1], "--write-skeleton-fixtures") == 0) + { + write_skeleton_fixtures(argv[2]); + return 0; + } test_pack_and_parse(); test_bad_checksum_and_resync(); test_float_positions(); test_a6_positions(); + run_reader_tests(); + run_skeleton_tests(); std::printf("OK: all %d checks passed\n", g_checks); return 0; } diff --git a/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_reader.cpp b/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_reader.cpp new file mode 100644 index 0000000000..64d1b5143a --- /dev/null +++ b/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_reader.cpp @@ -0,0 +1,198 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "realhand_ffg_glove_reader.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::realhand_ffg_glove; +using namespace std::chrono_literals; + +namespace +{ +void check(bool condition, const char* message) +{ + if (!condition) + { + std::fprintf(stderr, "FAIL: %s\n", message); + std::abort(); + } +} + +class FakeGlove +{ +public: + explicit FakeGlove(HandSide side = HandSide::Left) : side_(side) + { + fd_ = ::posix_openpt(O_RDWR | O_NOCTTY | O_NONBLOCK); + check(fd_ >= 0 && ::grantpt(fd_) == 0 && ::unlockpt(fd_) == 0, "create PTY"); + port = ::ptsname(fd_); + } + ~FakeGlove() + { + ::close(fd_); + } + void reply() + { + uint8_t bytes[1024]; + ssize_t size; + while ((size = ::read(fd_, bytes, sizeof(bytes))) > 0) + { + for (ssize_t i = 0; i < size; ++i) + { + if (auto frame = parser_.process(bytes[i])) + { + if (frame->command == Command::Version) + { + ++version_queries; + if (respond) + { + send(Command::Version, { 0x88, 0x4E, 0, 0, static_cast(side_ == HandSide::Right) }); + } + } + else if (frame->command == Command::Position) + { + ++position_queries; + if (respond) + { + std::vector payload(kNumSensors * sizeof(float)); + for (std::size_t index = 0; index < kNumSensors; ++index) + { + float angle = static_cast(index + 1); + std::memcpy(payload.data() + index * sizeof(float), &angle, sizeof(float)); + } + send(Command::Position, payload); + } + } + } + } + } + } + std::string port; + bool respond = true; + int version_queries = 0; + int position_queries = 0; + +private: + void send(Command command, const std::vector& payload) + { + const auto bytes = pack_frame(command, payload); + check(::write(fd_, bytes.data(), bytes.size()) == static_cast(bytes.size()), "PTY reply"); + } + int fd_ = -1; + HandSide side_; + FrameParser parser_; +}; + +void pump(GloveReader& reader, std::initializer_list gloves, std::chrono::milliseconds duration) +{ + const auto until = GloveReader::Clock::now() + duration; + while (GloveReader::Clock::now() < until) + { + const auto start = GloveReader::Clock::now(); + reader.update(); + check(GloveReader::Clock::now() - start < 100ms, "discovery must not wait 250ms for a silent device"); + for (auto* glove : gloves) + { + glove->reply(); + } + std::this_thread::sleep_for(2ms); + } +} + +void test_cold_start_and_silent_port() +{ + FakeGlove left; + FakeGlove silent; + silent.respond = false; + GloveReader reader({ left.port, silent.port }, { 460800, 921600 }); + pump(reader, { &left, &silent }, 650ms); + check(reader.endpoints()[0] && reader.endpoints()[0]->has_positions, "cold start discovers and queries left glove"); + check(!reader.endpoints()[1], "one glove never populates opposite side"); + check(left.position_queries >= 25, "silent probes must not stall connected glove queries"); + check(reader.endpoints()[0]->positions[0] > 0, "live position data decoded"); + left.respond = false; + pump(reader, { &left, &silent }, 1100ms); + check(!reader.endpoints()[0], "stale glove disconnected"); + left.respond = true; + pump(reader, { &left, &silent }, 2100ms); + check(reader.endpoints()[0] && reader.endpoints()[0]->has_positions, "disconnected glove rediscovered"); +} + +void test_discovery_stops_with_two_gloves() +{ + FakeGlove left; + FakeGlove right(HandSide::Right); + FakeGlove unused; + GloveReader reader({ left.port, right.port, unused.port }, { 460800 }); + pump(reader, { &left, &right, &unused }, 100ms); + check(reader.endpoints()[0] && reader.endpoints()[1], "both sides identified"); + check(unused.version_queries == 0, "no extra serial ports probed after both gloves connect"); +} + +void test_busy_port_is_not_modified() +{ + FakeGlove device; + const int owner = ::open(device.port.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + check(owner >= 0, "open non-exclusive device owner"); + termios before{}, after{}; + check(::tcgetattr(owner, &before) == 0, "read original serial configuration"); + bool rejected = false; + try + { + SerialGlove glove(device.port, 460800); + } + catch (const std::runtime_error&) + { + rejected = true; + } + check(rejected, "reject already open non-exclusive serial port"); + check(::tcgetattr(owner, &after) == 0 && ::cfgetospeed(&after) == ::cfgetospeed(&before), "busy port baud unchanged"); + device.reply(); + check(device.version_queries == 0, "no injected probe bytes on busy port"); + ::close(owner); + { + SerialGlove glove(device.port, 460800); + } + { + SerialGlove glove(device.port, 460800); + } +} + +void test_writes_timeout_when_device_stalls() +{ + FakeGlove device; + SerialGlove glove(device.port, 460800); + bool timed_out = false; + for (int i = 0; i < 10000 && !timed_out; ++i) + { + const auto start = GloveReader::Clock::now(); + try + { + glove.send(Command::ForceFeedback, std::vector(255, 0)); + } + catch (const std::runtime_error&) + { + timed_out = true; + } + check(GloveReader::Clock::now() - start < 100ms, "non-draining output must have a bounded write"); + } + check(timed_out, "backpressure reaches write timeout"); +} +} // namespace + +void run_reader_tests() +{ + test_cold_start_and_silent_port(); + test_discovery_stops_with_two_gloves(); + test_busy_port_is_not_modified(); + test_writes_timeout_when_device_stalls(); +} diff --git a/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_skeleton.cpp b/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_skeleton.cpp new file mode 100644 index 0000000000..7ab8f04f94 --- /dev/null +++ b/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_skeleton.cpp @@ -0,0 +1,522 @@ +// 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_skeleton.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace plugins::realhand_ffg_glove; + +namespace +{ +void check(bool value, const char* message) +{ + if (!value) + { + std::fprintf(stderr, "FAIL: skeleton: %s\n", message); + std::abort(); + } +} + +float distance(const XrVector3f& a, const XrVector3f& b) +{ + return std::sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z)); +} + +bool same(const XrHandJointLocationEXT& a, const XrHandJointLocationEXT& b) +{ + return distance(a.pose.position, b.pose.position) < 1e-6f && + std::abs(a.pose.orientation.x - b.pose.orientation.x) < 1e-6f && + std::abs(a.pose.orientation.y - b.pose.orientation.y) < 1e-6f && + std::abs(a.pose.orientation.z - b.pose.orientation.z) < 1e-6f && + std::abs(a.pose.orientation.w - b.pose.orientation.w) < 1e-6f; +} + +YAML::Node calibration(float origin = 0.0f, float span = 1.0f) +{ + YAML::Node data; + data["format"] = "realhand-ffg-glove-human-calibration-v1"; + for (const auto* side : { "left", "right" }) + { + for (const auto* pose : { "open", "fist", "thumb_curl", "thumb_opposition", "spread" }) + { + for (std::size_t sensor = 0; sensor < kNumSensors; ++sensor) + { + data["hands"][side][pose].push_back(origin + (std::string(pose) == "open" ? 0.0f : span)); + } + } + } + return data; +} + +struct CalibrationFile +{ + std::filesystem::path path; + CalibrationFile() + { + char name[] = "/tmp/realhand-ffg-human-XXXXXX"; + const int fd = mkstemp(name); + check(fd >= 0, "create temporary calibration"); + close(fd); + path = name; + } + ~CalibrationFile() + { + std::filesystem::remove(path); + } + void save(const YAML::Node& data) const + { + std::ofstream stream(path); + stream << data; + check(stream.good(), "write calibration"); + } +}; + +void expect_invalid(const YAML::Node& data, const CalibrationFile& file) +{ + file.save(data); + bool rejected = false; + try + { + HumanHandSkeleton model(file.path.string()); + } + catch (const std::exception&) + { + rejected = true; + } + check(rejected, "reject invalid calibration"); +} +} // namespace + +void run_skeleton_tests() +{ + { + RealHandFFGGloveConfig config; + const auto& left = config.wrist.left_aim_to_wrist; + const auto& right = config.wrist.right_aim_to_wrist; + check(config.wrist.mode == plugin_utils::WristSourceMode::Auto, "mount defaults preserve wrist source selection"); + check(distance(left.position, { 0.020534770647f, -0.047368722071f, 0.192378854628f }) < 1e-7f, + "default PICO bracket translation"); + check(distance(right.position, { -left.position.x, left.position.y, left.position.z }) < 1e-7f, + "default bracket positions are mirrored"); + const auto& q = left.orientation; + check(std::abs(q.x + 0.422424918198f) < 1e-7f && std::abs(q.y - 0.014580024139f) < 1e-7f && + std::abs(q.z - 0.024960358040f) < 1e-7f && std::abs(q.w - 0.905936858676f) < 1e-7f, + "default PICO bracket orientation"); + check(std::abs(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w - 1.0f) < 1e-6f, + "default bracket rotation is normalized"); + for (const auto point : { XrVector3f{ 0, 0, 0 }, XrVector3f{ 0.03f, 0.04f, -0.15f } }) + { + const auto a = oxr_utils::multiply_poses(left, { { 0, 0, 0, 1 }, point }); + const auto b = oxr_utils::multiply_poses(right, { { 0, 0, 0, 1 }, { -point.x, point.y, point.z } }); + check(distance(b.position, { -a.position.x, a.position.y, a.position.z }) < 1e-6f, + "default bracket rotations preserve mirrored hand geometry"); + } + config.wrist.left_aim_to_wrist = { { 0, 0, 0, 1 }, { 0, 0, 0 } }; + check(config.wrist.left_aim_to_wrist.position.z == 0 && config.wrist.right_aim_to_wrist.position.z > 0, + "each bracket offset remains independently overridable"); + } + { + WristPoseContinuity left, right; + XrPosef pose{ { 0, 0, 0, 1 }, { 0, 0, 0 } }; + check(left.accept(pose, 1'000'000'000, true), "initial valid wrist accepted"); + check(right.accept(pose, 1'000'000'000, true), "independent right wrist accepted"); + pose.position.x = 0.243f; + check(!left.accept(pose, 1'012'000'000, true), "reject recorded-scale 24 cm controller teleport"); + check(right.accept({ { 0, 0, 0, -1 }, { 0, 0, 0 } }, 1'012'000'000, true), + "left dropout and quaternion sign do not affect the right hand"); + check(!left.accept(pose, 1'024'000'000, true), "first stable sample remains inactive"); + check(!left.accept(pose, 1'036'000'000, true), "second stable sample remains inactive"); + check(left.accept(pose, 1'048'000'000, true), "reacquire without changing the wrist pose"); + check(!left.accept(pose, 1'060'000'000, false), "missing controller does not publish a local-origin hand"); + check(!left.accept(pose, 1'072'000'000, true), "tracking loss requires reacquisition"); + check(!left.accept(pose, 1'072'000'000, true), "nonmonotonic timestamps rejected"); + pose.position.x = std::numeric_limits::quiet_NaN(); + check(!left.accept(pose, 1'084'000'000, true), "nonfinite wrist rejected"); + WristPoseContinuity fast; + pose = { { 0, 0, 0, 1 }, { 0, 0, 0 } }; + check(fast.accept(pose, 2'000'000'000, true), "fast motion baseline"); + pose.position.x = 0.04f; + check(fast.accept(pose, 2'012'000'000, true), "ordinary fast motion passes unchanged"); + pose.orientation = { 1, 0, 0, 0 }; + check(!fast.accept(pose, 2'024'000'000, true), "controller orientation teleport rejected"); + } + CalibrationFile file; + file.save(calibration()); + const HumanHandSkeleton model(file.path.string()); + const HumanHandSkeleton large(file.path.string(), 1.5f); + std::array sensors{}; + const auto open = *model.reconstruct(sensors, HandSide::Left); + check(open.size() == 26, "all OpenXR joints populated"); + for (std::size_t base : { 6u, 11u, 16u, 21u }) + { + const auto& root = open[base].pose.position; + const auto& knuckle = open[base + 1].pose.position; + const float fraction = (root.x * knuckle.x + root.y * knuckle.y + root.z * knuckle.z) / + (knuckle.x * knuckle.x + knuckle.y * knuckle.y + knuckle.z * knuckle.z); + const XrVector3f projected{ fraction * knuckle.x, fraction * knuckle.y, fraction * knuckle.z }; + check(fraction > 0.0f && fraction < 1.0f && distance(root, projected) < 1e-6f, + "neutral metacarpal root lies between the wrist and its knuckle"); + } + for (float span : { -0.2f, 0.2f, 0.0f }) + { + auto data = calibration(6.2f); + for (const auto* name : { "left", "right" }) + { + for (std::size_t sensor = 5; sensor < kNumSensors; sensor += 4) + { + data["hands"][name]["spread"][sensor] = 6.2f + span; + } + } + file.save(data); + const HumanHandSkeleton lateral_model(file.path.string()); + for (const auto side : { HandSide::Left, HandSide::Right }) + { + sensors.fill(6.2f); + const auto neutral = *lateral_model.reconstruct(sensors, side); + const float mirror = side == HandSide::Left ? 1.0f : -1.0f; + for (std::size_t finger = 0; finger < 4; ++finger) + { + const auto sensor = 5 + 4 * finger; + const auto tip = 10 + 5 * finger; + float last_x = 1.0f; + for (int step = -10; step <= 10; ++step) + { + sensors.fill(6.2f); + const float delta = step * 0.02f; + // Equivalent anatomical motion has opposite raw lateral deltas on the two gloves. + sensors[sensor] = std::remainder(6.2f + mirror * delta, 2.0f * std::numbers::pi_v); + const auto moved = *lateral_model.reconstruct(sensors, side); + const float x = mirror * (moved[tip].pose.position.x - neutral[tip].pose.position.x); + check(step == 0 ? std::abs(x) < 1e-6f : x * delta < 0.0f, + "side-normalized lateral delta moves toward little-finger side for either hand"); + check(x <= last_x + 1e-6f, "lateral sweep is monotonic through zero and phase wrap"); + last_x = x; + check(std::abs(moved[tip].pose.position.y - neutral[tip].pose.position.y) < 1e-6f, + "sideways motion does not introduce flexion"); + for (std::size_t joint = 0; joint < moved.size(); ++joint) + { + if (joint < tip - 4 || joint > tip) + { + check(same(neutral[joint], moved[joint]), "lateral input never moves another finger"); + } + } + } + } + } + } + for (int step = 0; step <= 20; ++step) + { + sensors.fill(step / 20.0f); + const auto left = *model.reconstruct(sensors, HandSide::Left); + auto right_sensors = sensors; + for (std::size_t sensor = 5; sensor < kNumSensors; sensor += 4) + { + right_sensors[sensor] = -sensors[sensor]; + } + const auto right = *model.reconstruct(right_sensors, HandSide::Right); + const auto scaled = *large.reconstruct(sensors, HandSide::Left); + for (std::size_t joint = 0; joint < left.size(); ++joint) + { + const auto& p = left[joint].pose.position; + const auto& q = left[joint].pose.orientation; + check(std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z), "finite positions"); + check(std::abs(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w - 1.0f) < 1e-5f, "unit rotations"); + check(left[joint].locationFlags == + (XR_SPACE_LOCATION_POSITION_VALID_BIT | XR_SPACE_LOCATION_ORIENTATION_VALID_BIT), + "estimated skeleton is valid but not independently tracked"); + check(left[joint].radius > 0.0f, "positive radii"); + check(std::abs(p.x + right[joint].pose.position.x) < 1e-6f && + std::abs(p.y - right[joint].pose.position.y) < 1e-6f && + std::abs(p.z - right[joint].pose.position.z) < 1e-6f, + "symmetric left/right anatomy"); + check(std::abs(q.x - right[joint].pose.orientation.x) < 1e-6f && + std::abs(q.y + right[joint].pose.orientation.y) < 1e-6f && + std::abs(q.z + right[joint].pose.orientation.z) < 1e-6f && + std::abs(q.w - right[joint].pose.orientation.w) < 1e-6f, + "mirrored rotations"); + check(distance(scaled[joint].pose.position, { p.x * 1.5f, p.y * 1.5f, p.z * 1.5f }) < 1e-6f, + "hand size scales geometry consistently"); + } + for (std::size_t base : { 2u, 6u, 11u, 16u, 21u }) + { + const auto end = base == 2 ? 5u : base + 4; + if (base != 2) + { + const float length = distance(left[base].pose.position, left[base + 1].pose.position); + const auto knuckle = + oxr_utils::multiply_poses(left[base].pose, { { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, -length } }); + check(distance(knuckle.position, left[base + 1].pose.position) < 1e-6f, + "metacarpal minus-Z axis points along its bone toward the knuckle"); + } + for (std::size_t joint = base; joint < end; ++joint) + { + check(std::abs(distance(left[joint].pose.position, left[joint + 1].pose.position) - + distance(open[joint].pose.position, open[joint + 1].pose.position)) < 1e-6f, + "bone lengths remain rigid across flexion"); + } + } + } + for (std::size_t sensor = 0; sensor < kNumSensors; ++sensor) + { + sensors.fill(0.0f); + sensors[sensor] = 0.7f; + const auto moved = *model.reconstruct(sensors, HandSide::Left); + const std::size_t digit = sensor < 5 ? 0 : 1 + (sensor - 5) / 4; + const std::size_t first = digit == 0 ? 2 : 6 + (digit - 1) * 5; + const std::size_t last = digit == 0 ? 5 : first + 4; + for (std::size_t joint = 0; joint < moved.size(); ++joint) + { + if (joint < first || joint > last) + { + check(same(open[joint], moved[joint]), "each sensor affects only its own digit"); + } + } + const bool unused = sensor == 3 || (sensor >= 5 && (sensor - 5) % 4 == 2); + if (!unused) + { + check(!same(open[last], moved[last]), "every supported sensor has motion response"); + } + } + for (float sign : { -1.0f, 1.0f }) + { + file.save(calibration(6.0f, sign * 2.7f)); + const HumanHandSkeleton wrapped(file.path.string()); + sensors.fill(6.0f + sign * 2.7f); + for (std::size_t sensor = 5; sensor < kNumSensors; sensor += 4) + { + sensors[sensor] = 6.0f; + } + const auto boundary = *wrapped.reconstruct(sensors, HandSide::Left); + for (float overshoot : { 0.01f, 0.2f, 0.5f }) + { + sensors.fill(std::remainder(6.0f + sign * (2.7f + overshoot), 2.0f * std::numbers::pi_v)); + for (std::size_t sensor = 5; sensor < kNumSensors; sensor += 4) + { + sensors[sensor] = 6.0f; + } + const auto saturated = *wrapped.reconstruct(sensors, HandSide::Left); + for (std::size_t joint = 2; joint <= 5; ++joint) + { + check(same(boundary[joint], saturated[joint]), "thumb stays curled beyond wrapped endpoint"); + } + for (std::size_t finger = 0; finger < 4; ++finger) + { + const auto joint = 6 + finger * 5; + check(std::abs(boundary[joint + 1].pose.orientation.x - saturated[joint + 1].pose.orientation.x) < 1e-5f, + "MCP saturation across angular wrap"); + } + } + } + const XrPosef wrist{ { 0.0f, 0.0f, 1.0f, 0.0f }, { 1.0f, 2.0f, 3.0f } }; + const auto placed = place_hand(open, wrist, true, true); + const auto cached = place_hand(open, wrist, true, false); + const auto absent = place_hand(placed, wrist, false, true); + for (std::size_t joint = 0; joint < open.size(); ++joint) + { + const auto p = open[joint].pose.position; + check(distance(placed[joint].pose.position, { 1.0f - p.x, 2.0f - p.y, 3.0f + p.z }) < 1e-6f, + "global wrist rigid transform"); + check((placed[joint].locationFlags & XR_SPACE_LOCATION_POSITION_TRACKED_BIT) != 0, "live wrist tracked"); + check(cached[joint].locationFlags == open[joint].locationFlags, "cached wrist not tracked"); + check(absent[joint].locationFlags == open[joint].locationFlags, "missing wrist clears stale tracked flags"); + } + auto data = calibration(); + data["hands"].remove("right"); + file.save(data); + const HumanHandSkeleton left_only(file.path.string()); + sensors.fill(0.0f); + check(!left_only.reconstruct(sensors, HandSide::Right), "never borrow opposite side calibration"); + check(!HumanHandSkeleton().reconstruct(sensors, HandSide::Left), "uncalibrated hands inactive"); + sensors[0] = std::numeric_limits::quiet_NaN(); + check(!model.reconstruct(sensors, HandSide::Left), "nonfinite input does not publish a hand"); + data = calibration(); + data["format"] = "realhand-ffg-glove-raw-calibration-v2"; + expect_invalid(data, file); + data = calibration(); + data["hands"]["left"].remove("thumb_opposition"); + expect_invalid(data, file); + data = calibration(); + data["hands"]["left"]["fist"][6] = 0.0f; + expect_invalid(data, file); + data = calibration(); + data["hands"]["right"]["open"][0] = std::numeric_limits::infinity(); + expect_invalid(data, file); + data = calibration(); + data["hands"]["left"]["thumb_opposition"][0] = 0.049f; + expect_invalid(data, file); + auto mapping = YAML::Load(R"( +format: realhand-ffg-thumb-quadratic-v1 +input_origin: [0, 0, 0, 0] +input_scale: [1, 1, 1, 1] +input_min: [0, 0, 0, 0] +input_max: [0.049, 1, 1, 1] +base_position: [0.025, -0.005, -0.020] +lengths: [0.035, 0.030, 0.026] +coefficients: + - [-0.85, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] +)"); + data["hands"]["left"]["thumb_mapping"] = mapping; + file.save(data); + const HumanHandSkeleton multivariate(file.path.string()); + sensors.fill(0.0f); + check(multivariate.reconstruct(sensors, HandSide::Left).has_value(), + "multivariate thumb fit does not require a legacy single-pose span"); + for (std::size_t sensor = 0; sensor < 4; ++sensor) + { + data["hands"]["left"]["thumb_mapping"]["input_max"][sensor] = 0.0f; + } + expect_invalid(data, file); + std::printf("OK: 26-joint geometry, independence, wrap, placement, and calibration checks passed\n"); +} + +void write_skeleton_fixtures(const char* path) +{ + CalibrationFile file; + file.save(calibration()); + const HumanHandSkeleton model(file.path.string()); + YAML::Node data; + for (const auto side : { HandSide::Left, HandSide::Right }) + { + const auto* name = side == HandSide::Left ? "left" : "right"; + for (const float flexion : { 0.0f, 0.5f, 1.0f }) + { + std::array sensors{}; + sensors.fill(flexion); + auto joints = *model.reconstruct(sensors, side); + // Spread is tested independently; use neutral lateral channels in the curl sweep. + for (std::size_t sensor = 5; sensor < kNumSensors; sensor += 4) + { + sensors[sensor] = 0.0f; + } + joints = *model.reconstruct(sensors, side); + YAML::Node sample; + sample["flexion"] = flexion; + for (const auto& joint : joints) + { + const auto& p = joint.pose.position; + const auto& q = joint.pose.orientation; + YAML::Node location; + location["position"] = std::vector{ p.x, p.y, p.z }; + location["orientation"] = std::vector{ q.x, q.y, q.z, q.w }; + location["radius"] = joint.radius; + sample["joints"].push_back(location); + } + data[name].push_back(sample); + } + } + std::ofstream stream(path); + stream << data; + check(stream.good(), "write production skeleton fixtures"); +} + +void write_calibrated_skeleton_fixtures(const char* calibration_path, const char* output_path) +{ + const auto calibration_data = YAML::LoadFile(calibration_path); + const HumanHandSkeleton model(calibration_path); + YAML::Node data; + for (const auto side : { HandSide::Left, HandSide::Right }) + { + const auto* name = side == HandSide::Left ? "left" : "right"; + const auto poses = calibration_data["hands"][name]; + if (!poses) + { + continue; + } + const auto append = + [&](const std::string& key, const std::array& sensors, const char* collection) + { + const auto joints = *model.reconstruct(sensors, side); + YAML::Node sample; + sample["name"] = key; + for (float value : sensors) + { + sample["raw"].push_back(value); + } + for (const auto& joint : joints) + { + const auto& p = joint.pose.position; + const auto& q = joint.pose.orientation; + YAML::Node location; + location["position"] = std::vector{ p.x, p.y, p.z }; + location["orientation"] = std::vector{ q.x, q.y, q.z, q.w }; + sample["joints"].push_back(location); + } + data[collection][name].push_back(sample); + }; + for (std::size_t finger = 0; finger < 4; ++finger) + { + for (float delta : { -0.05f, 0.05f }) + { + std::array sensors{}; + for (std::size_t i = 0; i < kNumSensors; ++i) + { + sensors[i] = poses["open"][i].as(); + } + sensors[5 + 4 * finger] += (side == HandSide::Left ? delta : -delta); + append(std::to_string(finger), sensors, "lateral"); + } + } + for (const auto* key : + { "open", "fist", "thumb_curl", "thumb_index", "thumb_middle", "thumb_ring", "thumb_little" }) + { + if (!poses[key]) + { + continue; + } + std::array sensors{}; + for (std::size_t i = 0; i < kNumSensors; ++i) + { + sensors[i] = poses[key][i].as(); + } + append(key, sensors, "poses"); + for (int step = 0; step <= 100; ++step) + { + auto current = sensors; + for (std::size_t i = 0; i < kNumSensors; ++i) + { + const float origin = poses["open"][i].as(); + current[i] = + origin + (step / 100.0f) * std::remainder(sensors[i] - origin, 2.0f * std::numbers::pi_v); + } + append(key + std::string("_") + std::to_string(step), current, "sweeps"); + } + for (float shift : { -0.8f, 0.8f }) + { + auto current = sensors; + for (std::size_t i = 5; i < kNumSensors; ++i) + { + current[i] += shift; + } + append(key + std::string("_other_fingers"), current, "independence"); + } + for (float shift : { -2.0f * std::numbers::pi_v, 2.0f * std::numbers::pi_v }) + { + auto current = sensors; + for (float& value : current) + { + value += shift; + } + append(key + std::string("_phase_wrap"), current, "phase_wrap"); + } + } + } + std::ofstream stream(output_path); + stream << data; + check(stream.good(), "write calibrated production skeleton fixtures"); +} diff --git a/tests/python/CMakeLists.txt b/tests/python/CMakeLists.txt index f2609a4e58..3135c76150 100644 --- a/tests/python/CMakeLists.txt +++ b/tests/python/CMakeLists.txt @@ -26,7 +26,7 @@ if(BUILD_VIZ AND BUILD_PYTHON_BINDINGS) add_subdirectory(viz) endif() -if(BUILD_EXAMPLES AND BUILD_VIZ) +if(BUILD_EXAMPLES) add_subdirectory(examples) endif() diff --git a/tests/python/core/retargeting_engine/conftest.py b/tests/python/core/retargeting_engine/conftest.py index 63674cbd29..1b41a3a31d 100644 --- a/tests/python/core/retargeting_engine/conftest.py +++ b/tests/python/core/retargeting_engine/conftest.py @@ -1,44 +1,10 @@ # 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/test_realhand_example.py b/tests/python/core/retargeting_engine/test_realhand_example.py deleted file mode 100644 index 2ce0d0bca7..0000000000 --- a/tests/python/core/retargeting_engine/test_realhand_example.py +++ /dev/null @@ -1,61 +0,0 @@ -# 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 index 2d4555ebfe..e1eb3d8966 100644 --- a/tests/python/core/retargeting_engine/test_realhand_retargeters.py +++ b/tests/python/core/retargeting_engine/test_realhand_retargeters.py @@ -5,21 +5,16 @@ import numpy as np import pytest +import subprocess +import sys 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 ( @@ -35,18 +30,8 @@ ) 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( @@ -79,113 +64,6 @@ def _fill_controller(group, *, position=(0.0, 0.0, 0.0), trigger=0.0, squeeze=0. 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) @@ -197,151 +75,44 @@ def test_profile_resolves_downloaded_urdf(model, tmp_path): def test_profile_reports_asset_fetch_command_when_urdf_is_missing(tmp_path): - with pytest.raises(FileNotFoundError, match=r"fetch_realhand_assets\.py"): + with pytest.raises( + FileNotFoundError, + match=r"python -m isaaccapture_examples\.teleop\.fetch_realhand_assets", + ): 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_hand_imports_do_not_require_scipy_and_arm_has_install_hint(): + script = """ +import sys +import importlib.abc +class BlockScipy(importlib.abc.MetaPathFinder): + def find_spec(self, fullname, path=None, target=None): + if fullname == "scipy" or fullname.startswith("scipy."): + raise ModuleNotFoundError("blocked scipy", name=fullname) +sys.meta_path.insert(0, BlockScipy()) +from isaaccapture.retargeters import ( + ControllerTriggerRealHandRetargeter, ControllerTriggerRealHandRetargeterConfig, +) +from isaaccapture.retargeters.realhand import get_realhand_profile +assert get_realhand_profile("l6").model == "l6" +from isaaccapture import retargeters +try: + retargeters.P7ControllerPoseRetargeter +except ModuleNotFoundError as error: + assert "isaaccapture[retargeters-lite]" in str(error) +else: + raise AssertionError("arm retargeter must require scipy") +""" + subprocess.run([sys.executable, "-c", script], check=True) + + def test_p7_controller_pose_maps_absolute_workspace_and_clamps(): retargeter = P7ControllerPoseRetargeter( P7WorkspacePoseConfig( @@ -387,38 +158,3 @@ def test_controller_trigger_closes_only_configured_hand(model, side): 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 diff --git a/tests/python/examples/CMakeLists.txt b/tests/python/examples/CMakeLists.txt index a0cc7b40cc..b90b73e1a3 100644 --- a/tests/python/examples/CMakeLists.txt +++ b/tests/python/examples/CMakeLists.txt @@ -1,7 +1,12 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -add_subdirectory(camera_viz) -add_subdirectory(deviceio_live_view) -add_subdirectory(soma_body_publisher) -add_subdirectory(soma_hand_publisher) +if(BUILD_VIZ) + add_subdirectory(camera_viz) + add_subdirectory(deviceio_live_view) + add_subdirectory(soma_body_publisher) + add_subdirectory(soma_hand_publisher) +endif() +if(BUILD_PYTHON_BINDINGS) + add_subdirectory(realhand) +endif() diff --git a/tests/python/examples/realhand/CMakeLists.txt b/tests/python/examples/realhand/CMakeLists.txt new file mode 100644 index 0000000000..dcf7641f0c --- /dev/null +++ b/tests/python/examples/realhand/CMakeLists.txt @@ -0,0 +1,21 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +file(GLOB TEST_FILES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py") +set(SKELETON_ARGS) +if(TARGET test_realhand_ffg_glove_protocol) + list(APPEND SKELETON_ARGS --ffg-skeleton-executable=$) +endif() +foreach(test_file ${TEST_FILES}) + get_filename_component(test_name "${test_file}" NAME_WE) + add_test( + NAME "realhand_example_${test_name}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev pytest -v --tb=short "${test_file}" + ${SKELETON_ARGS} + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + set_tests_properties("realhand_example_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "realhand_examples" + ) +endforeach() diff --git a/tests/python/examples/realhand/conftest.py b/tests/python/examples/realhand/conftest.py new file mode 100644 index 0000000000..6cdc13aba0 --- /dev/null +++ b/tests/python/examples/realhand/conftest.py @@ -0,0 +1,68 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import pytest +import yaml +import subprocess +from pathlib import Path +from repo_paths import repo_root +import sys + +sys.path.insert(0, str(repo_root() / "examples/teleop/python")) + + +def pytest_addoption(parser): + parser.addoption("--ffg-skeleton-executable", default=None) + parser.addoption("--ffg-human-calibration", default=None) + + +@pytest.fixture(scope="session") +def calibrated_skeleton_samples(request, tmp_path_factory): + calibration = request.config.getoption("--ffg-human-calibration") + executable = request.config.getoption("--ffg-skeleton-executable") + if calibration is None or executable is None: + pytest.skip( + "Pass --ffg-human-calibration and --ffg-skeleton-executable for local replay" + ) + assert Path(calibration).is_file() + assert Path(executable).is_file() + output = tmp_path_factory.mktemp("calibrated_ffg_skeleton") / "samples.yml" + subprocess.run( + [executable, "--write-calibrated-skeleton-fixtures", calibration, str(output)], + check=True, + ) + return yaml.safe_load(output.read_text(encoding="utf-8")) + + +@pytest.fixture(scope="session") +def skeleton_samples(request, tmp_path_factory): + executable = request.config.getoption("--ffg-skeleton-executable") + if executable is None: + pytest.skip( + "Build the FFG C++ test executable and pass --ffg-skeleton-executable" + ) + assert Path(executable).is_file() + output = tmp_path_factory.mktemp("ffg_skeleton") / "samples.yml" + subprocess.run([executable, "--write-skeleton-fixtures", str(output)], check=True) + return yaml.safe_load(output.read_text(encoding="utf-8")) + + +@pytest.fixture +def human_calibration(tmp_path): + """Device measurements have no target robot model or joint names.""" + data = { + "format": "realhand-ffg-glove-human-calibration-v1", + "hands": { + side: { + "open": [0.0] * 21, + "fist": [1.0] * 21, + "thumb_curl": [1.0] * 21, + "thumb_opposition": [1.0] * 21, + "spread": [0.3] * 21, + } + for side in ("left", "right") + }, + } + path = tmp_path / "ffg_human.yml" + path.write_text(yaml.safe_dump(data), encoding="utf-8") + return path diff --git a/tests/python/examples/realhand/pyproject.toml b/tests/python/examples/realhand/pyproject.toml new file mode 100644 index 0000000000..062a7968be --- /dev/null +++ b/tests/python/examples/realhand/pyproject.toml @@ -0,0 +1,28 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "realhand-example-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = [ + "pytest", "numpy", "packaging", + # Mirror the runtime retargeters extra so tests exercise the shipped solver stack. + "dex-retargeting>=0.4.6,<0.6.0", "six>=1.16", + "pyyaml>=6.0.3", "scipy>=1.15.0", "torch>=2.7.0", + "nlopt>=2.6.2", "pin>=4.1", + "cmeel-urdfdom>=6,<7", "cmeel-tinyxml2>=11,<12", +] + +[tool.uv.sources] +torch = { index = "pytorch-cpu" } + +[[tool.uv.index]] +name = "pytorch-cpu" +url = "https://download.pytorch.org/whl/cpu" +explicit = true + +[tool.pytest.ini_options] +pythonpath = ["../.."] diff --git a/tests/python/core/retargeting_engine/test_realhand_asset_fetcher.py b/tests/python/examples/realhand/test_realhand_asset_fetcher.py similarity index 63% rename from tests/python/core/retargeting_engine/test_realhand_asset_fetcher.py rename to tests/python/examples/realhand/test_realhand_asset_fetcher.py index 1bde58d9ae..36c3bc3ced 100644 --- a/tests/python/core/retargeting_engine/test_realhand_asset_fetcher.py +++ b/tests/python/examples/realhand/test_realhand_asset_fetcher.py @@ -4,30 +4,20 @@ """Checks for the pinned RealHand robot-asset fetcher.""" import hashlib -import importlib.util -import sys -from pathlib import Path +import io 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) +from isaaccapture_examples.teleop import fetch_realhand_assets as _FETCHER + + +def test_default_assets_use_user_cache(monkeypatch, tmp_path): + monkeypatch.setenv("XDG_CACHE_HOME", str(tmp_path)) + assert _FETCHER._default_output_dir() == tmp_path / "isaaccapture/realhand" def _sibling(path: str, size: int = 4, sha256: str | None = None) -> dict: - sibling = {"rfilename": path, "size": size} + sibling = {"rfilename": path, "size": size, "blobId": "b" * 40} if sha256 is not None: sibling["lfs"] = {"sha256": sha256, "size": size} return sibling @@ -63,6 +53,41 @@ def test_repository_metadata_rejects_a_moved_revision(monkeypatch): _FETCHER._repository_metadata() +def test_non_lfs_file_without_blob_hash_is_rejected(): + root = _FETCHER.REMOTE_ASSET_ROOT + metadata = {"siblings": [{"rfilename": f"{root}/p7_l6/mesh.stl", "size": 4}]} + with pytest.raises(RuntimeError, match="Git blob hash"): + _FETCHER._selected_files(metadata, {"l6"}) + + +@pytest.mark.parametrize("is_lfs", (False, True)) +def test_same_size_corruption_is_detected_and_repaired(tmp_path, monkeypatch, is_lfs): + content = b"mesh" + blob_id = hashlib.sha1(b"blob 4\0" + content, usedforsecurity=False).hexdigest() + remote = _FETCHER.RemoteFile( + "LICENSE", + 4, + hashlib.sha256(content).hexdigest() if is_lfs else None, + None if is_lfs else blob_id, + ) + target = tmp_path / "LICENSE" + target.write_bytes(b"oops") + assert not _FETCHER._is_complete(target, remote) + monkeypatch.setattr( + _FETCHER.urllib.request, "urlopen", lambda *args, **kwargs: io.BytesIO(content) + ) + assert _FETCHER._download(remote, tmp_path, False) == (target, True) + assert target.read_bytes() == content + assert _FETCHER._download(remote, tmp_path, False) == (target, False) + monkeypatch.setattr( + _FETCHER.urllib.request, "urlopen", lambda *args, **kwargs: io.BytesIO(b"evil") + ) + with pytest.raises(RuntimeError, match=r"hash|SHA-256"): + _FETCHER._download(remote, tmp_path, True) + assert target.read_bytes() == content + assert not list(tmp_path.glob("*.part-*")) + + @pytest.mark.parametrize("path", ("../secret", "/absolute/path")) def test_remote_path_rejects_traversal(path): with pytest.raises(ValueError, match="Unsafe repository path"): diff --git a/tests/python/examples/realhand/test_realhand_dex_config.py b/tests/python/examples/realhand/test_realhand_dex_config.py new file mode 100644 index 0000000000..20efba4a66 --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_dex_config.py @@ -0,0 +1,367 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Validate robot data and reachable targets using the unmodified official solver.""" + +import xml.etree.ElementTree as ET +from copy import deepcopy +from pathlib import Path + +import numpy as np +import pytest +import yaml +from scipy.spatial.transform import Rotation + +from isaaccapture.retargeters import DexHandRetargeter +from isaaccapture.retargeters.realhand.profiles import get_realhand_profile +from isaaccapture.retargeting_engine.tensor_types import HandJointIndex +from test_realhand_example import _EXAMPLE +from test_realhand_ffg_glove_hand_input import _feed_samples + + +def _robot_positions(retargeter, qpos, names): + robot = retargeter._dex_hand.optimizer.robot + robot.compute_forward_kinematics(qpos) + return np.asarray( + [robot.get_link_pose(robot.get_link_index(name))[:3, 3] for name in names] + ) + + +def _full_qpos(retargeter, active): + optimizer = retargeter._dex_hand.optimizer + qpos = np.zeros(optimizer.robot.dof) + for name, value in zip(retargeter._config.hand_joint_names, active): + qpos[optimizer.robot.get_joint_index(name)] = value + if optimizer.adaptor is not None: + qpos = optimizer.adaptor.forward_qpos(qpos) + return qpos + + +def _reachable_sample(retargeter, data, qpos, rotation, translation): + origins = _robot_positions(retargeter, qpos, data["target_origin_link_names"]) + tasks = _robot_positions(retargeter, qpos, data["target_task_link_names"]) + points = np.zeros((21, 3)) + for indices, positions in zip(data["target_link_human_indices"], (origins, tasks)): + for index, position in zip(indices, positions): + points[index] = position + transform = np.asarray( + retargeter._config.handtracking_to_baselink_frame_transform + ).reshape(3, 3) + points = rotation.apply(points @ transform.T / data["scaling_factor"]) + translation + joints = [ + {"position": translation, "orientation": rotation.as_quat(), "radius": 0.008} + for _ in range(26) + ] + for index, point in zip(retargeter._hand_joints_index, points): + joints[index] = { + "position": point, + "orientation": rotation.as_quat(), + "radius": 0.008, + } + return {"joints": joints}, tasks - origins + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +@pytest.mark.parametrize("side", ("left", "right")) +def test_model_config_contains_only_robot_data(model, side): + config = _EXAMPLE.dex_hand_config(model, side) + data = yaml.safe_load(Path(config.hand_retargeting_config).read_text())[ + "retargeting" + ] + root = ET.parse(config.hand_urdf).getroot() + active = [ + j.get("name") + for j in root.findall("joint") + if j.get("type") == "revolute" and j.find("mimic") is None + ] + assert ( + set(active) == set(config.hand_joint_names) == set(data["target_joint_names"]) + ) + assert data["type"] == "vector" + assert data["scaling_factor"] == (0.9 if model == "l20" else 0.75) + assert data["normal_delta"] == (0.016 if model == "l20" else 0.001) + assert data["huber_delta"] == (0.005 if model == "l20" else 0.02) + assert data["low_pass_alpha"] == 0.4 + assert not {"project_dist", "escape_dist", "gamma"}.intersection(data) + if model == "l20": + assert f"{side}_hand_hand_base_link" not in data["target_origin_link_names"] + pairs = set( + zip(data["target_origin_link_names"], data["target_task_link_names"]) + ) + for finger in ("index", "middle", "ring", "pinky"): + assert ( + f"{side}_hand_{finger}_proximal", + f"{side}_hand_{finger}_middle", + ) in pairs + assert ( + f"{side}_hand_thumb_metacarpals", + f"{side}_hand_thumb_proximal", + ) in pairs + assert root.find(".//mesh") is None + assert all( + j.find("parent").get("link").startswith(f"{side}_hand_") + for j in root.findall("joint") + ) + assert len(root.findall("joint/mimic")) == (0 if model == "l6" else 5) + transform = np.asarray(config.handtracking_to_baselink_frame_transform).reshape( + 3, 3 + ) + np.testing.assert_allclose(transform.T @ transform, np.eye(3)) + assert np.linalg.det(transform) == pytest.approx(1.0) + np.testing.assert_allclose(np.asarray((0, 1, 0)) @ transform, (-1, 0, 0)) + np.testing.assert_allclose(np.asarray((0, 0, -1)) @ transform, (0, 0, 1)) + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +@pytest.mark.parametrize("side", ("left", "right")) +def test_official_solver_round_trip_and_world_pose_invariance(model, side, caplog): + config = _EXAMPLE.dex_hand_config(model, side) + data = yaml.safe_load(Path(config.hand_retargeting_config).read_text())[ + "retargeting" + ] + specs = get_realhand_profile(model).joints(side) + all_errors = [] + for rotated in (False, True): + retargeter = DexHandRetargeter( + _EXAMPLE.dex_hand_config(model, side), name=f"{model}_{side}_{rotated}" + ) + rotation = ( + Rotation.from_euler("xyz", (0.4, -0.7, 1.2)) + if rotated + else Rotation.identity() + ) + translation = np.asarray((0.6, -0.3, 1.1)) if rotated else np.zeros(3) + errors = [] + for fraction in (0.15, 0.35, 0.60, 0.80, 0.50, 0.20): + active = np.asarray( + [s.lower + fraction * (s.upper - s.lower) for s in specs] + ) + qpos = _full_qpos(retargeter, active) + sample, expected_vectors = _reachable_sample( + retargeter, data, qpos, rotation, translation + ) + result = _feed_samples(retargeter, side, [sample] * 15)[-1] + actual_qpos = _full_qpos(retargeter, result) + origins = _robot_positions( + retargeter, actual_qpos, data["target_origin_link_names"] + ) + tasks = _robot_positions( + retargeter, actual_qpos, data["target_task_link_names"] + ) + error = np.linalg.norm(tasks - origins - expected_vectors, axis=1) + # This checks reachable robot geometry, not measured glove accuracy. + assert np.max(error) < 0.005, (model, side, fraction, error) + assert np.mean(error) < 0.002 + errors.append(error) + all_errors.append(errors) + np.testing.assert_allclose(all_errors[0], all_errors[1], atol=0.001) + assert not [r for r in caplog.records if r.levelname == "ERROR"] + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +@pytest.mark.parametrize("side", ("left", "right")) +def test_official_filter_reduces_stationary_noise_without_changing_solver(model, side): + filtered = DexHandRetargeter( + _EXAMPLE.dex_hand_config(model, side), name=f"filtered_{model}_{side}" + ) + direct = DexHandRetargeter( + _EXAMPLE.dex_hand_config(model, side), name=f"direct_{model}_{side}" + ) + direct._dex_hand.filter.alpha = 1.0 + data = yaml.safe_load( + Path(_EXAMPLE.dex_hand_config(model, side).hand_retargeting_config).read_text() + )["retargeting"] + specs = get_realhand_profile(model).joints(side) + active = np.asarray([s.lower + 0.45 * (s.upper - s.lower) for s in specs]) + sample, _ = _reachable_sample( + filtered, data, _full_qpos(filtered, active), Rotation.identity(), np.zeros(3) + ) + rng = np.random.default_rng(73) + samples = [sample] * 35 + for _ in range(100): + noisy = deepcopy(sample) + for index, joint in enumerate(noisy["joints"]): + if index != HandJointIndex.WRIST: + joint["position"] = joint["position"] + rng.normal(0, 0.0005, 3) + samples.append(noisy) + raw = np.asarray(_feed_samples(direct, side, samples)) + smooth = np.asarray(_feed_samples(filtered, side, samples)) + expected = raw[0].copy() + for target, actual in zip(raw[1:], smooth[1:]): + expected += 0.4 * (target - expected) + np.testing.assert_allclose(actual, expected, atol=2e-5) + raw_step = np.sqrt(np.mean(np.diff(raw[55:], axis=0) ** 2)) + smooth_step = np.sqrt(np.mean(np.diff(smooth[55:], axis=0) ** 2)) + assert smooth_step < raw_step * 0.65 + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +@pytest.mark.parametrize("side", ("left", "right")) +def test_official_solver_step_response_and_static_hold(model, side): + retargeter = DexHandRetargeter( + _EXAMPLE.dex_hand_config(model, side), name=f"step_{model}_{side}" + ) + data = yaml.safe_load( + Path(_EXAMPLE.dex_hand_config(model, side).hand_retargeting_config).read_text() + )["retargeting"] + specs = get_realhand_profile(model).joints(side) + endpoints = [] + for fraction in (0.2, 0.7): + active = np.asarray([s.lower + fraction * (s.upper - s.lower) for s in specs]) + endpoints.append( + _reachable_sample( + retargeter, + data, + _full_qpos(retargeter, active), + Rotation.identity(), + np.zeros(3), + ) + ) + _feed_samples(retargeter, side, [endpoints[0][0]] * 35) + outputs = _feed_samples(retargeter, side, [endpoints[1][0]] * 35) + errors = [] + for output in outputs: + qpos = _full_qpos(retargeter, output) + vectors = _robot_positions( + retargeter, qpos, data["target_task_link_names"] + ) - _robot_positions(retargeter, qpos, data["target_origin_link_names"]) + errors.append(np.mean(np.linalg.norm(vectors - endpoints[1][1], axis=1))) + # Frame bounds, not a latency claim: the acquisition rate determines milliseconds. + assert errors[9] < 0.003 + assert errors[-1] < 0.002 + assert np.max(np.ptp(np.asarray(outputs[-10:]), axis=0)) < np.deg2rad(1) + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +@pytest.mark.parametrize("side", ("left", "right")) +def test_smooth_reachable_curl_sweep_respects_limits(model, side): + retargeter = DexHandRetargeter( + _EXAMPLE.dex_hand_config(model, side), name=f"sweep_{model}_{side}" + ) + data = yaml.safe_load( + Path(_EXAMPLE.dex_hand_config(model, side).hand_retargeting_config).read_text() + )["retargeting"] + specs = get_realhand_profile(model).joints(side) + fractions = np.concatenate( + (np.linspace(0.15, 0.85, 40), np.linspace(0.85, 0.15, 40)) + ) + samples = [] + for fraction in fractions: + active = np.asarray([s.lower + fraction * (s.upper - s.lower) for s in specs]) + sample, _ = _reachable_sample( + retargeter, + data, + _full_qpos(retargeter, active), + Rotation.identity(), + np.zeros(3), + ) + samples.append(sample) + _feed_samples(retargeter, side, [samples[0]] * 35) + outputs = np.asarray(_feed_samples(retargeter, side, samples)) + assert np.all(outputs >= np.asarray([s.lower for s in specs]) - 1.01e-3) + assert np.all(outputs <= np.asarray([s.upper for s in specs]) + 1.01e-3) + assert np.max(np.abs(np.diff(outputs, axis=0))) < np.deg2rad(15) + assert np.max(np.ptp(outputs, axis=0)) > 0.5 + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +@pytest.mark.parametrize("side", ("left", "right")) +def test_official_solver_invalid_wrist_returns_safe_zero(model, side, skeleton_samples): + 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.tensor_types import HandInputIndex + + retargeter = DexHandRetargeter( + _EXAMPLE.dex_hand_config(model, side), name=f"invalid_{model}_{side}" + ) + _feed_samples(retargeter, side, skeleton_samples[side]) + name, spec = next(iter(retargeter.input_spec().items())) + inputs = {name: OptionalTensorGroup(spec)} + outputs = { + key: TensorGroup(value) for key, value in retargeter.output_spec().items() + } + context = ComputeContext( + graph_time=GraphTime(sim_time_ns=0, real_time_ns=0), + execution_events=ExecutionEvents( + reset=False, execution_state=ExecutionState.RUNNING + ), + ) + retargeter.compute(inputs, outputs, context) + np.testing.assert_allclose([float(v) for v in outputs["hand_joints"]], 0) + joints = skeleton_samples[side][0]["joints"] + inputs[name][HandInputIndex.JOINT_POSITIONS] = np.asarray( + [j["position"] for j in joints], dtype=np.float32 + ) + inputs[name][HandInputIndex.JOINT_ORIENTATIONS] = np.asarray( + [j["orientation"] for j in joints], dtype=np.float32 + ) + valid = np.ones(26, dtype=np.uint8) + valid[HandJointIndex.WRIST] = 0 + inputs[name][HandInputIndex.JOINT_VALID] = valid + retargeter.compute(inputs, outputs, context) + np.testing.assert_allclose([float(v) for v in outputs["hand_joints"]], 0) + + +@pytest.mark.parametrize("side", ("left", "right")) +@pytest.mark.parametrize( + ("subdivisions", "noise"), ((1, 0), (2, 0), (4, 0), (2, 0.00015)) +) +def test_l20_slow_calibrated_thumb_sweeps_without_filter( + side, subdivisions, noise, calibrated_skeleton_samples +): + source = calibrated_skeleton_samples.get("sweeps", {}).get(side) + if source is None: + pytest.skip(f"No {side} human calibration supplied") + groups = {} + for sample in source: + name, _ = sample["name"].rsplit("_", 1) + groups.setdefault(name, []).append(sample) + specs = get_realhand_profile("l20").joints(side) + lower = np.asarray([s.lower for s in specs]) - 1.01e-3 + upper = np.asarray([s.upper for s in specs]) + 1.01e-3 + rng = np.random.default_rng(116) + for name, anchors in groups.items(): + retargeter = DexHandRetargeter( + _EXAMPLE.dex_hand_config("l20", side), name=f"slow_{side}_{name}" + ) + # Reachable robot targets and output smoothing can hide human-space branch changes. + retargeter._dex_hand.filter.alpha = 1.0 + samples = [] + for first, second in zip(anchors, anchors[1:]): + for alpha in np.linspace(0, 1, subdivisions, endpoint=False): + joints = [] + for index, (a, b) in enumerate(zip(first["joints"], second["joints"])): + position = (1 - alpha) * np.asarray( + a["position"] + ) + alpha * np.asarray(b["position"]) + if index != HandJointIndex.WRIST: + position += rng.normal(0, noise, 3) + joints.append({**a, "position": position, "radius": 0.008}) + samples.append({"joints": joints}) + final = deepcopy(anchors[-1]) + for joint in final["joints"]: + joint["radius"] = 0.008 + samples.append(final) + samples += samples[::-1] + _feed_samples(retargeter, side, [samples[0]] * 35) + outputs = np.asarray(_feed_samples(retargeter, side, samples)) + assert np.all(outputs >= lower) + assert np.all(outputs <= upper) + mask = np.asarray( + ["thumb" in joint for joint in retargeter._config.hand_joint_names] + ) + steps = np.abs(np.diff(outputs[:, mask], axis=0)) + assert np.max(steps) < np.deg2rad(8), ( + side, + name, + subdivisions, + np.rad2deg(np.max(steps)), + ) diff --git a/tests/python/examples/realhand/test_realhand_example.py b/tests/python/examples/realhand/test_realhand_example.py new file mode 100644 index 0000000000..8e8ee57c5b --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_example.py @@ -0,0 +1,163 @@ +# 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 +import sys +from copy import deepcopy +from contextlib import nullcontext +from repo_paths import repo_root + +import pytest + +_REPO_ROOT = repo_root() +_EXAMPLE_PATH = ( + _REPO_ROOT + / "examples/teleop/python/isaaccapture_examples/teleop/p7_realhand_bimanual_example.py" +) +_SPEC = importlib.util.spec_from_file_location( + "isaaccapture_examples.teleop.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): + pipeline, action_order = _EXAMPLE.build_pipeline(mode, model) + + 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_uses_the_same_hand_retargeter_as_optical_tracking(model, monkeypatch): + configs = [] + import isaaccapture.retargeters as official + + original = official.DexHandRetargeter + + def capture(config, **kwargs): + configs.append(deepcopy(config)) + return original(config, **kwargs) + + monkeypatch.setattr(official, "DexHandRetargeter", capture) + _EXAMPLE.build_pipeline("ffg", model) + glove_configs = list(configs) + configs.clear() + _EXAMPLE.build_pipeline("handtracking", model) + assert configs == glove_configs + assert [config.hand_side for config in configs] == ["left", "right"] + assert all(f"/{model}/" in config.hand_urdf for config in configs) + assert all(not hasattr(config, "calibration_path") for config in configs) + + +def test_combined_example_has_no_raw_sensor_source(): + assert not hasattr(_EXAMPLE, "JointStateSource") + assert not hasattr(_EXAMPLE, "RealHandFFGGloveRetargeter") + assert not hasattr(_EXAMPLE, "RealHandHandTrackingRetargeter") + + +def test_starting_ffg_plugin_requires_human_calibration(monkeypatch): + monkeypatch.setattr( + sys, + "argv", + [str(_EXAMPLE_PATH), "--mode", "ffg", "--plugin-path", "install/plugins"], + ) + with pytest.raises(SystemExit) as error: + _EXAMPLE.main() + assert error.value.code == 2 + + +def test_external_calibrated_glove_plugin_does_not_require_robot_calibration( + monkeypatch, +): + monkeypatch.setattr( + sys, + "argv", + [str(_EXAMPLE_PATH), "--mode", "ffg", "--hand-model", "l20", "--dry-run"], + ) + _EXAMPLE.main() + + +@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) + + +@pytest.mark.parametrize("calibration_example", (False, True)) +@pytest.mark.parametrize("ports", (None, "/dev/ttyUSB0,/dev/ttyUSB1")) +def test_example_forwards_verified_glove_ports( + calibration_example, ports, monkeypatch, human_calibration, tmp_path +): + module = _EXAMPLE + if calibration_example: + path = _EXAMPLE_PATH.with_name("realhand_ffg_glove_calibration.py") + spec = importlib.util.spec_from_file_location( + "isaaccapture_examples.teleop.realhand_ffg_glove_calibration", path + ) + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + arguments = ["--output", str(tmp_path / "new.yml")] + else: + arguments = [ + "--mode", + "ffg", + "--hand-model", + "l6", + "--realhand-ffg-glove-calibration", + str(human_calibration), + ] + arguments.extend(["--plugin-path", str(_REPO_ROOT / "install" / "plugins")]) + if ports: + arguments.append(f"--realhand-ffg-glove-ports={ports}") + monkeypatch.setattr(sys, "argv", [str(_EXAMPLE_PATH), *arguments]) + captured = [] + + class StopBeforeOpenXR(Exception): + pass + + def capture_config(config): + captured.append(config) + raise StopBeforeOpenXR + + monkeypatch.setattr(module, "TeleopSession", capture_config) + monkeypatch.setattr( + module.CloudXRLauncher, "launch_context", lambda args: nullcontext() + ) + with pytest.raises(StopBeforeOpenXR): + module.main() + assert len(captured) == 1 + expected = ( + ["--calibration="] + if calibration_example + else [ + "--wrist-source=controller", + f"--calibration={human_calibration.resolve()}", + ] + ) + if ports: + expected.append(f"--ports={ports}") + assert captured[0].plugins[0].plugin_args == expected diff --git a/tests/python/examples/realhand/test_realhand_ffg_diagnostics.py b/tests/python/examples/realhand/test_realhand_ffg_diagnostics.py new file mode 100644 index 0000000000..fe424ae292 --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_ffg_diagnostics.py @@ -0,0 +1,72 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Raw diagnostics retain all independent device inputs without selecting poses.""" + +import sys + +import numpy as np +import pytest +from isaaccapture_examples.teleop import realhand_ffg_diagnostics as _DIAGNOSTICS + + +def test_pipeline_records_hands_controllers_and_raw_sensors(): + spec = _DIAGNOSTICS.build_pipeline().output_types() + assert set(spec) == { + "hand_left", + "hand_right", + "controller_left", + "controller_right", + "raw_left", + "raw_right", + } + + +def test_periodic_phase_summary_does_not_invent_a_half_turn(): + values = np.tile(np.array((6.27, 0.01, 0.02)), 20) + samples = np.repeat(values[:, None], 21, axis=1) + summary = _DIAGNOSTICS.summarize_samples(samples) + np.testing.assert_allclose(summary["median_rad"], 2 * np.pi + 0.01) + assert max(summary["robust_sigma_rad"]) < 0.04 + + +@pytest.mark.parametrize( + "values", (np.zeros((29, 21)), np.zeros((40, 20)), np.full((40, 21), np.nan)) +) +def test_incomplete_or_invalid_raw_capture_is_rejected(values): + with pytest.raises(ValueError): + _DIAGNOSTICS.summarize_samples(values) + + +def test_repeated_open_pose_can_measure_baseline_drift(): + labels = [label for _, label in _DIAGNOSTICS.POSES] + assert labels[0] == "open" + assert labels[-1] == "open_end" + assert labels[1:5] == ["fist", "thumb_curl", "thumb_opposition", "spread"] + assert labels[5:9] == ["thumb_index", "thumb_middle", "thumb_ring", "thumb_little"] + + +def test_diagnostic_refuses_to_overwrite_existing_take(monkeypatch, tmp_path): + recording = tmp_path / "existing.mcap" + recording.write_bytes(b"preserve this") + calibration = tmp_path / "calibration.yml" + calibration.write_text("format: placeholder\n") + monkeypatch.setattr( + sys, + "argv", + [ + "diagnostic", + "--output", + str(recording), + "--calibration", + str(calibration), + "--plugin-path", + str(tmp_path), + "--ports", + "/dev/ttyUSB0,/dev/ttyUSB1", + ], + ) + with pytest.raises(SystemExit) as failed: + _DIAGNOSTICS.main() + assert failed.value.code == 2 + assert recording.read_bytes() == b"preserve this" diff --git a/tests/python/examples/realhand/test_realhand_ffg_glove_calibration.py b/tests/python/examples/realhand/test_realhand_ffg_glove_calibration.py new file mode 100644 index 0000000000..17786cc5a0 --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_ffg_glove_calibration.py @@ -0,0 +1,247 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Human calibration is robot-independent, side-specific, and wrap-safe.""" + +from contextlib import nullcontext +import sys +from types import SimpleNamespace +from pathlib import Path + +import numpy as np +import pytest +import yaml + +from isaaccapture_examples.teleop import realhand_ffg_glove_calibration as _CALIBRATION + +_PATH = Path(_CALIBRATION.__file__) + + +def test_five_general_poses_do_not_select_robot_or_contact_targets(): + keys = [key for _, key in _CALIBRATION._POSES] + assert keys == ["open", "fist", "thumb_curl", "thumb_opposition", "spread"] + assert not any( + "touch" in label or "pinch" in label for label, _ in _CALIBRATION._POSES + ) + + +def test_cli_captures_explicit_endpoints_repeated_contacts_and_open(): + keys = [key for _, key in _CALIBRATION._CAPTURE_POSES] + assert keys == [ + "open", + "fist", + "thumb_curl", + "thumb_opposition", + "spread", + "thumb_index", + "thumb_middle", + "thumb_ring", + "thumb_little", + "repeat_thumb_index", + "repeat_thumb_middle", + "repeat_thumb_ring", + "repeat_thumb_little", + "open_repeat", + ] + + +def test_capture_keeps_repeated_observations_and_unwraps_median(): + poses = {} + samples = np.tile([6.27, 0.01, 0.02], 20)[:, None] * np.ones((1, 21)) + for key in ("thumb_index", "repeat_thumb_index", "open_repeat", "spread"): + _CALIBRATION._store_capture(poses, key, samples) + assert poses["thumb_index"] == pytest.approx(np.full(21, 2 * np.pi + 0.01)) + observations = poses["thumb_observations"] + assert len(observations) == 48 + assert [row["pose"] for row in observations] == ["thumb_index"] * 32 + ["open"] * 16 + assert observations[1]["values"] == samples[3].tolist() + + +@pytest.mark.parametrize( + "samples", [np.zeros((29, 21)), np.zeros((30, 20)), np.full((30, 21), np.nan)] +) +def test_invalid_hold_does_not_mutate_captures(samples): + poses = {} + with pytest.raises(ValueError, match="30 finite"): + _CALIBRATION._store_capture(poses, "open", samples) + assert poses == {} + + +def test_disconnected_status_rejects_cached_raw_value(monkeypatch): + monkeypatch.setattr(_CALIBRATION, "_sample_group", lambda *_: np.ones(21)) + report = SimpleNamespace( + schema_version=1, + report_time_ns=_CALIBRATION.time.monotonic_ns(), + devices=[ + SimpleNamespace( + path="/glove/realhand_ffg_glove_left", + state=SimpleNamespace(name="DISCONNECTED"), + ) + ], + ) + tracker = SimpleNamespace(get_device_status_snapshot=lambda _: report) + session = SimpleNamespace(deviceio_session=None, step=lambda: None) + assert _CALIBRATION._current_sample(session, None, "left", tracker) is None + with pytest.raises(RuntimeError, match="glove data lost"): + _CALIBRATION._capture_pose( + session, ("left",), 30, 1, hold_seconds=0.1, status_tracker=tracker + ) + + +def test_fit_failure_keeps_file_and_other_side_unchanged( + human_calibration, monkeypatch +): + from isaaccapture_examples.teleop import realhand_ffg_thumb_calibration as fitter + + before = human_calibration.read_bytes() + captured = _CALIBRATION._load_output(human_calibration)["hands"] + + def reject(*_): + raise ValueError("bad fit") + + monkeypatch.setattr(fitter, "fit_thumb_mapping", reject) + with pytest.raises(ValueError, match="bad fit"): + _CALIBRATION._fit_and_save(human_calibration, {"left": captured["left"]}) + assert human_calibration.read_bytes() == before + assert "thumb_mapping" not in captured["left"] + + +def test_refit_replaces_stale_coefficients_only_on_selected_side( + human_calibration, monkeypatch +): + from isaaccapture_examples.teleop import realhand_ffg_thumb_calibration as fitter + + original = _CALIBRATION._load_output(human_calibration) + original["hands"]["left"]["thumb_mapping"] = {"stale": True} + human_calibration.write_text(yaml.safe_dump(original)) + captured = { + key: value + for key, value in original["hands"]["left"].items() + if key != "thumb_mapping" + } + mapping = {"format": fitter.FORMAT, "input_min": [-0.2] * 4, "input_max": [0.2] * 4} + monkeypatch.setattr(fitter, "fit_thumb_mapping", lambda poses, side: mapping) + _CALIBRATION._fit_and_save(human_calibration, {"left": captured}) + saved = _CALIBRATION._load_output(human_calibration) + assert saved["hands"]["left"]["thumb_mapping"] == mapping + assert saved["hands"]["right"] == original["hands"]["right"] + assert "thumb_mapping" not in captured + + +def test_failed_atomic_replace_preserves_existing_calibration( + human_calibration, monkeypatch +): + from isaaccapture_examples.teleop import realhand_ffg_thumb_calibration as fitter + + before = human_calibration.read_bytes() + captured = _CALIBRATION._load_output(human_calibration)["hands"] + mapping = {"format": fitter.FORMAT, "input_min": [-0.2] * 4, "input_max": [0.2] * 4} + monkeypatch.setattr(fitter, "fit_thumb_mapping", lambda *_: mapping) + + def fail(*_): + raise OSError("replace failed") + + monkeypatch.setattr(_CALIBRATION.os, "replace", fail) + with pytest.raises(OSError, match="replace failed"): + _CALIBRATION._fit_and_save(human_calibration, {"left": captured["left"]}) + assert human_calibration.read_bytes() == before + assert list(human_calibration.parent.iterdir()) == [human_calibration] + + +def test_failed_repeated_open_check_preserves_existing_calibration( + human_calibration, monkeypatch +): + from isaaccapture_examples.teleop import realhand_ffg_thumb_calibration as fitter + + before = human_calibration.read_bytes() + poses = _CALIBRATION._load_output(human_calibration)["hands"]["left"] + poses["open_repeat"] = poses["open"] + monkeypatch.setattr( + fitter, + "fit_thumb_mapping", + lambda *_: {"fit_report": {"repeat_open_training_tip_mm": 16.0}}, + ) + with pytest.raises(ValueError, match="repeated-open"): + _CALIBRATION._fit_and_save(human_calibration, {"left": poses}) + assert human_calibration.read_bytes() == before + + +def test_cli_runs_all_fourteen_holds_before_fitting(tmp_path, monkeypatch): + monkeypatch.setattr( + sys, "argv", [str(_PATH), "--output", str(tmp_path / "human.yml")] + ) + monkeypatch.setattr("builtins.input", lambda _: "") + monkeypatch.setattr( + _CALIBRATION.CloudXRLauncher, "launch_context", lambda _: nullcontext() + ) + monkeypatch.setattr(_CALIBRATION, "TeleopSession", lambda _: nullcontext(object())) + monkeypatch.setattr(_CALIBRATION, "_wait_for_sides", lambda *_: None) + preparations = [] + monkeypatch.setattr( + _CALIBRATION, "_prepare", lambda _, seconds: preparations.append(seconds) + ) + holds = [] + + def capture(session, sides, minimum, timeout, **kwargs): + holds.append(kwargs) + assert minimum == 120 + assert kwargs["hold_seconds"] == 6.0 + assert kwargs["status_tracker"] is not None + assert kwargs["return_samples"] + return {side: np.full((120, 21), len(holds) * 0.01) for side in sides} + + monkeypatch.setattr(_CALIBRATION, "_capture_pose", capture) + results = [] + monkeypatch.setattr( + _CALIBRATION, "_fit_and_save", lambda path, data: results.append(data) + ) + _CALIBRATION.main() + assert preparations == [8] * 14 + assert len(holds) == 14 + assert len(results) == 1 + for poses in results[0].values(): + assert set(poses) == {key for _, key in _CALIBRATION._CAPTURE_POSES} | { + "thumb_observations" + } + assert len(poses["thumb_observations"]) == 12 * 16 + + +def test_updating_one_side_preserves_other_measurements(human_calibration): + data = _CALIBRATION._load_output(human_calibration) + right = yaml.safe_dump(data["hands"]["right"]) + data["hands"]["left"]["open"][0] = 0.2 + assert yaml.safe_dump(data["hands"]["right"]) == right + _CALIBRATION._validate_calibration(data, ("left", "right")) + assert "hand_model" not in data + + +@pytest.mark.parametrize( + "key,sensor", (("fist", 6), ("thumb_curl", 2), ("thumb_opposition", 0)) +) +def test_rejects_insufficient_motion(human_calibration, key, sensor): + data = _CALIBRATION._load_output(human_calibration) + data["hands"]["left"][key][sensor] = data["hands"]["left"]["open"][sensor] + with pytest.raises(ValueError, match="insufficient calibration motion"): + _CALIBRATION._validate_calibration(data, ("left",)) + + +def test_legacy_calibration_is_not_silently_reused(tmp_path): + path = tmp_path / "old.yml" + path.write_text("format: realhand-ffg-glove-raw-calibration-v2\n", encoding="utf-8") + with pytest.raises(ValueError, match="not a human device calibration"): + _CALIBRATION._load_output(path) + + +def test_capture_handles_periodic_sensor_phase(monkeypatch): + readings = iter([np.full(21, 6.27), np.full(21, 0.01), np.full(21, 0.02)]) + monkeypatch.setattr( + _CALIBRATION, "_sample_group", lambda result, side: next(readings) + ) + monkeypatch.setattr(_CALIBRATION.time, "sleep", lambda seconds: None) + + class Session: + def step(self): + return None + + captured = _CALIBRATION._capture_pose(Session(), ("left",), 3, 1.0) + assert np.asarray(captured["left"]) == pytest.approx(np.full(21, 2 * np.pi + 0.01)) diff --git a/tests/python/examples/realhand/test_realhand_ffg_glove_hand_input.py b/tests/python/examples/realhand/test_realhand_ffg_glove_hand_input.py new file mode 100644 index 0000000000..a63b895d9f --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_ffg_glove_hand_input.py @@ -0,0 +1,111 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Feed production C++ glove geometry through the standard Python HandInput contract.""" + +import numpy as np +import pytest +from importlib.resources import files + +from isaaccapture.retargeters import DexHandRetargeter +from test_realhand_example import _EXAMPLE +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.tensor_types import HandInputIndex + + +def _feed_samples(retargeter, side, samples): + name, spec = next(iter(retargeter.input_spec().items())) + assert name == f"hand_{side}" + inputs = {name: OptionalTensorGroup(spec)} + outputs = { + key: TensorGroup(value) for key, value in retargeter.output_spec().items() + } + context = ComputeContext( + graph_time=GraphTime(sim_time_ns=0, real_time_ns=0), + execution_events=ExecutionEvents( + reset=False, execution_state=ExecutionState.RUNNING + ), + ) + results = [] + for sample in samples: + joints = sample["joints"] + group = inputs[name] + group[HandInputIndex.JOINT_POSITIONS] = np.asarray( + [j["position"] for j in joints], dtype=np.float32 + ) + group[HandInputIndex.JOINT_ORIENTATIONS] = np.asarray( + [j["orientation"] for j in joints], dtype=np.float32 + ) + group[HandInputIndex.JOINT_RADII] = np.asarray( + [j["radius"] for j in joints], dtype=np.float32 + ) + group[HandInputIndex.JOINT_VALID] = np.ones(26, dtype=np.uint8) + retargeter.compute(inputs, outputs, context) + result = np.asarray([float(value) for value in outputs["hand_joints"]]) + assert np.all(np.isfinite(result)) + results.append(result) + return results + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +@pytest.mark.parametrize("side", ("left", "right")) +def test_production_glove_skeleton_retargets_every_hand( + model, side, skeleton_samples, caplog +): + retargeter = DexHandRetargeter( + _EXAMPLE.dex_hand_config(model, side), + name=f"ffg_{model}_{side}", + ) + results = _feed_samples(retargeter, side, skeleton_samples[side]) + assert np.linalg.norm(results[-1] - results[0]) > 0.5 + assert np.linalg.norm(results[1] - results[0]) > 0.1 + for result in results: + specs = get_realhand_profile(model).joints(side) + # dex-retargeting expands optimizer limits by 1e-3 rad internally. + assert np.all(result >= np.asarray([s.lower for s in specs]) - 1.01e-3) + assert np.all(result <= np.asarray([s.upper for s in specs]) + 1.01e-3) + assert not [r for r in caplog.records if r.levelname == "ERROR"] + + +@pytest.mark.parametrize("side", ("left", "right")) +def test_production_glove_skeleton_retargets_wuji(side, skeleton_samples): + pytest.importorskip("wuji_sdk") + from isaaccapture.retargeters import WujiHandRetargeter, WujiHandRetargeterConfig + + retargeter = WujiHandRetargeter( + WujiHandRetargeterConfig(model="wuji_hand_2", hand_side=side), + name=f"ffg_wuji_{side}", + ) + results = _feed_samples(retargeter, side, skeleton_samples[side]) + assert results[0].shape == (20,) + assert np.linalg.norm(results[-1] - results[0]) > 0.1 + + +def test_production_glove_skeleton_retargets_sharpa(skeleton_samples): + pytest.importorskip("pink") + from isaaccapture.retargeters import ( + SharpaHandRetargeter, + SharpaHandRetargeterConfig, + ) + + asset = ( + files("robotic_grounding") + / "assets/xmls/sharpawave/right_sharpawave_nomesh.xml" + ) + retargeter = SharpaHandRetargeter( + SharpaHandRetargeterConfig( + robot_asset_path=str(asset), hand_side="right", max_iter=50, frequency=200.0 + ), + name="ffg_sharpa_right", + ) + results = _feed_samples(retargeter, "right", skeleton_samples["right"]) + assert results[0].shape == (22,) + assert np.linalg.norm(results[-1] - results[0]) > 0.1 diff --git a/tests/python/examples/realhand/test_realhand_ffg_hand_demo.py b/tests/python/examples/realhand/test_realhand_ffg_hand_demo.py new file mode 100644 index 0000000000..6345a8803a --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_ffg_hand_demo.py @@ -0,0 +1,749 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Test the PC calibration state machine without Isaac Sim or a connected glove.""" + +import importlib +import subprocess +import time +from copy import deepcopy +from types import SimpleNamespace + +import numpy as np +import pytest +import yaml + + +def load_example(name): + return importlib.import_module(f"isaaccapture_examples.teleop.{name}") + + +_BRIDGE = load_example("realhand_ffg_hand_demo_bridge") +_VIEWER = load_example("realhand_ffg_hand_demo") +_THUMB = load_example("realhand_ffg_thumb_calibration") +_CALIBRATION = load_example("realhand_ffg_glove_calibration") + + +@pytest.mark.parametrize("ports", (None, "/dev/ttyUSB0", "/dev/ttyUSB0,/dev/ttyUSB1")) +def test_demo_forwards_optional_ports_without_an_implicit_single_glove(tmp_path, ports): + interpreter = tmp_path / "python" + interpreter.touch() + argv = [ + "--capture-python", + str(interpreter), + "--capture-package", + str(tmp_path), + "--plugin-path", + str(tmp_path), + ] + if ports is not None: + argv.extend(("--ports", ports)) + args = _VIEWER.parse_args(argv) + assert args.ports == ports + worker = _VIEWER.worker_command(args, tmp_path) + native = _BRIDGE.plugin_arguments(args.ports, args.calibration) + assert "--wrist-source=auto" in native + assert "--wrist-source=controller" not in native + if ports is None: + assert "--ports" not in worker + assert not any(value.startswith("--ports=") for value in native) + else: + assert worker[worker.index("--ports") + 1] == ports + assert f"--ports={ports}" in native + + +def test_demo_worker_preserves_calibration_and_runtime_flags(tmp_path): + interpreter = tmp_path / "python" + interpreter.touch() + calibration = tmp_path / "human.yml" + calibration.touch() + args = _VIEWER.parse_args( + [ + "--capture-python", + str(interpreter), + "--capture-package", + str(tmp_path), + "--plugin-path", + str(tmp_path), + "--calibration", + str(calibration), + "--no-launch-cloudxr-runtime", + ] + ) + command = _VIEWER.worker_command(args, tmp_path) + assert command[1:3] == [ + "-m", + "isaaccapture_examples.teleop.realhand_ffg_hand_demo_bridge", + ] + assert command[command.index("--calibration") + 1] == str(calibration) + assert "--no-launch-cloudxr-runtime" in command + assert f"--calibration={calibration.resolve()}" in _BRIDGE.plugin_arguments( + None, calibration + ) + + +@pytest.fixture(scope="module") +def fitted_thumb(): + poses = calibration_pose_samples() + poses["thumb_mapping"] = _THUMB.fit_thumb_mapping(poses) + return poses + + +@pytest.fixture(scope="module") +def fitted_native_samples(request, tmp_path_factory, fitted_thumb): + executable = request.config.getoption("--ffg-skeleton-executable") + if executable is None: + pytest.skip("Pass --ffg-skeleton-executable for native thumb-map verification") + directory = tmp_path_factory.mktemp("fitted_thumb") + calibration = directory / "calibration.yml" + calibration.write_text( + yaml.safe_dump( + { + "format": "realhand-ffg-glove-human-calibration-v1", + "hands": dict.fromkeys(("left", "right"), fitted_thumb), + } + ), + encoding="utf-8", + ) + output = directory / "samples.yml" + subprocess.run( + [ + executable, + "--write-calibrated-skeleton-fixtures", + str(calibration), + str(output), + ], + check=True, + ) + return yaml.safe_load(output.read_text(encoding="utf-8")) + + +def test_thumb_contacts_improve_without_losing_open_and_curl(fitted_thumb): + mapping = fitted_thumb["thumb_mapping"] + assert mapping["base_position"] == pytest.approx(_THUMB.BASE) + assert mapping["fit_model"] == _THUMB.FIT_MODEL + assert mapping["fit_report"]["training_only"] + report = mapping["fit_report"] + assert max(report["contact_surface_error_mm"]) < 6 + assert ( + np.mean(report["contact_surface_error_mm"]) + < np.mean(report["contact_surface_error_before_mm"]) / 3 + ) + assert _THUMB.mapped_angles(mapping, fitted_thumb["open"]) == pytest.approx( + _THUMB.OPEN_ANGLES + ) + full = _THUMB.mapped_angles(mapping, fitted_thumb["thumb_curl"]) + assert full[2] > 0.9 + assert full[3] > 1.0 + opening = np.asarray(fitted_thumb["open"]) + slight_curl = opening + 0.1 * _THUMB.angle_delta( + fitted_thumb["thumb_curl"], opening + ) + assert ( + np.linalg.norm(_THUMB.mapped_angles(mapping, slight_curl) - _THUMB.OPEN_ANGLES) + < 0.4 + ) + + +def test_quadratic_sensor_jacobian_matches_finite_differences(): + values = np.random.default_rng(44).uniform(-1, 1, (9, 4)) + actual = _THUMB.quadratic_feature_jacobian(values) + expected = np.empty_like(actual) + for sensor in range(4): + step = np.eye(4)[sensor] * 1e-6 + expected[..., sensor] = ( + _THUMB.quadratic_features(values + step) + - _THUMB.quadratic_features(values - step) + ) / 2e-6 + np.testing.assert_allclose(actual, expected, atol=1e-9) + + +def test_contact_quality_is_not_traded_away_for_strong_smoothness_priors(monkeypatch): + poses = calibration_pose_samples() + monkeypatch.setattr(_THUMB, "SENSOR_GAIN_SCALE", 0.5) + mapping = _THUMB.fit_thumb_mapping(poses) + assert mapping["fit_report"]["solver"] == "noise_regularized_contact_surface" + assert max(mapping["fit_report"]["contact_surface_error_mm"]) < 6 + assert mapping["base_position"] == pytest.approx(_THUMB.BASE) + assert _THUMB.mapped_angles(mapping, poses["open"]) == pytest.approx( + _THUMB.OPEN_ANGLES + ) + + +def test_failed_constrained_fit_reports_side_without_accepting_it(monkeypatch): + poses = calibration_pose_samples() + monkeypatch.setattr(_THUMB, "SENSOR_GAIN_SCALE", 0.5) + invalid = np.r_[np.zeros(56), 1.0] + monkeypatch.setattr( + _THUMB, + "least_squares", + lambda *args, **kwargs: SimpleNamespace( + x=invalid, success=False, message="No fit" + ), + ) + monkeypatch.setattr( + _THUMB, + "minimize", + lambda objective, initial, **kwargs: SimpleNamespace( + x=initial, success=False, message="No feasible contact fit" + ), + ) + with pytest.raises(_THUMB.ThumbFitError, match=r"RIGHT.*quality checks") as caught: + _THUMB.fit_thumb_mapping(poses, "right") + assert caught.value.report["side"] == "right" + assert caught.value.report["solver_message"] == "No feasible contact fit" + + +def test_repeated_open_is_an_additional_training_observation(fitted_thumb): + poses = deepcopy(fitted_thumb) + repeated = np.asarray(poses["open"]).copy() + repeated[list(_THUMB.CHANNELS)] += (0.01, 0.025, 0.015, 0.02) + poses["open_repeat"] = repeated.tolist() + assert _THUMB.calibration_fingerprint(poses) != _THUMB.calibration_fingerprint( + fitted_thumb + ) + mapping = _THUMB.fit_thumb_mapping(poses) + assert mapping["fit_report"]["repeat_open_training_tip_mm"] < 15 + assert max(mapping["fit_report"]["contact_surface_error_mm"]) < 6 + assert mapping["fit_report"]["training_only"] + + +def test_malformed_repeated_open_is_rejected(fitted_thumb): + poses = dict(fitted_thumb, open_repeat=[0.0] * 20) + with pytest.raises(ValueError, match="21 finite"): + _THUMB.fit_thumb_mapping(poses) + + +def test_vectorized_thumb_tip_matches_joint_forward_kinematics(): + angles = np.random.default_rng(29).uniform(_THUMB.LOWER, _THUMB.UPPER, (100, 4)) + actual = _THUMB.thumb_tip_positions(angles) + expected = np.asarray([_THUMB.thumb_joints(a)[-1] for a in angles]) + np.testing.assert_allclose(actual, expected, atol=1e-12) + + +def test_sensor_gain_constraint_invalidates_cached_coefficients( + monkeypatch, fitted_thumb +): + before = _THUMB.calibration_fingerprint(fitted_thumb) + monkeypatch.setattr(_THUMB, "SENSOR_GAIN_SCALE", _THUMB.SENSOR_GAIN_SCALE * 2) + assert _THUMB.calibration_fingerprint(fitted_thumb) != before + + +def test_indistinguishable_thumb_contacts_are_rejected(): + poses = calibration_pose_samples() + for key in _THUMB.POSES[4:]: + poses[key][:5] = poses["thumb_index"][:5] + with pytest.raises(ValueError, match="indistinguishable"): + _THUMB.fit_thumb_mapping(poses) + + +def test_invalid_saved_calibration_is_not_rewritten(human_calibration): + calibration = yaml.safe_load(human_calibration.read_text()) + calibration["hands"]["left"]["fist"] = [0.0] * 21 + human_calibration.write_text(yaml.safe_dump(calibration), encoding="utf-8") + before = human_calibration.read_bytes() + with pytest.raises(ValueError, match="insufficient"): + _BRIDGE.validate_saved_calibration(human_calibration, _CALIBRATION) + assert human_calibration.read_bytes() == before + + +@pytest.mark.parametrize("boundary,direction", (("input_min", -1), ("input_max", 1))) +def test_thumb_saturates_beyond_captured_ranges(fitted_thumb, boundary, direction): + mapping = fitted_thumb["thumb_mapping"] + endpoint = np.asarray(fitted_thumb["open"]).copy() + endpoint[list(_THUMB.CHANNELS)] = ( + np.asarray(mapping["input_origin"]) + mapping[boundary] + ) + overshoot = endpoint.copy() + overshoot[list(_THUMB.CHANNELS)] += direction * 0.15 + assert _THUMB.mapped_angles(mapping, overshoot) == pytest.approx( + _THUMB.mapped_angles(mapping, endpoint) + ) + + +def test_preview_preserves_saved_calibration_without_fitting( + human_calibration, fitted_thumb, monkeypatch +): + calibration = yaml.safe_load(human_calibration.read_text()) + calibration["hands"]["left"] = fitted_thumb + human_calibration.write_text(yaml.safe_dump(calibration), encoding="utf-8") + before = human_calibration.read_bytes() + + def unexpected_fit(*_): + pytest.fail("Loading a calibration must not refit it") + + monkeypatch.setattr(_THUMB, "fit_thumb_mapping", unexpected_fit) + for _ in range(2): + assert ( + _BRIDGE.validate_saved_calibration(human_calibration, _CALIBRATION) + == calibration + ) + assert human_calibration.read_bytes() == before + assert list(human_calibration.parent.iterdir()) == [human_calibration] + + +@pytest.mark.parametrize("side, mirror", (("left", 1), ("right", -1))) +def test_native_thumb_matches_fitter_for_every_pose_and_trajectory( + fitted_thumb, fitted_native_samples, side, mirror +): + mapping = fitted_thumb["thumb_mapping"] + for sample in fitted_native_samples["sweeps"][side]: + expected = _THUMB.thumb_joints( + _THUMB.mapped_angles(mapping, sample["raw"]), + mapping["base_position"], + mapping["lengths"], + ) + expected[:, 0] *= mirror + points = np.asarray([joint["position"] for joint in sample["joints"]]) + assert points[2:6] == pytest.approx(expected, abs=2e-6) + quaternions = np.asarray([joint["orientation"] for joint in sample["joints"]]) + assert np.linalg.norm(quaternions, axis=1) == pytest.approx( + np.ones(26), abs=2e-6 + ) + samples = fitted_native_samples["sweeps"][side] + for pose in range(7): + tips = np.asarray( + [ + sample["joints"][5]["position"] + for sample in samples[pose * 101 : (pose + 1) * 101] + ] + ) + assert np.max(np.linalg.norm(np.diff(tips, axis=0), axis=1)) < 0.003 + + +def test_multivariate_mapping_accepts_a_small_legacy_opposition_span(fitted_thumb): + poses = deepcopy(fitted_thumb) + poses["thumb_opposition"][0] = poses["open"][0] + 0.049 + example = _BRIDGE.load_calibration_example() + example._validate_calibration({"hands": {"left": poses}}, ("left",)) + del poses["thumb_mapping"] + with pytest.raises(ValueError, match="insufficient calibration motion"): + example._validate_calibration({"hands": {"left": poses}}, ("left",)) + + +def test_multivariate_mapping_still_rejects_motionless_inputs(fitted_thumb): + poses = deepcopy(fitted_thumb) + poses["thumb_mapping"]["input_min"] = [0.0] * 4 + poses["thumb_mapping"]["input_max"] = [0.0] * 4 + with pytest.raises(ValueError, match="invalid thumb mapping input ranges"): + _BRIDGE.load_calibration_example()._validate_calibration( + {"hands": {"left": poses}}, ("left",) + ) + + +@pytest.mark.parametrize("side", ("left", "right")) +def test_native_other_fingers_cannot_move_thumb_and_phase_wrap_is_safe( + fitted_native_samples, side +): + poses = fitted_native_samples["poses"][side] + for group in ("independence", "phase_wrap"): + for index, sample in enumerate(fitted_native_samples[group][side]): + expected = np.asarray( + [joint["position"] for joint in poses[index // 2]["joints"]] + ) + points = np.asarray([joint["position"] for joint in sample["joints"]]) + assert points[2:6] == pytest.approx(expected[2:6], abs=2e-6) + + +def test_native_right_sway_and_offline_finger_geometry_use_the_same_sensor_convention( + fitted_thumb, fitted_native_samples +): + for finger in range(4): + for direction in range(2): + index = 2 * finger + direction + left = fitted_native_samples["lateral"]["left"][index] + right = fitted_native_samples["lateral"]["right"][index] + left_points = np.asarray([joint["position"] for joint in left["joints"]]) + right_points = np.asarray([joint["position"] for joint in right["joints"]]) + assert right_points[:, 0] == pytest.approx(-left_points[:, 0], abs=2e-6) + assert right_points[:, 1:] == pytest.approx(left_points[:, 1:], abs=2e-6) + for side, sample, points in ( + ("left", left, left_points), + ("right", right, right_points), + ): + expected = _THUMB.finger_tip(fitted_thumb, sample["raw"], finger, side) + expected[0] *= 1 if side == "left" else -1 + assert points[10 + 5 * finger] == pytest.approx(expected, abs=2e-6) + + +def test_explicit_right_calibration_preserves_left(human_calibration, fitted_thumb): + calibration = _CALIBRATION._load_output(human_calibration) + before = deepcopy(calibration["hands"]["left"]) + saved = _CALIBRATION._fit_and_save(human_calibration, {"right": fitted_thumb}) + assert saved["hands"]["left"] == before + mapping = saved["hands"]["right"]["thumb_mapping"] + assert mapping["calibration_sha256"] == _THUMB.calibration_fingerprint( + fitted_thumb, "right" + ) + assert ( + mapping["calibration_sha256"] + != fitted_thumb["thumb_mapping"]["calibration_sha256"] + ) + + +@pytest.mark.parametrize( + "field, bad", + ( + ("input_scale", [0] * 4), + ("coefficients", [[0] * 15] * 3), + ("base_position", [1] * 3), + ("format", "unknown"), + ), +) +def test_native_rejects_malformed_thumb_mapping( + request, tmp_path, fitted_thumb, field, bad +): + executable = request.config.getoption("--ffg-skeleton-executable") + if executable is None: + pytest.skip("Pass --ffg-skeleton-executable") + poses = dict( + fitted_thumb, thumb_mapping=dict(fitted_thumb["thumb_mapping"], **{field: bad}) + ) + path = tmp_path / "invalid.yml" + path.write_text( + yaml.safe_dump( + { + "format": "realhand-ffg-glove-human-calibration-v1", + "hands": {"left": poses}, + } + ), + encoding="utf-8", + ) + result = subprocess.run( + [ + executable, + "--write-calibrated-skeleton-fixtures", + str(path), + str(tmp_path / "out.yml"), + ], + capture_output=True, + check=False, + ) + assert result.returncode != 0 + + +def calibration_pose_samples(): + poses = {key: np.zeros(21) for key in _THUMB.POSES} + for finger in range(4): + poses["fist"][6 + 4 * finger] = -1.2 + poses["fist"][8 + 4 * finger] = 0.9 + poses["fist"][[0, 1, 2, 4]] = (-0.04, -0.3, -0.5, 0.1) + poses["thumb_curl"][[0, 1, 2, 4]] = (-0.1, -0.5, -1.6, 0.6) + thumb_values = ( + (-0.12, -0.85, -0.9, 0.4), + (-0.18, -0.98, -0.9, 0.4), + (-0.22, -0.97, -1, 0.4), + (-0.25, -0.99, -1.3, 0.46), + ) + for finger, key in enumerate(_THUMB.POSES[3:]): + poses[key][list(_THUMB.CHANNELS)] = thumb_values[finger] + poses[key][6 + 4 * finger] = -0.8 + poses[key][8 + 4 * finger] = 0.6 + poses["thumb_opposition"] = np.zeros(21) + poses["thumb_opposition"][[0, 1]] = (-0.25, -0.99) + poses["spread"] = np.zeros(21) + poses["spread"][[5, 9, 13, 17]] = 0.3 + for finger in _CALIBRATION._CONTACTS: + poses[f"repeat_thumb_{finger}"] = poses[f"thumb_{finger}"].copy() + poses["open_repeat"] = poses["open"].copy() + return {key: value.tolist() for key, value in poses.items()} + + +def capture_step(capture, values, connected, now): + capture.command({"action": "capture"}, connected, now) + assert capture.state == "preparing" + assert not capture.update(values, now + capture.prepare_seconds - 0.1) + assert not any(capture.samples.values()) + return [ + capture.update(values, now + capture.prepare_seconds + elapsed) + for elapsed in np.linspace(0, capture.hold_seconds, capture.sample_count + 1) + ] + + +def capture_all(capture, *, connected=None, identical=False, sides=("left",)): + connected = connected or {"left": True, "right": True} + capture.command({"action": "begin", "sides": list(sides)}, connected, 0.0) + reloads = [] + poses = calibration_pose_samples() + for step, (_, key) in enumerate(capture.poses): + now = step * (capture.prepare_seconds + capture.hold_seconds + 1) + value = np.zeros(21) if identical else poses[key] + reloads.extend( + capture_step(capture, dict.fromkeys(sides, value), connected, now) + ) + return reloads + + +def test_ui_uses_full_shared_calibration_and_preserves_other_side(human_calibration): + before = yaml.safe_load(human_calibration.read_text())["hands"]["right"] + capture = _BRIDGE.CalibrationCapture( + human_calibration, _BRIDGE.load_calibration_example() + ) + reloads = capture_all(capture) + assert sum(reloads) == 1 + assert capture.state == "complete" + data = yaml.safe_load(human_calibration.read_text()) + assert data["hands"]["right"] == before + assert set(data["hands"]["left"]) == {key for _, key in capture.poses} | { + "thumb_mapping", + "thumb_observations", + } + assert capture.status(100)["total"] == 14 + assert len(data["hands"]["left"]["thumb_observations"]) == 12 * 16 + assert data["hands"]["left"]["spread"] == calibration_pose_samples()["spread"] + + +def test_invalid_captures_do_not_overwrite_working_calibration(human_calibration): + before = human_calibration.read_bytes() + capture = _BRIDGE.CalibrationCapture( + human_calibration, _BRIDGE.load_calibration_example() + ) + assert not any(capture_all(capture, identical=True)) + assert capture.state == "error" + assert capture.message + assert human_calibration.read_bytes() == before + + +def test_disconnection_and_cancel_keep_existing_calibration(human_calibration): + before = human_calibration.read_bytes() + capture = _BRIDGE.CalibrationCapture( + human_calibration, _BRIDGE.load_calibration_example() + ) + connected = {"left": True, "right": False} + capture.command({"action": "begin", "sides": ["right"]}, connected, 0.0) + assert capture.state == "idle" + capture.command({"action": "begin", "sides": ["left"]}, connected, 0.0) + capture.command({"action": "capture"}, connected, 1.0) + capture.update({"left": None}, 1.0 + capture.prepare_seconds) + assert capture.state == "error" + capture.command({"action": "cancel"}, connected, 3.0) + assert capture.state == "idle" + assert human_calibration.read_bytes() == before + + +def test_cached_raw_values_are_not_used_after_device_disconnect(): + report = SimpleNamespace( + schema_version=1, + report_time_ns=time.monotonic_ns(), + devices=[ + SimpleNamespace( + path="/glove/realhand_ffg_glove_left", + state=SimpleNamespace(name="DISCONNECTED"), + error="USB disconnected", + ), + SimpleNamespace( + path="/glove/realhand_ffg_glove_right", + state=SimpleNamespace(name="CONNECTED"), + error="", + ), + ], + ) + tracker = SimpleNamespace(get_device_status_snapshot=lambda session: report) + session = SimpleNamespace(deviceio_session=object()) + example = SimpleNamespace(_sample_group=lambda result, name: np.ones(21)) + raw, errors = _BRIDGE.sample_glove_inputs(session, {}, example, tracker) + assert raw["left"] is None + assert raw["right"].shape == (21,) + assert errors["left"] == "USB disconnected" + report.report_time_ns -= 3_000_000_000 + raw, _ = _BRIDGE.sample_glove_inputs(session, {}, example, tracker) + assert raw == {"left": None, "right": None} + + +@pytest.mark.parametrize("side", ("left", "right")) +@pytest.mark.parametrize( + "reason", + ( + "missing human calibration for this hand", + "controller wrist unavailable or discontinuous; awaiting stable tracking", + ), +) +def test_connected_glove_reports_unavailable_hand_stream(side, reason): + report = SimpleNamespace( + schema_version=1, + report_time_ns=time.monotonic_ns(), + devices=[ + SimpleNamespace( + path=f"/glove/realhand_ffg_glove_{side}", + state=SimpleNamespace(name="CONNECTED"), + error="", + ), + SimpleNamespace( + path=f"/hand/{side}", + state=SimpleNamespace(name="DEGRADED"), + error=reason, + ), + ], + ) + tracker = SimpleNamespace(get_device_status_snapshot=lambda session: report) + session = SimpleNamespace(deviceio_session=object()) + example = SimpleNamespace(_sample_group=lambda result, name: np.ones(21)) + raw, errors = _BRIDGE.sample_glove_inputs(session, {}, example, tracker) + assert raw[side].shape == (21,) + assert errors[side] == reason + report.devices[1].state.name = "CONNECTED" + report.devices[1].error = "" + _, errors = _BRIDGE.sample_glove_inputs(session, {}, example, tracker) + assert errors[side] == "" + + +@pytest.mark.parametrize("side, wrist_x", (("left", -0.15), ("right", 0.15))) +def test_viewer_is_a_rigid_axis_change_not_a_gesture_retarget( + skeleton_samples, side, wrist_x +): + points = np.asarray( + [joint["position"] for joint in skeleton_samples[side][1]["joints"]] + ) + before = points.copy() + displayed = _VIEWER.display_positions(points, side) + for first, second in _VIEWER._EDGES: + assert np.linalg.norm(displayed[first] - displayed[second]) == pytest.approx( + np.linalg.norm(points[first] - points[second]) + ) + assert displayed[1] == pytest.approx((wrist_x, -0.08, 0.20)) + assert points == pytest.approx(before) + + +@pytest.mark.parametrize("side", ("left", "right")) +def test_viewer_faces_palms_up_without_mirroring_or_reversing_fingers(side): + points = np.zeros((26, 3)) + points[6] = (0, 0.03, 0) + points[7] = (0.02, 0, 0) + points[8] = (0, 0, -0.04) + displayed = _VIEWER.display_positions(points, side) + local = displayed - displayed[1] + assert local[6] == pytest.approx((0, 0, -0.03)) + assert local[8] == pytest.approx((0, 0.04, 0)) + assert np.linalg.det(local[[7, 6, 8]]) == pytest.approx( + np.linalg.det(points[[7, 6, 8]]) + ) + + +@pytest.mark.parametrize("bad", (np.zeros((25, 3)), np.full((26, 3), np.nan))) +def test_viewer_rejects_invalid_geometry(bad): + with pytest.raises(ValueError, match="26 finite"): + _VIEWER.display_positions(bad, "left") + + +@pytest.mark.parametrize("side, mirror", (("left", 1), ("right", -1))) +@pytest.mark.parametrize("finger", range(4)) +def test_palm_up_view_preserves_sideways_motion_relative_to_anatomy( + skeleton_samples, side, mirror, finger +): + points = np.asarray( + [joint["position"] for joint in skeleton_samples[side][0]["joints"]] + ) + before = _VIEWER.display_positions(points, side) + first = 6 + 5 * finger + for direction in (-1, 1): + moved = points.copy() + moved[first : first + 5, 0] -= mirror * direction * 0.01 + delta = _VIEWER.display_positions(moved, side) - before + assert delta[first : first + 5, 0] == pytest.approx( + [mirror * direction * 0.01] * 5 + ) + assert delta[:, 1:] == pytest.approx(np.zeros((26, 2))) + assert delta[:first] == pytest.approx(np.zeros((first, 3))) + assert delta[first + 5 :] == pytest.approx(np.zeros((26 - first - 5, 3))) + + +def test_pc_and_cli_share_pose_order_timing_and_sampling(tmp_path): + capture = _BRIDGE.CalibrationCapture(tmp_path / "human.yml", _CALIBRATION) + assert capture.poses is _CALIBRATION._CAPTURE_POSES + assert capture.prepare_seconds == _CALIBRATION._DEFAULT_PREPARE_SECONDS == 8 + assert capture.hold_seconds == _CALIBRATION._DEFAULT_HOLD_SECONDS == 6 + assert capture.sample_count == _CALIBRATION._DEFAULT_SAMPLES == 120 + + +def test_both_hands_complete_in_one_workflow(human_calibration): + capture = _BRIDGE.CalibrationCapture( + human_calibration, _BRIDGE.load_calibration_example() + ) + assert sum(capture_all(capture, sides=("left", "right"))) == 1 + data = yaml.safe_load(human_calibration.read_text()) + for side in ("left", "right"): + assert all(key in data["hands"][side] for _, key in capture.poses) + + +@pytest.mark.parametrize("bad", (np.zeros(20), np.full(21, np.nan))) +def test_invalid_live_samples_do_not_replace_calibration(human_calibration, bad): + before = human_calibration.read_bytes() + capture = _BRIDGE.CalibrationCapture( + human_calibration, _BRIDGE.load_calibration_example() + ) + capture.command({"action": "begin", "sides": ["left"]}, {"left": True}, 0) + capture.command({"action": "capture"}, {"left": True}, 0) + assert not capture.update({"left": bad}, capture.prepare_seconds) + assert capture.state == "error" + assert human_calibration.read_bytes() == before + + +def test_cancel_during_contacts_does_not_save_partial_calibration(human_calibration): + before = human_calibration.read_bytes() + capture = _BRIDGE.CalibrationCapture( + human_calibration, _BRIDGE.load_calibration_example() + ) + capture.command({"action": "begin", "sides": ["left"]}, {"left": True}, 0) + for step in range(6): + now = step * 20 + capture_step(capture, {"left": np.full(21, float(step))}, {"left": True}, now) + assert capture.status(120)["instruction"].startswith("touch") + capture.command({"action": "cancel"}, {}, 120) + assert human_calibration.read_bytes() == before + assert not human_calibration.with_suffix(".validation.yml").exists() + + +def test_pc_uses_cli_sample_reduction_and_save(tmp_path, monkeypatch): + captured = [] + original = _CALIBRATION._store_capture + + def store(poses, key, samples): + captured.append(key) + original(poses, key, samples) + + saved = [] + monkeypatch.setattr(_CALIBRATION, "_store_capture", store) + monkeypatch.setattr( + _CALIBRATION, "_fit_and_save", lambda path, poses: saved.append(deepcopy(poses)) + ) + capture = _BRIDGE.CalibrationCapture(tmp_path / "human.yml", _CALIBRATION) + assert sum(capture_all(capture)) == 1 + assert captured == [key for _, key in _CALIBRATION._CAPTURE_POSES] + assert len(saved) == 1 + expected = {} + values = calibration_pose_samples() + for _, key in _CALIBRATION._CAPTURE_POSES: + original(expected, key, np.tile(values[key], (121, 1))) + assert saved[0]["left"] == expected + + +def test_preparation_can_be_cancelled_without_sampling_or_saving(human_calibration): + before = human_calibration.read_bytes() + capture = _BRIDGE.CalibrationCapture(human_calibration, _CALIBRATION) + capture.command({"action": "begin", "sides": ["left"]}, {"left": True}, 0) + capture.command({"action": "capture"}, {"left": True}, 0) + assert capture.status(1)["state"] == "preparing" + assert capture.status(1)["remaining"] == 7 + capture.command({"action": "cancel"}, {}, 2) + assert not capture.update({"left": np.ones(21)}, 20) + assert capture.state == "idle" + assert human_calibration.read_bytes() == before + + +@pytest.mark.parametrize("side", ("left", "right")) +def test_solid_bones_connect_every_joint_for_every_pose(skeleton_samples, side): + for pose in skeleton_samples[side]: + points = _VIEWER.display_positions( + [joint["position"] for joint in pose["joints"]], side + ) + centers, vectors, lengths = _VIEWER.bone_segments(points) + assert len(lengths) == 25 + assert np.all(lengths > 1e-6) + for index, (first, second) in enumerate(_VIEWER._EDGES): + assert centers[index] - vectors[index] * 0.5 == pytest.approx(points[first]) + assert centers[index] + vectors[index] * 0.5 == pytest.approx( + points[second] + ) + assert lengths[index] == pytest.approx( + np.linalg.norm(points[first] - points[second]) + ) diff --git a/tests/python/examples/realhand/test_realhand_ffg_robot_scene.py b/tests/python/examples/realhand/test_realhand_ffg_robot_scene.py new file mode 100644 index 0000000000..90748a4242 --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_ffg_robot_scene.py @@ -0,0 +1,180 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Check all three robot scenes without requiring Isaac Sim in the test environment.""" + +import json +from copy import deepcopy + +import numpy as np +import pytest + +from test_realhand_ffg_hand_demo import _BRIDGE, _VIEWER + + +@pytest.fixture +def scene(): + from isaaccapture_examples.teleop import realhand_ffg_robot_scene as module + + return module + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +def test_robot_live_worker_preserves_model_and_human_calibration(model, tmp_path): + interpreter = tmp_path / "python" + interpreter.touch() + args = _VIEWER.parse_args( + [ + "--hand-model", + model, + "--capture-python", + str(interpreter), + "--capture-package", + str(tmp_path), + "--plugin-path", + str(tmp_path), + "--calibration", + str(tmp_path / "human.yml"), + ] + ) + command = _VIEWER.worker_command(args, tmp_path) + assert command[command.index("--hand-model") + 1] == model + assert command[command.index("--calibration") + 1] == str(tmp_path / "human.yml") + assert "--ports" not in command + pipeline = _BRIDGE.build_pipeline(model) + assert set(pipeline.output_types()) == { + "left", + "right", + "raw_left", + "raw_right", + "robot_left", + "robot_right", + } + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +def test_existing_human_calibration_drives_every_model_without_refit( + model, request, calibrated_skeleton_samples, skeleton_samples, monkeypatch +): + from pathlib import Path + from isaaccapture_examples.teleop import ( + realhand_ffg_glove_calibration as calibration, + ) + from isaaccapture_examples.teleop import realhand_ffg_thumb_calibration as fitter + from test_realhand_ffg_glove_hand_input import _feed_samples + + path = Path(request.config.getoption("--ffg-human-calibration")) + before = path.read_bytes() + + def unexpected_fit(*_): + pytest.fail("Changing robot models must not refit the human calibration") + + monkeypatch.setattr(fitter, "fit_thumb_mapping", unexpected_fit) + _BRIDGE.validate_saved_calibration(path, calibration) + retargeters = {} + _BRIDGE.build_pipeline(model, retargeters) + for side, retargeter in retargeters.items(): + samples = deepcopy(calibrated_skeleton_samples["poses"][side]) + # Calibrated fixtures omit radii; use the same native model's neutral radii. + for sample in samples: + for joint, neutral in zip( + sample["joints"], skeleton_samples[side][0]["joints"] + ): + joint["radius"] = neutral["radius"] + results = _feed_samples(retargeter, side, samples) + assert np.linalg.norm(results[1] - results[0]) > 0.1 + assert path.read_bytes() == before + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +def test_robot_fixture_replay_is_offline_and_matches_named_output( + model, tmp_path, skeleton_samples, caplog +): + interpreter = tmp_path / "python" + interpreter.touch() + args = _VIEWER.parse_args( + [ + "--hand-model", + model, + "--test-frames", + str(tmp_path / "fixture.yml"), + "--capture-python", + str(interpreter), + "--capture-package", + str(tmp_path), + ] + ) + command = _VIEWER.worker_command(args, tmp_path) + assert "--plugin-path" not in command + assert "--ipc-dir" not in command + packets = _BRIDGE.replay_robot_frames(model, skeleton_samples) + assert len(packets) == 60 + for packet in packets: + assert len(json.dumps(packet).encode()) < 65536 + for side in ("left", "right"): + robot = packet["robots"][side] + assert len(robot["positions"]) == {"l6": 11, "o6": 6, "l20": 16}[model] + assert all( + name.startswith(f"{side}_hand_") for name in robot["joint_names"] + ) + assert f"{side}_hand_hand_base_link" in robot["link_poses"] + for matrix in robot["link_poses"].values(): + matrix = np.asarray(matrix) + assert matrix.shape == (4, 4) + np.testing.assert_allclose(matrix[3], (0, 0, 0, 1), atol=1e-6) + np.testing.assert_allclose( + matrix[:3, :3].T @ matrix[:3, :3], np.eye(3), atol=1e-6 + ) + for side in ("left", "right"): + first = packets[0]["robots"][side]["positions"] + last = packets[40]["robots"][side]["positions"] + assert np.linalg.norm(np.asarray(first) - last) > 0.5 + assert not [r for r in caplog.records if r.levelname == "ERROR"] + + +@pytest.mark.parametrize("model", ("l6", "o6", "l20")) +def test_disconnected_glove_has_neutral_robot_output(model): + retargeters = {} + _BRIDGE.build_pipeline(model, retargeters) + for retargeter in retargeters.values(): + frame = _BRIDGE.robot_frame(retargeter, None) + np.testing.assert_allclose(frame["positions"], 0.0) + assert frame["link_poses"] + + +def test_robot_display_is_palm_up_without_reflection(scene): + for side in ("left", "right"): + matrix = scene.robot_display_transform(side) + assert np.linalg.det(matrix[:3, :3]) == pytest.approx(1.0) + np.testing.assert_allclose(matrix[:3, :3] @ (1, 0, 0), (0, 0, 1)) + np.testing.assert_allclose(matrix[:3, :3] @ (0, 0, 1), (0, 1, 0)) + assert np.sign(matrix[0, 3]) == (-1 if side == "left" else 1) + + +def test_robot_visual_loading_uses_mesh_scale_origin_and_only_selected_hand( + scene, monkeypatch, tmp_path +): + urdf = tmp_path / "assembly.urdf" + urdf.write_text( + """ + + + + + + + + + + +""", + encoding="utf-8", + ) + monkeypatch.setattr(scene, "_validate_model", lambda root, model: urdf) + visuals = scene.visual_links(tmp_path, "l6", "left") + assert list(visuals) == ["left_hand_hand_base_link"] + filename, matrix, color = visuals["left_hand_hand_base_link"][0] + assert filename == tmp_path / "left.stl" + np.testing.assert_allclose(matrix[:3, 3], (0.1, 0.2, 0.3)) + np.testing.assert_allclose(matrix[:3, :3], np.diag((2, 3, 4))) + assert color == pytest.approx((0.1, 0.2, 0.3, 1)) diff --git a/tests/python/examples/realhand/test_realhand_module_entrypoints.py b/tests/python/examples/realhand/test_realhand_module_entrypoints.py new file mode 100644 index 0000000000..ba645fd84a --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_module_entrypoints.py @@ -0,0 +1,67 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Examples must run as package modules, including their relative imports.""" + +import os +import subprocess +import sys +import tomllib +from pathlib import Path + +import pytest +from packaging.requirements import Requirement +from repo_paths import repo_root + + +def test_retargeter_dependency_declarations_match(): + root = repo_root() + requirements = root / "src/core/python/requirements-retargeters.txt" + runtime = { + Requirement(line) + for line in requirements.read_text(encoding="utf-8").splitlines() + if line.strip() and not line.lstrip().startswith("#") + } + source = tomllib.loads((root / "pyproject.toml").read_text(encoding="utf-8")) + examples = tomllib.loads( + Path(__file__).with_name("pyproject.toml").read_text(encoding="utf-8") + ) + assert runtime == { + Requirement(value) + for value in source["project"]["optional-dependencies"]["retargeters"] + } + assert runtime <= { + Requirement(value) + for value in examples["project"]["optional-dependencies"]["dev"] + } + assert any(requirement.name == "six" for requirement in runtime) + + +@pytest.mark.parametrize( + "module", + ( + "p7_realhand_bimanual_example", + "realhand_ffg_glove_calibration", + "realhand_ffg_diagnostics", + "realhand_ffg_hand_demo", + "realhand_ffg_hand_demo_bridge", + "fetch_realhand_assets", + ), +) +def test_module_help_outside_checkout(module, tmp_path): + environment = os.environ.copy() + namespace = str(repo_root() / "examples/teleop/python") + environment["PYTHONPATH"] = os.pathsep.join( + (namespace, environment.get("PYTHONPATH", "")) + ) + result = subprocess.run( + [sys.executable, "-m", f"isaaccapture_examples.teleop.{module}", "--help"], + cwd=tmp_path, + env=environment, + capture_output=True, + text=True, + timeout=30, + check=False, # Assert below so failures include the child's stderr. + ) + assert result.returncode == 0, result.stderr + assert "usage:" in result.stdout