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..6da58f9c08 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://www.realhand.com/ + group: peripheral + modes: 26-joint human hand tracking from USB glove sensors + 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..c595546571 --- /dev/null +++ b/docs/source/device/realhand_ffg_glove.rst @@ -0,0 +1,172 @@ +.. 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 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. + +.. contents:: On this page + :local: + :depth: 2 + +Data flow +--------- + +.. code-block:: text + + 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 IsaacCapture 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 IsaacCapture 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 + +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 + + $ ./install/plugins/realhand_ffg_glove/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. + +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 +--------------------------- + +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: + +* **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. + +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. + +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 + + $ pip install -e ./examples/teleop + $ python -m isaaccapture_examples.teleop.realhand_ffg_glove_calibration \ + --side both \ + --plugin-path install/plugins \ + --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. 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 935996d6a0..83673472ea 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -26,6 +26,15 @@ Source Nodes Available Retargeters --------------------- +.. dropdown:: P7 and RealHand L6 / O6 / L20 + + ``P7ControllerPoseRetargeter`` and ``P7HandPoseRetargeter`` map an absolute OpenXR pose into + 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 Maps hand or controller tracking to end-effector pose. ``Se3AbsRetargeter`` outputs a 7D @@ -381,3 +390,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..43d000d6d9 --- /dev/null +++ b/docs/source/references/retargeting/realhand.rst @@ -0,0 +1,166 @@ +.. 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, hand retargeting, and +simulation control along IsaacCapture'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 + * - Official ``DexHandRetargeter`` + - One 26-joint OpenXR hand (optical tracking or a glove plugin) + - 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. 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 +------------------------ + +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 + + $ 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 -m isaaccapture_examples.teleop.p7_realhand_bimanual_example \ + --mode handtracking --hand-model l20 + + $ python -m isaaccapture_examples.teleop.p7_realhand_bimanual_example \ + --mode controller --hand-model l6 + + $ python -m isaaccapture_examples.teleop.p7_realhand_bimanual_example \ + --mode ffg --hand-model o6 \ + --plugin-path install/plugins \ + --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 +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 -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 -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 +``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 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 +---------------------- + +``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/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/assets/realhand/.gitignore b/examples/teleop/assets/realhand/.gitignore new file mode 100644 index 0000000000..4eb456db83 --- /dev/null +++ b/examples/teleop/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/assets/realhand/README.md b/examples/teleop/assets/realhand/README.md new file mode 100644 index 0000000000..e3ad8df05a --- /dev/null +++ b/examples/teleop/assets/realhand/README.md @@ -0,0 +1,32 @@ + + +# P7 and RealHand Robot Assets + +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 Capture. + +From the Isaac Capture repository root, install the examples and download all three assemblies: + +```bash +pip install -e ./examples/teleop +python3 -m isaaccapture_examples.teleop.fetch_realhand_assets +``` + +Download one assembly or choose another destination with: + +```bash +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, 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/isaaccapture_examples/teleop/fetch_realhand_assets.py b/examples/teleop/python/isaaccapture_examples/teleop/fetch_realhand_assets.py new file mode 100755 index 0000000000..fcda9bad54 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/fetch_realhand_assets.py @@ -0,0 +1,324 @@ +#!/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 + blob_id: str | None = None + + +def _default_output_dir() -> Path: + 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]: + 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 + or any(ch not in "0123456789abcdef" for ch in sha256) + ): + raise RuntimeError( + f"Invalid LFS SHA-256 in Hugging Face metadata for {path}" + ) + 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 + 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 + 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]: + 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}" + ) + 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 + 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/examples/teleop/python/isaaccapture_examples/teleop/p7_realhand_bimanual_example.py b/examples/teleop/python/isaaccapture_examples/teleop/p7_realhand_bimanual_example.py new file mode 100644 index 0000000000..68e9dc8041 --- /dev/null +++ b/examples/teleop/python/isaaccapture_examples/teleop/p7_realhand_bimanual_example.py @@ -0,0 +1,385 @@ +# 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, + P7ControllerPoseRetargeter, + P7HandPoseRetargeter, + P7WorkspacePoseConfig, + TensorReorderer, +) +from isaaccapture.retargeters.realhand import get_realhand_profile +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + ControllersSource, + HandsSource, +) +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 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") + + 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 = _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 + 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: + # 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( + {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="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", + action="store_true", + help="Build and validate the graph without OpenXR", + ) + 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 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)}" + ) + 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], + 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, + ) + ) + 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/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/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/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..df0fc52ca7 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/CMakeLists.txt @@ -0,0 +1,29 @@ +# 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_reader.cpp + realhand_ffg_glove_skeleton.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 + log_bridge::log_bridge_core + yaml-cpp::yaml-cpp +) + +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..5138421dbb --- /dev/null +++ b/src/plugins/realhand_ffg_glove/README.md @@ -0,0 +1,87 @@ + + +# RealHand FFG Glove plugin + +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). +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. + +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: + +```bash +sudo usermod -aG dialout "$USER" +``` + +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. + +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 new file mode 100644 index 0000000000..a9c0191b2e --- /dev/null +++ b/src/plugins/realhand_ffg_glove/main.cpp @@ -0,0 +1,172 @@ +// 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 +#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; +} + +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) +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]; + 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 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); + } + } + + 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::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)) + { + 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) +{ + 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 new file mode 100644 index 0000000000..80d114f392 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/plugin.yaml @@ -0,0 +1,20 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: realhand_ffg_glove_plugin +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 raw sensors for calibration and diagnostics" + - path: "/glove/realhand_ffg_glove_right" + type: "joint_state" + 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 new file mode 100644 index 0000000000..b599e7077b --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.cpp @@ -0,0 +1,222 @@ +// 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 plugins::realhand_ffg_glove +{ + +namespace +{ + +constexpr std::size_t kMaxFlatbufferSize = 4096; +constexpr std::array kCollectionIds = { "realhand_ffg_glove_left", "realhand_ffg_glove_right" }; +constexpr std::array kDevicePaths = { "/hand/left", "/hand/right" }; +constexpr std::array kSensorPaths = { "/glove/realhand_ffg_glove_left", + "/glove/realhand_ffg_glove_right" }; +constexpr auto kSkeletonStaleThreshold = std::chrono::milliseconds(200); + +const char* side_name(std::size_t side) +{ + return side == 0 ? "left" : "right"; +} + +std::vector required_extensions(const plugin_utils::WristPoseSource::Requirements& wrist) +{ + 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()) + { + extensions.push_back(extension); + } + } + return extensions; +} + +} // namespace + +RealHandFFGGlovePlugin::RealHandFFGGlovePlugin(std::vector configured_ports, + std::vector baudrates, + std::string plugin_root_id, + RealHandFFGGloveConfig config) + : reader_(std::move(configured_ports), std::move(baudrates)), skeleton_(config.calibration_path, config.hand_scale) +{ + 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()) + { + 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) + { + 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); +} + +RealHandFFGGlovePlugin::~RealHandFFGGlovePlugin() = default; + +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 = reader_.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(4); + for (std::size_t side = 0; side < reader_.endpoints().size(); ++side) + { + plugin_utils::PluginDeviceStatusEntry entry{ .path = kDevicePaths[side] }; + if (!reader_.endpoints()[side]) + { + entry.state = core::PluginDeviceState_DISCONNECTED; + entry.reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + entry.error = reader_.errors()[side]; + } + else if (!reader_.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; + } + 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()); +} + +void RealHandFFGGlovePlugin::update() +{ + 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(); +} + +} // 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..087616210d --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_plugin.hpp @@ -0,0 +1,78 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "realhand_ffg_glove_reader.hpp" +#include "realhand_ffg_glove_skeleton.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace core +{ +class OpenXRSession; +} + +namespace plugin_utils +{ +class PluginDeviceStatusPublisher; +} + +namespace plugins::realhand_ffg_glove +{ + +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", + RealHandFFGGloveConfig config = {}); + ~RealHandFFGGlovePlugin(); + + void update(); + +private: + void push_state(std::size_t side_index); + void publish_status(); + + 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_; +}; + +} // 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_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 new file mode 100644 index 0000000000..1ed46592ef --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.cpp @@ -0,0 +1,296 @@ +// 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 +#include +#include + +#ifndef _WIN32 +# include +# include +# include +# include + +# 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); +} + +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 + +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 + 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); + ::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); + ::ioctl(fd_, TIOCNXCL); + ::close(fd_); + fd_ = -1; + throw std::runtime_error("tcsetattr failed for " + port + ": " + error); + } + ::tcflush(fd_, TCIOFLUSH); +#endif +} + +SerialGlove::~SerialGlove() +{ +#ifndef _WIN32 + if (fd_ >= 0) + { + ::ioctl(fd_, TIOCNXCL); + ::close(fd_); + } +#endif +} + +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) + { + 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 + (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 + // 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; +} + +} // 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..6165272262 --- /dev/null +++ b/src/plugins/realhand_ffg_glove/realhand_ffg_glove_serial.hpp @@ -0,0 +1,46 @@ +// 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(); + +} // 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 518fb9b2bf..d230cd0173 100644 --- a/src/python/isaaccapture/retargeters/__init__.py +++ b/src/python/isaaccapture/retargeters/__init__.py @@ -238,6 +238,32 @@ "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", + ), + "ControllerTriggerRealHandRetargeter": ( + ".realhand.hand", + "ControllerTriggerRealHandRetargeter", + None, + ), + "ControllerTriggerRealHandRetargeterConfig": ( + ".realhand.hand", + "ControllerTriggerRealHandRetargeterConfig", + None, + ), # .joint_space (generic joint-space devices: leader arms, exoskeletons, ...) "JointStateRetargeter": ( ".joint_space.joint_state_retargeter", @@ -410,6 +436,12 @@ 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", + "ControllerTriggerRealHandRetargeter", + "ControllerTriggerRealHandRetargeterConfig", # 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..81648d4e52 --- /dev/null +++ b/src/python/isaaccapture/retargeters/realhand/__init__.py @@ -0,0 +1,64 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""RealHand P7 arm and L6/O6/L20 hand retargeters.""" + +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", + "L6_PROFILE", + "L20_PROFILE", + "O6_PROFILE", + "P7ControllerPoseRetargeter", + "P7HandPoseRetargeter", + "P7WorkspacePoseConfig", + "REALHAND_PROFILES", + "RealHandJointSpec", + "RealHandProfile", + "get_realhand_profile", +] 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..557802024c --- /dev/null +++ b/src/python/isaaccapture/retargeters/realhand/hand.py @@ -0,0 +1,143 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 RealHand. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Controller-trigger commands for RealHand simulation models.""" + +from __future__ import annotations + +import logging +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, +) + +from .profiles import get_realhand_profile + +_LOGGER = logging.getLogger(__name__) + + +@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 + 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) + + _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): + output[index] = float(value) + + +__all__ = [ + "ControllerTriggerRealHandRetargeter", + "ControllerTriggerRealHandRetargeterConfig", +] diff --git a/src/python/isaaccapture/retargeters/realhand/profiles.py b/src/python/isaaccapture/retargeters/realhand/profiles.py new file mode 100644 index 0000000000..b9b8828d46 --- /dev/null +++ b/src/python/isaaccapture/retargeters/realhand/profiles.py @@ -0,0 +1,295 @@ +# 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: + """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 + + +@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 " + "python -m isaaccapture_examples.teleop.fetch_realhand_assets first." + ) + return path.resolve() + + +def _joint( + name: str, + upper: float, + semantic: str, + *, + lower: float = 0.0, + closed: float | None = None, +) -> RealHandJointSpec: + return RealHandJointSpec( + short_name=name, + lower=lower, + upper=upper, + trigger_closed=upper if closed is None else closed, + semantic=semantic, + ) + + +_L6_LEFT = ( + _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"), + _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 = ( + "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"), + _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"), + ) + + +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", 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 in ("index", "middle", "ring", "pinky"): + joints.extend( + ( + _joint( + f"{finger}_mcp_roll", + 0.23, + f"{finger}_spread", + lower=-0.23, + closed=0.0, + ), + _joint( + f"{finger}_mcp_pitch", + 1.22, + f"{finger}_base_flex", + closed=1.20, + ), + _joint( + f"{finger}_pip", + pip_upper, + f"{finger}_distal_flex", + 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/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..792e3014c2 --- /dev/null +++ b/tests/cpp/plugins/realhand_ffg_glove/CMakeLists.txt @@ -0,0 +1,17 @@ +# 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 + 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 +) +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..6b4a525838 --- /dev/null +++ b/tests/cpp/plugins/realhand_ffg_glove/test_realhand_ffg_glove_protocol.cpp @@ -0,0 +1,151 @@ +// 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 +#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 +{ + +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(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/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_retargeters.py b/tests/python/core/retargeting_engine/test_realhand_retargeters.py new file mode 100644 index 0000000000..e1eb3d8966 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_realhand_retargeters.py @@ -0,0 +1,160 @@ +# 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 +import subprocess +import sys + +from isaaccapture.retargeters.realhand.arm import ( + P7ControllerPoseRetargeter, + P7WorkspacePoseConfig, +) +from isaaccapture.retargeters.realhand.hand import ( + ControllerTriggerRealHandRetargeter, + ControllerTriggerRealHandRetargeterConfig, +) +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, +) + + +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 + + +@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"python -m isaaccapture_examples\.teleop\.fetch_realhand_assets", + ): + get_realhand_profile("l6").resolve_urdf(tmp_path) + + +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( + 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) 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/examples/realhand/test_realhand_asset_fetcher.py b/tests/python/examples/realhand/test_realhand_asset_fetcher.py new file mode 100644 index 0000000000..36c3bc3ced --- /dev/null +++ b/tests/python/examples/realhand/test_realhand_asset_fetcher.py @@ -0,0 +1,128 @@ +# 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 io + +import pytest + +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, "blobId": "b" * 40} + 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() + + +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"): + _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/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