From 0b98b174d24b7b24b33df75c604e6dc7bd7367ff Mon Sep 17 00:00:00 2001 From: HAOTianGa03 <2022280352@email.szu.edu.cn> Date: Mon, 2 Nov 2026 09:44:38 -0500 Subject: [PATCH 1/3] feat(revohuman): add the RevoHuman glove plugin A Linux-only plugin for BrainCo RevoHuman exoskeleton gloves. It reads each glove's 21 encoder angles and its tactile forces through the RevoHuman C SDK, maps the SDK's hand points to the 26 OpenXR hand joints and injects them into the OpenXR hand-tracking layer, placed at a wrist pose from optical hand tracking or a controller aim pose (REVOHUMAN_WRIST_SOURCE). It pushes the encoder angles as joint states and the forces with the common tactile contract, under a TactileMetadata it builds at start-up from the SDK's taxel layout of each hand; a hand model without a layout pushes no tactile. Each glove's mechanical model follows the product the SDK identifies; P1 gloves have no tactile sensors. The OpenXR mapping was reviewed on the hand model of each product, and the plugin refuses an SDK model with another SHA-256. The glove's WRIST is its palm_link origin moved 12.6 mm towards the palm, to the depth of PALM, and every hand point and taxel is relative to it. It is a point of the glove's model, not the anatomical wrist centre; the README gives the nominal offset between the two, which the plugin does not correct. Gloves are found through the SDK, at most one per side. Both gloves plugged in open together in one two-hand kit with time sync on, so their samples come from the same USB frame; a glove plugged in alone opens on its own until its partner arrives. A missing glove is looked for again every two seconds; one of a product, hand model or firmware the plugin does not support is reported as failed and not retried. Recording and playback are left to the DeviceIO MCAP session (McapRecordingConfig, McapReplayConfig), as for the other trackers. The plugin publishes /device_status when launched with a plugin root id, and TeleopSession monitors it like manus_hand_plugin, spacemouse and gamepad. install.sh downloads the pinned C SDK release (c-sdk-v0.1.3), verifies its per-arch SHA-256 and installs the plugin with the SDK library and its licence next to it. BUILD_PLUGIN_REVOHUMAN is OFF by default; the plugin is skipped when the SDK is missing, and only its libraries build when the SDK has no library to link. The tests are one suite per plugin library. revohuman_core_tests covers the pipeline, the tactile contract, the device status and the other core helpers. revohuman_live_tests runs the live source against a fake of the C SDK library, compiled against the SDK's own revohuman.h. revohuman_integration_tests checks the reviewed OpenXR mapping and the tactile layout against the SDK's hand points on both hand models. The tests are skipped with the plugin when the SDK is missing; with an SDK whose library does not link, only the core and live suites build. Two ctest cases run the plugin binary to check how the wrist source variable and flag are refused. The device page covers the data flow, the published collections, prerequisites, the C SDK installation, running the plugin, wrist-pose calibration, the tactile layout and troubleshooting. The hand-tracking index lists it, and the ecosystem device table has a row for the glove. Co-authored-by: chenwenjun <1131359622@qq.com> Co-authored-by: ljt-brainco <2275816616@qq.com> Co-authored-by: woltim Co-Authored-By: Claude Opus 5.5 Signed-off-by: chenwenjun <1131359622@qq.com> Signed-off-by: woltim Signed-off-by: ljt-brainco <2275816616@qq.com> Signed-off-by: HAOTianGa03 <2022280352@email.szu.edu.cn> --- CMakeLists.txt | 1 + cmake/ClangFormat.cmake | 1 + docs/source/_data/devices.yaml | 17 + docs/source/device/hand_tracking/index.rst | 1 + .../source/device/hand_tracking/revohuman.rst | 227 ++++ src/plugins/CMakeLists.txt | 5 + src/plugins/revohuman/.gitignore | 5 + src/plugins/revohuman/CMakeLists.txt | 107 ++ src/plugins/revohuman/README.md | 271 ++++ src/plugins/revohuman/app/CMakeLists.txt | 57 + src/plugins/revohuman/app/live_factory.cpp | 67 + src/plugins/revohuman/app/main.cpp | 205 +++ .../revohuman/app/revohuman_plugin.cpp | 399 ++++++ .../revohuman/app/revohuman_plugin.hpp | 109 ++ src/plugins/revohuman/core/CMakeLists.txt | 41 + src/plugins/revohuman/core/device_clock.cpp | 39 + src/plugins/revohuman/core/device_status.cpp | 144 ++ src/plugins/revohuman/core/event_log.cpp | 116 ++ .../inc/revohuman_plugin/device_clock.hpp | 36 + .../inc/revohuman_plugin/device_status.hpp | 48 + .../core/inc/revohuman_plugin/event_log.hpp | 42 + .../core/inc/revohuman_plugin/pipeline.hpp | 234 ++++ .../core/inc/revohuman_plugin/pose_math.hpp | 47 + .../core/inc/revohuman_plugin/serialize.hpp | 31 + .../core/inc/revohuman_plugin/sha256.hpp | 20 + .../core/inc/revohuman_plugin/source.hpp | 104 ++ .../core/inc/revohuman_plugin/tactile.hpp | 100 ++ .../core/inc/revohuman_plugin/types.hpp | 89 ++ .../inc/revohuman_plugin/wrist_tracking.hpp | 46 + src/plugins/revohuman/core/pipeline.cpp | 470 +++++++ src/plugins/revohuman/core/pose_math.cpp | 203 +++ src/plugins/revohuman/core/serialize.cpp | 72 + src/plugins/revohuman/core/sha256.cpp | 113 ++ src/plugins/revohuman/core/tactile.cpp | 206 +++ src/plugins/revohuman/core/types.cpp | 17 + src/plugins/revohuman/core/wrist_tracking.cpp | 41 + src/plugins/revohuman/install.sh | 232 ++++ .../revohuman/kinematics/CMakeLists.txt | 65 + .../revohuman/kinematics/hand_geometry.cpp | 347 +++++ .../revohuman/kinematics/hand_geometry.hpp | 62 + .../revohuman/kinematics/hand_kinematics.cpp | 341 +++++ .../inc/revohuman_plugin/hand_kinematics.hpp | 67 + .../revohuman/kinematics/openxr_model.inc | 115 ++ src/plugins/revohuman/live/CMakeLists.txt | 25 + .../live/inc/revohuman_plugin/live_source.hpp | 225 ++++ .../live/inc/revohuman_plugin/rh_frame.hpp | 23 + src/plugins/revohuman/live/live_source.cpp | 1159 ++++++++++++++++ src/plugins/revohuman/live/rh_frame.cpp | 68 + src/plugins/revohuman/plugin.yaml | 26 + .../teleop_session_manager/teleop_session.py | 9 +- tests/cpp/plugins/CMakeLists.txt | 5 + tests/cpp/plugins/revohuman/CMakeLists.txt | 6 + .../cpp/plugins/revohuman/core/CMakeLists.txt | 23 + .../revohuman/core/test_device_clock.cpp | 41 + .../revohuman/core/test_device_status.cpp | 242 ++++ .../plugins/revohuman/core/test_event_log.cpp | 72 + .../plugins/revohuman/core/test_pipeline.cpp | 1180 +++++++++++++++++ .../plugins/revohuman/core/test_pose_math.cpp | 131 ++ .../plugins/revohuman/core/test_serialize.cpp | 98 ++ .../plugins/revohuman/core/test_sha256.cpp | 43 + .../plugins/revohuman/core/test_shapes.hpp | 58 + .../plugins/revohuman/core/test_tactile.cpp | 244 ++++ .../revohuman/core/test_wrist_tracking.cpp | 51 + .../revohuman/integration/CMakeLists.txt | 41 + .../integration/test_integration.cpp | 467 +++++++ .../cpp/plugins/revohuman/live/CMakeLists.txt | 18 + .../live/fake_sdk/fake_revohuman.cpp | 780 +++++++++++ .../live/fake_sdk/fake_revohuman.hpp | 101 ++ .../revohuman/live/test_live_source.cpp | 1146 ++++++++++++++++ .../plugins/revohuman/live/test_rh_frame.cpp | 107 ++ .../test_teleop_session.py | 2 +- 71 files changed, 11349 insertions(+), 2 deletions(-) create mode 100644 docs/source/device/hand_tracking/revohuman.rst create mode 100644 src/plugins/revohuman/.gitignore create mode 100644 src/plugins/revohuman/CMakeLists.txt create mode 100644 src/plugins/revohuman/README.md create mode 100644 src/plugins/revohuman/app/CMakeLists.txt create mode 100644 src/plugins/revohuman/app/live_factory.cpp create mode 100644 src/plugins/revohuman/app/main.cpp create mode 100644 src/plugins/revohuman/app/revohuman_plugin.cpp create mode 100644 src/plugins/revohuman/app/revohuman_plugin.hpp create mode 100644 src/plugins/revohuman/core/CMakeLists.txt create mode 100644 src/plugins/revohuman/core/device_clock.cpp create mode 100644 src/plugins/revohuman/core/device_status.cpp create mode 100644 src/plugins/revohuman/core/event_log.cpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/device_clock.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/device_status.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/event_log.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/pipeline.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/pose_math.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/serialize.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/sha256.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/source.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/tactile.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/types.hpp create mode 100644 src/plugins/revohuman/core/inc/revohuman_plugin/wrist_tracking.hpp create mode 100644 src/plugins/revohuman/core/pipeline.cpp create mode 100644 src/plugins/revohuman/core/pose_math.cpp create mode 100644 src/plugins/revohuman/core/serialize.cpp create mode 100644 src/plugins/revohuman/core/sha256.cpp create mode 100644 src/plugins/revohuman/core/tactile.cpp create mode 100644 src/plugins/revohuman/core/types.cpp create mode 100644 src/plugins/revohuman/core/wrist_tracking.cpp create mode 100755 src/plugins/revohuman/install.sh create mode 100644 src/plugins/revohuman/kinematics/CMakeLists.txt create mode 100644 src/plugins/revohuman/kinematics/hand_geometry.cpp create mode 100644 src/plugins/revohuman/kinematics/hand_geometry.hpp create mode 100644 src/plugins/revohuman/kinematics/hand_kinematics.cpp create mode 100644 src/plugins/revohuman/kinematics/inc/revohuman_plugin/hand_kinematics.hpp create mode 100644 src/plugins/revohuman/kinematics/openxr_model.inc create mode 100644 src/plugins/revohuman/live/CMakeLists.txt create mode 100644 src/plugins/revohuman/live/inc/revohuman_plugin/live_source.hpp create mode 100644 src/plugins/revohuman/live/inc/revohuman_plugin/rh_frame.hpp create mode 100644 src/plugins/revohuman/live/live_source.cpp create mode 100644 src/plugins/revohuman/live/rh_frame.cpp create mode 100644 src/plugins/revohuman/plugin.yaml create mode 100644 tests/cpp/plugins/revohuman/CMakeLists.txt create mode 100644 tests/cpp/plugins/revohuman/core/CMakeLists.txt create mode 100644 tests/cpp/plugins/revohuman/core/test_device_clock.cpp create mode 100644 tests/cpp/plugins/revohuman/core/test_device_status.cpp create mode 100644 tests/cpp/plugins/revohuman/core/test_event_log.cpp create mode 100644 tests/cpp/plugins/revohuman/core/test_pipeline.cpp create mode 100644 tests/cpp/plugins/revohuman/core/test_pose_math.cpp create mode 100644 tests/cpp/plugins/revohuman/core/test_serialize.cpp create mode 100644 tests/cpp/plugins/revohuman/core/test_sha256.cpp create mode 100644 tests/cpp/plugins/revohuman/core/test_shapes.hpp create mode 100644 tests/cpp/plugins/revohuman/core/test_tactile.cpp create mode 100644 tests/cpp/plugins/revohuman/core/test_wrist_tracking.cpp create mode 100644 tests/cpp/plugins/revohuman/integration/CMakeLists.txt create mode 100644 tests/cpp/plugins/revohuman/integration/test_integration.cpp create mode 100644 tests/cpp/plugins/revohuman/live/CMakeLists.txt create mode 100644 tests/cpp/plugins/revohuman/live/fake_sdk/fake_revohuman.cpp create mode 100644 tests/cpp/plugins/revohuman/live/fake_sdk/fake_revohuman.hpp create mode 100644 tests/cpp/plugins/revohuman/live/test_live_source.cpp create mode 100644 tests/cpp/plugins/revohuman/live/test_rh_frame.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 521ec03c30..21853cd641 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,6 +71,7 @@ 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) +option(BUILD_PLUGIN_REVOHUMAN "Build RevoHuman glove plugin (Linux; skips if the RevoHuman C SDK is missing)" OFF) 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/cmake/ClangFormat.cmake b/cmake/ClangFormat.cmake index b0500093e1..1abdc2ea4e 100644 --- a/cmake/ClangFormat.cmake +++ b/cmake/ClangFormat.cmake @@ -40,6 +40,7 @@ set(_cf_exclude_patterns ".*/deps/.*" ".*/manus/ManusSDK/.*" ".*/wuji_glove/wuji-sdk-c-.*" + ".*/revohuman/revohuman-c-sdk-.*" ".*/node_modules/.*" ".*/third_party/.*" ".*/venv/.*" diff --git a/docs/source/_data/devices.yaml b/docs/source/_data/devices.yaml index 92facf6c24..a19a644dd4 100644 --- a/docs/source/_data/devices.yaml +++ b/docs/source/_data/devices.yaml @@ -273,6 +273,23 @@ devices: url: https://wuji.tech/en/ - email: sales@wuji.tech + - id: revohuman-glove + name: RevoHuman Glove + url: https://github.com/BrainCoTech/brainco-revohuman-sdk + group: peripheral + modes: Hand tracking, joint encoders, tactile + since: main + details: + setup: + - label: RevoHuman Glove docs + doc: /device/hand_tracking/revohuman + - label: RevoHuman Glove Plugin + url: https://github.com/NVIDIA/IsaacCapture/tree/main/src/plugins/revohuman + requirements: + - Linux, x86_64 or aarch64 + - RevoHuman C SDK 0.1.3 + - USB connection to the plugin host + - id: noitom name: Noitom Robotics Motion Capture url: https://www.noitomrobotics.com diff --git a/docs/source/device/hand_tracking/index.rst b/docs/source/device/hand_tracking/index.rst index e792568c1c..d761a83aa4 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 + revohuman diff --git a/docs/source/device/hand_tracking/revohuman.rst b/docs/source/device/hand_tracking/revohuman.rst new file mode 100644 index 0000000000..1fd8be13e0 --- /dev/null +++ b/docs/source/device/hand_tracking/revohuman.rst @@ -0,0 +1,227 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +RevoHuman Glove +=============== + +A Linux-only plugin for BrainCo RevoHuman exoskeleton gloves. It reads each +glove's 21 encoder angles and its tactile forces through the RevoHuman C SDK, +maps the SDK's hand points to the 26 OpenXR hand joints and injects them into +the OpenXR hand-tracking layer. It also pushes the encoder angles as joint +states and the forces with the common tactile contract, laid out with the SDK's +taxel layout. + +.. contents:: On this page + :local: + :depth: 2 + +Data flow +--------- + +.. code-block:: text + + RevoHuman glove (USB) ──► RevoHuman C SDK ─┐ + ├─► revohuman plugin ──► CloudXR OpenXR runtime + Optical hand / controller ─────────────────┘ (hand points, (hand joints, joint states, + wrist-pose fusion) tactile forces) + +The plugin computes no kinematics of its own: the SDK turns the angles into hand +points for the glove's mechanical model, and the plugin maps those points to +OpenXR order with corrections reviewed for that model. No URDF is loaded. + +Outputs +------- + +Per hand, with the default collection prefix ``revohuman`` +(``--collection-prefix=NAME`` changes it): + +.. list-table:: + :widths: 40 60 + :header-rows: 1 + + * - Output + - Contents + * - OpenXR hand ``/hand/left``, ``/hand/right`` + - 26 joints relative to the wrist, placed at the wrist pose described below + * - ``revohuman__joint_state`` / tensor ``joint_state`` + - ``JointStateOutput`` with the 21 encoder angles in radians, in SDK order; + see :doc:`/device/joint_space/index` + * - ``revohuman__tactile`` / tensor ``tactile`` + - ``TactileDataArray`` taxel forces: the SDK's calibrated reading, unchanged, + along each taxel's outward normal; see :doc:`/device/tactile/index` + * - ``revohuman__tactile/metadata`` + - The ``TactileMetadata`` layout the forces belong to + +Started by hand, the plugin publishes ``/device_status`` only with +``--plugin-root-id=``. + +Prerequisites +------------- + +- **Linux** — x86_64 or aarch64. +- **Source-build prerequisites** — the toolchain and system packages in + :doc:`/development/build_from_source/index`, for ``install.sh`` and the manual + CMake build alike. +- **RevoHuman C SDK 0.1.3** — the ``c-sdk-v0.1.3`` release of + `brainco-revohuman-sdk `_, + which extracts to ``include/revohuman.h``, ``lib/librevohuman.so`` with its + versioned names, and ``LICENSE``. The install script downloads it + automatically. A Python SDK wheel cannot replace it. +- **RevoHuman gloves** connected over USB, with access to their serial ports. +- **CloudXR runtime** running on the plugin host. + +Installation +------------ + +``install.sh`` downloads the pinned C SDK release, verifies it against a +per-architecture SHA-256, extracts it next to the script (re-runs reuse the +extracted copy), and configures, builds, and installs the plugin with the SDK +library and its licence next to it: + +.. code-block:: bash + + ./src/plugins/revohuman/install.sh + +Pass ``--build-dir `` to reuse a non-default top-level CMake build +directory and ``--install-prefix `` to choose where the plugin goes. +``--sdk-root `` (or ``REVOHUMAN_SDK_ROOT``) uses an SDK package you already +extracted. + +To drive CMake directly: + +.. code-block:: bash + + cmake -B build -DBUILD_PLUGINS=ON -DBUILD_PLUGIN_REVOHUMAN=ON \ + -DREVOHUMAN_SDK_ROOT=/path/to/revohuman-c-sdk + cmake --build build --target revohuman_plugin + cmake --install build --component revohuman + +The installed plugin finds the SDK library next to itself, so it needs neither +the download nor the build directory afterwards. Keep the library's symlinks. + +Running the Plugin +------------------ + +The plugin reaches the Teleop session through the CloudXR / OpenXR runtime, so +start the runtime first and source its environment in the shell that launches +the plugin: + +.. code-block:: bash + + python -m isaaccapture.cloudxr.service start # runs in the background + source ~/.cloudxr/run/cloudxr.env + ./install/plugins/revohuman/revohuman_plugin + +See :ref:`dedicated-cloudxr-runtime` and +:ref:`load-cloudxr-environment-variables` for the full service setup. +Applications can also launch it through its ``plugin.yaml`` using +``TeleopSession``. The gloves are its only source, so it takes no source +argument; an unknown argument such as ``--live`` or ``--replay`` is ignored with +a warning. + +The plugin neither records nor plays back: record its collections with +``McapRecordingConfig`` and play them back with ``McapReplayConfig``, as for any +other tracker (see :doc:`/references/mcap_record_replay`). + +The plugin finds the gloves through the SDK, at most one left and one right, +with nothing to declare. When both are plugged in, it opens them in one +two-hand SDK kit with time sync on, so the two hands' samples come from the +same USB frame. A glove plugged in alone opens on its own; when the other one +arrives, both are reopened together. Gloves that cannot be opened together +keep streaming on their own. Until a glove is found, the plugin looks for it +every 2 s and reports the SDK's reason in its status, without holding up the +other hand. The SDK reconnects a glove after a short dropout; when the stream +ends anyway, the plugin closes the glove, together with its partner when the +two share a kit, and looks for them again. + +Each glove's mechanical model follows the product the SDK identifies: a PTV1 +glove uses the DV1 model and a P1 glove the P model. A P1 glove has no tactile +sensors, so it streams the hand and joint states without tactile outputs. A +glove of the wrong product or with incompatible firmware is reported as failed. + +Wrist pose +---------- + +The glove stream contains wrist-relative joints but no absolute 6DoF wrist +pose. Before injection, the plugin places the hand at a wrist pose from optical +hand tracking or from a controller aim pose combined with a rigid per-hand +offset. + +Set ``REVOHUMAN_WRIST_SOURCE`` (or pass ``--wrist-source``) to ``auto`` (the +default), ``hand_tracking``, or ``controller``. In ``auto`` mode, optical +tracking is preferred; the controller is used when optical tracking provides no +valid pose. The plugin refuses a ``--wrist-source`` that disagrees with the +variable. + +A controller reports its aim pose, not the wrist. No controller mount is +calibrated for these gloves yet, so the aim-to-wrist offsets default to +identity and the plugin warns for each hand without one unless the source is +``hand_tracking``. Set them with: + +.. code-block:: bash + + ./install/plugins/revohuman/revohuman_plugin \ + --aim-to-wrist-left=px,py,pz,qx,qy,qz,qw \ + --aim-to-wrist-right=px,py,pz,qx,qy,qz,qw + +Position values are in meters and quaternion values use ``(x, y, z, w)``. + +Without a tracked wrist the hand is still injected, without tracking flags, at +the last valid wrist pose (or the stage origin), and its status is degraded. + +The glove's wrist is a reference point of its mechanical model: the origin of +its ``palm_link``, on the back of the hand near the end of the forearm, moved +about 12.6 mm towards the palm to the depth of the palm centre. It is not the +anatomical wrist centre that the OpenXR wrist joint stands for. Nominally the +wrist centre is about 4 cm further towards the fingers and about 0.2 to 0.7 cm +further towards the palm; these figures come from typical adult hand +dimensions, not from measurements on the glove, and vary with the wearer. With +optical hand tracking, the injected hand therefore sits that much further +towards the fingers and the palm than the tracked hand. With a controller, set +the aim-to-wrist offsets to end at the glove's wrist point. Verify the +alignment for the wearer. + +Tactile layout +-------------- + +At start-up the plugin builds each hand's ``TactileMetadata`` from the C SDK's +taxel layout for the DV1 model (``rh_hand_model_taxel_layout``); there is no +layout file or option. Each taxel is bound rigidly to the OpenXR joint its SDK +link moves with, with its +Z along the outward normal. That joint is rigid on +the link, so the skinned taxels stay on the SDK's taxels as the fingers bend. +The log names the layout source, taxel count and SHA-256 of each hand's +metadata. When the SDK has no layout, the plugin logs why and pushes no tactile +data; the other outputs are unaffected. + +A force sample is pushed only while it is calibrated, recent and newer than the +last one, and its status names the reason when it is not. Consumers must check +that status as well as the presence of data. + +Troubleshooting +--------------- + +.. list-table:: + :widths: 40 60 + :header-rows: 1 + + * - Symptom + - Fix + * - ``install.sh`` fails with a SHA-256 mismatch + - The downloaded package does not match the pinned release. Re-run to + rule out a corrupted download; if it persists, do not bypass the check + — verify the release assets upstream. + * - No ``revohuman_plugin`` is built, ``install.sh`` refuses the install, or + the plugin stops before the OpenXR session naming an SDK function + - The SDK found during configuration is incomplete: its library is missing + or lacks a function the plugin calls. Point ``REVOHUMAN_SDK_ROOT`` at a + complete C SDK release and reconfigure. + * - A glove is never opened and its status names a busy port + - Another process, such as the SDK viewer, holds the glove's serial port. + Close it; the plugin opens the glove on its next attempt. + * - A glove is reported as failed + - The status carries the SDK's reason: a product or variant the plugin + does not support, or incompatible firmware. These are not retried. + * - Hands appear but a downstream client sees no tactile data + - A P1 glove has no tactile sensors. Otherwise check the start-up log for + why there is no ``TactileMetadata`` (an SDK without the taxel layout), + and the tactile status for the reason a sample was not pushed. diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index 4e5fa231f1..bc00801c16 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -43,3 +43,8 @@ endif() if(BUILD_PLUGIN_SHARPA_AVATAR) add_subdirectory(sharpa_avatar) endif() +# Gated: Linux + the RevoHuman C SDK (REVOHUMAN_SDK_ROOT). Missing SDK skips the plugin and its tests; +# an SDK without a library the live source links against skips only the plugin executable. +if(BUILD_PLUGIN_REVOHUMAN) + add_subdirectory(revohuman) +endif() diff --git a/src/plugins/revohuman/.gitignore b/src/plugins/revohuman/.gitignore new file mode 100644 index 0000000000..1f38a3920c --- /dev/null +++ b/src/plugins/revohuman/.gitignore @@ -0,0 +1,5 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Vendor SDK extracted by install.sh +revohuman-c-sdk-*/ diff --git a/src/plugins/revohuman/CMakeLists.txt b/src/plugins/revohuman/CMakeLists.txt new file mode 100644 index 0000000000..2e724fd36d --- /dev/null +++ b/src/plugins/revohuman/CMakeLists.txt @@ -0,0 +1,107 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# RevoHuman exoskeleton gloves: OpenXR hands, encoder joint states and tactile data with its +# TactileMetadata, pushed via OpenXR. + +# Outcome of this configure for scripts such as install.sh, which must not accept a build without +# the plugin; OFF until the live source is known to link, so an early return below leaves it OFF. +set(REVOHUMAN_LIVE_AVAILABLE OFF CACHE INTERNAL "RevoHuman live source links against the C SDK") + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(STATUS "Skipping revohuman plugin (Linux only)") + add_custom_target(revohuman_plugin + COMMAND ${CMAKE_COMMAND} -E echo "Skipping revohuman: Linux only") + return() +endif() + +# The plugin needs the C SDK header (revohuman.h); the live source also needs its library. +# There is no default location: without the cache variable or the environment variable the +# plugin is skipped, as it is when the header is missing. install.sh always passes it. +set(REVOHUMAN_SDK_ROOT "" CACHE PATH "RevoHuman C SDK root (contains include/revohuman.h and lib/)") +if(NOT REVOHUMAN_SDK_ROOT AND DEFINED ENV{REVOHUMAN_SDK_ROOT}) + set(REVOHUMAN_SDK_ROOT "$ENV{REVOHUMAN_SDK_ROOT}") +endif() +set(_revohuman_no_sdk "") +if(NOT REVOHUMAN_SDK_ROOT) + set(_revohuman_no_sdk "RevoHuman C SDK root not set (set -DREVOHUMAN_SDK_ROOT=/path/to/sdk or REVOHUMAN_SDK_ROOT)") +elseif(NOT EXISTS "${REVOHUMAN_SDK_ROOT}/include/revohuman.h") + set(_revohuman_no_sdk "RevoHuman C SDK not found (set -DREVOHUMAN_SDK_ROOT=/path/to/sdk)") +endif() +if(_revohuman_no_sdk) + message(STATUS "${_revohuman_no_sdk} — skipping revohuman plugin build.") + return() +endif() +set(REVOHUMAN_SDK_INCLUDE_DIR "${REVOHUMAN_SDK_ROOT}/include") + +add_library(revohuman_sdk_headers INTERFACE) +target_include_directories(revohuman_sdk_headers INTERFACE $) +add_library(revohuman_plugin::sdk_headers ALIAS revohuman_sdk_headers) + +# The library is used only when it links with every function the live source calls, so a partial +# SDK build skips the plugin executable instead of failing the build. Checked on every configure, +# and searched again whenever the SDK root changes (find_library alone keeps its first result). +if(NOT "${REVOHUMAN_SDK_ROOT}" STREQUAL "${_REVOHUMAN_SDK_LIBRARY_ROOT}") + unset(REVOHUMAN_SDK_LIBRARY CACHE) + set(_REVOHUMAN_SDK_LIBRARY_ROOT "${REVOHUMAN_SDK_ROOT}" CACHE INTERNAL "SDK root REVOHUMAN_SDK_LIBRARY was found in") +endif() +find_library(REVOHUMAN_SDK_LIBRARY NAMES revohuman PATHS "${REVOHUMAN_SDK_ROOT}/lib" NO_DEFAULT_PATH) +set(REVOHUMAN_LIVE_AVAILABLE OFF) +if(REVOHUMAN_SDK_LIBRARY) + include(CheckCXXSourceCompiles) + include(CMakePushCheckState) + cmake_push_check_state(RESET) + set(CMAKE_REQUIRED_INCLUDES "${REVOHUMAN_SDK_INCLUDE_DIR}") + set(CMAKE_REQUIRED_LIBRARIES "${REVOHUMAN_SDK_LIBRARY}") + set(CMAKE_REQUIRED_QUIET ON) + unset(REVOHUMAN_SDK_LINKS CACHE) + check_cxx_source_compiles([=[ +#include +using Fn = void (*)(); +#define RH_FN(f) reinterpret_cast(&f) +int main(int argc, char**) +{ + const Fn used[] = { + RH_FN(rh_init_options_init), RH_FN(rh_init), RH_FN(rh_shutdown), RH_FN(rh_version), + RH_FN(rh_set_log_callback), RH_FN(rh_last_error), RH_FN(rh_last_error_code), RH_FN(rh_error_class_name), + RH_FN(rh_kit_options_init), RH_FN(rh_kit_open), + RH_FN(rh_kit_close), RH_FN(rh_kit_identity), RH_FN(rh_kit_glove), RH_FN(rh_glove_info), + RH_FN(rh_joint_name), RH_FN(rh_joints_to_radians), RH_FN(rh_subscribe_options_init), + RH_FN(rh_glove_subscribe), RH_FN(rh_sub_recv_glove), RH_FN(rh_sub_dropped), RH_FN(rh_sub_close), + RH_FN(rh_glove_capture_ok), RH_FN(rh_glove_capture_cause), RH_FN(rh_scan_options_init), + RH_FN(rh_scan), RH_FN(rh_scan_count), RH_FN(rh_scan_get), RH_FN(rh_scan_free), + }; + return used[argc] == nullptr; +} +]=] REVOHUMAN_SDK_LINKS) + cmake_pop_check_state() + if(REVOHUMAN_SDK_LINKS) + set(REVOHUMAN_LIVE_AVAILABLE ON) + add_library(revohuman_plugin::sdk UNKNOWN IMPORTED) + set_target_properties(revohuman_plugin::sdk PROPERTIES + IMPORTED_LOCATION "${REVOHUMAN_SDK_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${REVOHUMAN_SDK_INCLUDE_DIR}") + message(STATUS "RevoHuman C SDK library ${REVOHUMAN_SDK_LIBRARY}: live source available " + "(installed next to the plugin)") + else() + message(STATUS "RevoHuman C SDK library ${REVOHUMAN_SDK_LIBRARY} lacks functions the live source " + "calls — skipping the plugin executable") + endif() +else() + message(STATUS "RevoHuman C SDK library not found in ${REVOHUMAN_SDK_ROOT}/lib — skipping the plugin executable") +endif() +set(REVOHUMAN_LIVE_AVAILABLE "${REVOHUMAN_LIVE_AVAILABLE}" + CACHE INTERNAL "RevoHuman live source links against the C SDK") + +add_subdirectory(core) +add_subdirectory(live) +if(REVOHUMAN_LIVE_AVAILABLE) + add_subdirectory(kinematics) +endif() +add_subdirectory(app) + +# One component for the plugin, its manifest, README and the SDK library (app/), so install.sh installs +# exactly these; a plain `cmake --install` still installs every component. No plugin, no manifest. +if(REVOHUMAN_LIVE_AVAILABLE) + install(FILES plugin.yaml README.md DESTINATION plugins/revohuman COMPONENT revohuman) +endif() diff --git a/src/plugins/revohuman/README.md b/src/plugins/revohuman/README.md new file mode 100644 index 0000000000..2f16cba70f --- /dev/null +++ b/src/plugins/revohuman/README.md @@ -0,0 +1,271 @@ + + +# RevoHuman plugin + +The plugin reads 21 glove angles from the unmodified RevoHuman C SDK, takes the +SDK's hand points for them and maps those to 26 wrist-relative OpenXR joints with +the reviewed corrections in its `kinematics/` module; no URDF is loaded at +runtime. The `isaaccapture` wheel does not contain this plugin or the SDK; start +it from the CMake install tree. + +Prerequisites, installation, running, the wrist pose and troubleshooting are +covered in the +[RevoHuman Glove](../../../docs/source/device/hand_tracking/revohuman.rst) +device page. This README adds the build details, the hand calculation and the +tactile contract. + +## Tactile streams + +| Stream | Contract | +| --- | --- | +| `__tactile` / `tactile` | `TactileDataArray.taxel_forces`, taxel-local, the SDK's reading unchanged | +| `__tactile/metadata` / `metadata` | bare `TactileMetadata`, LBS + HandSkeletonRef, 68,864-byte cap | + +A taxel's segment index addresses `joint_ids`, not the OpenXR enum directly. +For example, with `joint_ids=[INDEX_TIP, WRIST]`, index 0 means INDEX_TIP and +index 1 means WRIST. Taxel pose orientation stores the tangent/normal frame. + +## Build and install + +Follow the repository's build-from-source prerequisites first: CMake >= 3.24, +a C++20 compiler, uv, Python 3.11–3.13, clang-format-14, and patchelf. Put the +selected Python environment and `~/.local/bin` on PATH. + +`install.sh` gets the C SDK in one of two ways: + +- By default it downloads the public release pinned at the top of the script, + checks it against the pinned SHA-256 for the host architecture (x86_64 or + aarch64) and extracts it next to the script, where later runs reuse it. +- `--sdk-root ` (or `REVOHUMAN_SDK_ROOT`) uses an extracted C SDK + package: `include/revohuman.h`, `lib/librevohuman.so` with its versioned + names, and `LICENSE`. + +A Python SDK wheel cannot replace the C headers and library. The hand model +assumes the SDK's 21-angle order; an SDK that reorders the angles needs a review +of the plugin mapping, which the configure-time function checks do not catch. + +Run from the IsaacCapture repository root: + +```bash +CMAKE_BUILD_PARALLEL_LEVEL=4 ./src/plugins/revohuman/install.sh \ + --build-dir "$PWD/build-revohuman" \ + --install-prefix "$PWD/install" \ + -- -DBUILD_VIZ=OFF -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_EXAMPLES=OFF +``` + +Add `--sdk-root /path/to/revohuman-c-sdk` to use an SDK package you already have. + +| Path | Contents | +| --- | --- | +| `revohuman-c-sdk--linux-/` (next to `install.sh`) | Downloaded release, reused on later runs; ignored by git | +| `` | Framework/plugin build; defaults to `/build` | +| `/plugins/revohuman` | Plugin, SDK shared library and SDK licence (`LICENSE.revohuman-sdk-c`) | + +Without `--install-prefix`, the build's `CMAKE_INSTALL_PREFIX` applies (a fresh +framework build defaults to `/install`). + +The installed plugin finds the SDK library next to itself through `$ORIGIN`, +so it needs neither the SDK download nor its build directory. Keep the +library's symlinks. An OpenXR runtime and USB device permissions are still +required. + +## Hand poses + +Each glove's mechanical model follows the product the SDK identifies once the +glove is open, through the SDK's `rh_variant_hand_model`: P1 uses P and PTV1 +uses DV1. There is no option to choose it. + +Started by hand, the plugin publishes `/device_status` only with +`--plugin-root-id=`; consumers that gate on it show no forces otherwise. + +For every glove sample the plugin passes the SDK angles to `rh_hand_points` for +the glove's model and uses its 27 points: 21 joint child links, five fingertips +and the palm_link origin, in the `{hand}_tracker_link` frame. Positions are meters +and rotations are row-major 3x3 matrices; the SDK writes NaN for every quantity +an invalid angle affects. The plugin re-expresses the points from WRIST (the +tracker origin becomes PALM, the palm_link origin moved to the depth of PALM +becomes WRIST), takes the SDK fingertips as given and applies the reviewed +OpenXR mapping below. It computes no FK of its own. + +The SDK's P model has its own link positions and fingertips. The corrections +fitted on DV1 have been reviewed for P and apply to it unchanged. A P1 glove has +no tactile sensors: it streams the hand and joint states like a PTV1 glove, its +hand has no tactile collections, and its tactile status is disabled with "this +glove has no tactile sensors". + +The plugin requires `rh_variant_hand_model` (C SDK 0.1.1 or later), +`rh_hand_model_info` and `rh_hand_points` with points version 1, all detected +during configuration. Without them the plugin stops before the OpenXR session +and names the reason, and `install.sh` refuses the install. With an SDK whose +library is missing or lacks a function the glove source calls, no plugin +executable is built at all. Each SDK model must match the SHA-256 reviewed for +it in `kinematics/openxr_model.inc`; for DV1 that is +`9e343e1f1ffccee7bf8dbfcd24bf05b085ff400668c9bc3b071fd174b7d55592`. A different +model is rejected before opening the gloves. + +## Hand calculation boundary + +The numeric geometry is the reviewed mapping in `kinematics/openxr_model.inc`: +for each hand, the WRIST rotation relative to palm_link and the per-point side +and height corrections. It was fitted on the SDK DV1 model and reviewed on the +P model, both named by their SHA-256 there. Neither compilation nor runtime reads +a URDF. + +`live/live_source.cpp` reads the SDK angles and calls `rh_joints_to_radians`. +`kinematics/hand_kinematics.cpp` calls `rh_hand_points`, and +`kinematics/hand_geometry.cpp` maps the points to OpenXR order and applies the +corrections. + +The SDK's 21 link points follow its angle order, **not** the OpenXR order: + +| Finger | SDK angle/link slots | OpenXR output slots | +| --- | --- | --- | +| Index | 0–3 | 7, 8, 9, 10 | +| Middle | 4–7 | 12, 13, 14, 15 | +| Ring | 8–11 | 17, 18, 19, 20 | +| Little | 12–15 | 22, 23, 24, 25 | +| Thumb | 16–20 | 2, 3, 4, 5 | + +The first four groups run from the distal link towards the palm. For the thumb, +slot 20 is the proximal anchor, 17 and 16 the next two points, and slot 19 sets +the proximal orientation. Output slot 0 is PALM, 1 is WRIST, and 6/11/16/21 are +invalid metacarpal placeholders. These six poses remain fixed relative to WRIST +when finger angles change. The C++ `kFingerMap` table states SDK and output +indices separately; a point's orientation can depend on several upstream angles. + +Four-finger side corrections affect downstream points and tips, not their +proximal anchors. The thumb has its own side correction. Independent height +corrections apply to WRIST, the four proximal anchors, PALM, thumb metacarpal +and four placeholders. Each output point owns separate lateral and pad +corrections in `openxr_model.inc`, including zero entries. A non-thumb proximal +joint is placed with its corrections at its anchor, then carried with the MCP +flexion and oriented by the MCP link, so it is rigid on that link like the +intermediate and distal joints: the bone keeps its length and the joint turns by +the encoder angle. It therefore follows the glove's MCP link, not a measured +anatomical MCP centre. An invalid MCP angle invalidates it. Each tip is the SDK +fingertip, rigid on its distal link, oriented along the distal-to-tip direction. +Position is in meters, orientation is normalized XYZW, and all 26 output poses +are relative to WRIST. OpenXR position/orientation validity follows each point +independently; actual tracking flags require a usable wrist source. + +Valid points carry separate position and orientation flags, and a partially +valid hand keeps them per point. Point radii are nominal visualization values. + +## Real devices + +The wrist pose comes from the OpenXR runtime through the framework's +`WristPoseSource`, as for the other glove plugins; the device page lists the +wrist sources and the aim-to-wrist options. No separate tracker process is +needed. + +The plugin finds the gloves through the SDK by itself, at most one left and one +right, with nothing to declare. When both are plugged in, it opens them in one +two-hand SDK kit with time sync on, so the two hands' samples come from the same +USB frame. A glove that is plugged in alone opens on its own; when the other one +arrives, it too opens on its own first and then both are reopened together. +Gloves that cannot be opened together (no time sync, different products, ...) +keep streaming on their own and are not tried together again until either stream +ends. Until a glove is found, the plugin looks for it every 2 s and reports its +`/glove` status with the SDK's reason, without holding up the other hand. The +SDK reconnects a glove after a short dropout; when its stream ends anyway, the +plugin closes it, together with its partner when the two share a kit, and looks +for them again. A glove of the wrong product or variant, with no hand model, or +with incompatible firmware, is given up and reported as failed; an invalid SDK +configuration stops the plugin. Calibration stays in the SDK. Do not run the +plugin while the SDK viewer holds a glove's serial port. + +SDK capture gates are reported as degraded on the glove output. Cached samples +do not override these statuses. + +Each hand is placed with the host wrist pose at its glove sample time. Without a +tracked wrist the hand is still injected, without TRACKED flags, at the last valid +wrist pose or at the stage origin if there is none, and the `/hand` status is +degraded. Glove stalls remove the hand; the next glove sample restores it. + +The plugin adds the OpenXR wrist-axis convention: WRIST is `palm_link` turned +by the reviewed WRIST rotation, with its origin moved 12.6 mm along palm_link +X, +from the back of the hand towards the palm (`kWristPadM` in `openxr_model.inc`). +That is the depth of PALM in both models; at zero flexion the middle and ring +proximal joints are within 0.5 mm of it, while the tilted thumb anchor plays no +part. Both hands use the same depth. All 26 poses and the taxel layout are +relative to this WRIST. + +The `palm_link` origin is a reference point of the glove's mechanical model on +the back of the hand, near the end of the forearm, and the same point for both +models, so WRIST is a model point, not the anatomical wrist centre that OpenXR's +WRIST stands for. Nominally the wrist centre is about 4 cm further towards the +fingers and about 0.2 to 0.7 cm further towards the palm; these figures come +from typical adult hand dimensions, not from measurements on the glove, and +vary with the wearer. The plugin places the glove's WRIST at the host WRIST. +When the host wrist comes from optical hand tracking, it is the anatomical +wrist, so the injected hand sits that much further towards the fingers and the +palm than the tracked hand. With a controller, set `--aim-to-wrist-left` and +`--aim-to-wrist-right` to end at the glove's WRIST. Consumers that need the +anatomical wrist, for example to attach a forearm, must apply their own offset. +Verify the alignment before using the output for control. + +## Tests + +With `BUILD_TESTING=ON`, the tests build together with the plugin, so without +the C SDK they are skipped with it. Each library has its own suite under +`tests/cpp/plugins/revohuman/`: + +- `core/` (`revohuman_core_tests`): the sample pipeline, tactile serialization + and skinning, device status, event log and pose math. +- `live/` (`revohuman_live_tests`): the live source against a fake of the C SDK + library, compiled against the SDK's header, without gloves. +- `integration/` (`revohuman_integration_tests`): both hands through the SDK + points and the reviewed mapping, DIP-driven tips, the proximal joints rigid on + the MCP link, each product's hand model, the P model's poses without taxels, + the per-hand status outputs in the manifest, and the TactileMetadata built + from the SDK taxel layout, at the bind pose and under finger bends against the + SDK's own taxels. Two more ctest cases check how the plugin refuses the wrist + source variable and flag. + +The integration suite and the two plugin checks need the SDK library; with only +its header, the core and live suites still build. Run them all with +`ctest --test-dir build-revohuman/tests/cpp/plugins/revohuman --output-on-failure`. + +## Tactile layout + +At start-up the plugin builds each hand's TactileMetadata from the C SDK's +taxel layout for the DV1 model (`rh_hand_model_taxel_layout`, C SDK 0.1.3 or +later, layout version 1); there is no layout file or option. The 26 joints above +are bound where the reviewed mapping places them at zero joint angles, in the +WRIST basis. Each taxel is bound rigidly (one segment, weight 1) to the OpenXR +joint its SDK link moves with, with its +Z along the outward normal; a layout +with a taxel on a link no joint follows is refused. The log names each hand's +layout source, taxel count, metadata size and SHA-256. When the SDK has no +layout, the plugin logs why and pushes no tactile; the other outputs are +unaffected. + +Reviewed XR frames are not mechanical CAD link frames, but every joint a taxel +is bound to is rigid on that taxel's SDK link. So the skinned taxels are the +SDK's at the bind pose and stay on them as the fingers bend; the integration +suite checks them against the SDK's own taxels to 1e-6 m and 1e-6 rad. + +The tactile pipeline requires calibrated values and places the SDK's reading, +unchanged, on the taxel-local positive Z axis, the outward normal. The value is +the normal force the skin exerts on what it touches; the press on the skin is +its negative. Consumers use the layout to rotate into other frames. There is no +measured tangential component. Encoder and wrist validity do not gate these +local forces. Consumers need valid, time-aligned hand poses to deform the layout +or rotate forces into a wrist/world frame; bind-pose preview is only an +approximation when those poses are unavailable. + +A tactile sample is pushed only when its sampling time is present, no later than +the processing clock and no more than 100 ms old, and its device time advanced +since the last pushed sample; repeated tactile ticks are suppressed. The status +names the reason a sample was not pushed: `invalid`, `uncalibrated`, +`count_mismatch`, `no_sample_time`, `stale_sample` or `nonmonotonic_sample`. An +invalid sample makes it unavailable at once, except an invalid re-send of the +last pushed sample, with the same device time, while that sample is no more than +100 ms old: it counts as a repeat. A rejected sample does not remove the forces +already pushed, so the force collection is not recreated when skipped and valid +samples alternate: the last forces keep their own sampling time and are removed +once older than 100 ms. A stream generation change, a new shape or a reset +removes them at once and clears the time anchors. Consumers must check the +tactile status as well as data presence. diff --git a/src/plugins/revohuman/app/CMakeLists.txt b/src/plugins/revohuman/app/CMakeLists.txt new file mode 100644 index 0000000000..677aea54cc --- /dev/null +++ b/src/plugins/revohuman/app/CMakeLists.txt @@ -0,0 +1,57 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The plugin's only source is the gloves through the C SDK, so without a library the live source +# links against there is no plugin to build. +if(NOT REVOHUMAN_LIVE_AVAILABLE) + add_custom_target(revohuman_plugin + COMMAND ${CMAKE_COMMAND} -E echo "Skipping revohuman_plugin: no RevoHuman C SDK library the live source links against") + return() +endif() + +add_executable(revohuman_plugin + main.cpp + revohuman_plugin.cpp + revohuman_plugin.hpp + live_factory.cpp +) + +target_link_libraries(revohuman_plugin PRIVATE + revohuman_plugin::revohuman_live + revohuman_plugin::revohuman_core + Teleop::plugin_utils + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema + log_bridge::log_bridge_core + # Device I/O owns the SDK dependency; do not obtain it through the geometry library. + revohuman_plugin::revohuman_hand_kinematics + revohuman_plugin::sdk +) + +# The SDK library is installed next to the plugin, so the plugin looks there first. +# BUILD_RPATH adds $ORIGIN to CMake's default build-tree RPATH, which keeps the absolute SDK lib +# directory, so the build-tree binary runs in place. +set_target_properties(revohuman_plugin PROPERTIES + BUILD_RPATH "$ORIGIN" + INSTALL_RPATH "$ORIGIN" +) + +install(TARGETS revohuman_plugin RUNTIME DESTINATION plugins/revohuman COMPONENT revohuman) + +# Every name of the shared library (librevohuman.so -> .so. -> .so.); +# install(FILES) keeps symlinks as symlinks. The static library is not installed. +get_filename_component(_revohuman_sdk_lib_dir "${REVOHUMAN_SDK_LIBRARY}" DIRECTORY) +file(GLOB _revohuman_sdk_shared_libs LIST_DIRECTORIES false "${_revohuman_sdk_lib_dir}/librevohuman.so*") +install(FILES ${_revohuman_sdk_shared_libs} DESTINATION plugins/revohuman COMPONENT revohuman) +# The SDK licence goes next to its library: the library is redistributed with the plugin, so a +# prefix without it (such as an old source build) is refused rather than shipped unlicensed. +if(NOT EXISTS "${REVOHUMAN_SDK_ROOT}/LICENSE") + message(FATAL_ERROR "RevoHuman C SDK at ${REVOHUMAN_SDK_ROOT} has no LICENSE; its library cannot be " + "installed without it. Use a release package (install.sh downloads one).") +endif() +install(FILES "${REVOHUMAN_SDK_ROOT}/LICENSE" + DESTINATION plugins/revohuman + COMPONENT revohuman + RENAME LICENSE.revohuman-sdk-c +) diff --git a/src/plugins/revohuman/app/live_factory.cpp b/src/plugins/revohuman/app/live_factory.cpp new file mode 100644 index 0000000000..3bf7276c84 --- /dev/null +++ b/src/plugins/revohuman/app/live_factory.cpp @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Built when the RevoHuman C SDK library was found. + +#include "revohuman_plugin.hpp" + +#include +#include + +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +namespace +{ + +//! Each glove gets the hand model the SDK gives its identified product. +KinematicsFactory kinematics() +{ + return [](const std::string& product_code) + { + // An SDK whose model differs from the reviewed one makes make_hand_kinematics throw; the caller + // (LiveSource::open_hand) catches it and gives up on just this glove, logging the reason at error level. + return make_hand_kinematics(hand_model_of_product(product_code)); + }; +} + +} // namespace + +std::optional live_source_unavailable_reason(const LiveSourceConfig&) +{ + // Neither the SDK hand points nor the hand model has a fallback: a build or SDK without either refuses + // to start. The model of each glove is known only once it is open, so DV1 stands in here. + try + { + make_hand_kinematics(HandModel::Dv1); + } + catch (const std::exception& e) + { + return e.what(); + } + if (!sdk_has_product_hand_models()) + return "this build's C SDK cannot tell the hand model of a glove's product (no rh_variant_hand_model); " + "rebuild with -DREVOHUMAN_SDK_ROOT pointing at a C SDK that has it"; + return std::nullopt; +} + +std::unique_ptr make_live_source(const LiveSourceConfig& config) +{ + return std::make_unique(config, kinematics()); +} + +TactileShape make_tactile_shape(Hand hand) +{ + // Built before any glove is open. Only DV1 has a taxel layout: a P1 glove has no tactile sensors, and the + // pipeline pushes this metadata only once a glove with tactile sensors is identified. + auto built = make_sdk_tactile_shape(HandModel::Dv1, hand); + return { std::make_shared(std::move(built.metadata)), "C SDK taxel layout " + built.layout_source }; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/app/main.cpp b/src/plugins/revohuman/app/main.cpp new file mode 100644 index 0000000000..fd42162eea --- /dev/null +++ b/src/plugins/revohuman/app/main.cpp @@ -0,0 +1,205 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "revohuman_plugin.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::revohuman; + +namespace +{ + +constexpr std::string_view kUsage = + "Usage: revohuman_plugin\n" + " [--collection-prefix=NAME] [--event-log=FILE]\n" + " [--wrist-source=auto|hand_tracking|controller]\n" + " [--aim-to-wrist-left=px,py,pz,qx,qy,qz,qw] [--aim-to-wrist-right=px,py,pz,qx,qy,qz,qw]\n" + " [--stall-ms=N] [--plugin-root-id=ID]"; + +volatile std::sig_atomic_t g_stop = 0; + +void on_signal(int) +{ + g_stop = 1; +} + +// The value of `--name=value` in @p arg, or nullopt when @p arg is another flag. +std::optional flag_value(std::string_view arg, std::string_view name) +{ + if (arg.starts_with(name) && arg.size() > name.size() && arg[name.size()] == '=') + return std::string(arg.substr(name.size() + 1)); + return std::nullopt; +} + +// The wrist source named @p value; @p name is the flag or variable it was given as. +plugin_utils::WristSourceMode wrist_source_mode(const std::string& value, std::string_view name) +{ + if (value == "auto") + return plugin_utils::WristSourceMode::Auto; + if (value == "hand_tracking") + return plugin_utils::WristSourceMode::HandTracking; + if (value == "controller") + return plugin_utils::WristSourceMode::Controller; + throw std::invalid_argument(std::string(name) + " must be auto, hand_tracking or controller, got '" + value + "'"); +} + +std::int64_t ms_to_ns(const std::string& value, std::string_view flag) +{ + std::size_t used = 0; + long long ms = -1; + try + { + ms = std::stoll(value, &used); + } + catch (const std::exception&) + { + used = 0; + } + if (used != value.size() || ms <= 0) + { + throw std::invalid_argument(std::string(flag) + " needs a positive number of milliseconds, got '" + value + "'"); + } + if (ms > std::numeric_limits::max() / 1'000'000) + { + throw std::invalid_argument(std::string(flag) + " is too large, got '" + value + "'"); + } + return static_cast(ms) * 1'000'000; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + auto logger = isaaccapture::Logger::get("isaaccapture.plugins.revohuman.main"); + + RevoHumanPluginConfig config; + std::array aim_to_wrist_given{}; + std::optional wrist_source_flag; + for (int i = 1; i < argc; ++i) + { + const std::string_view arg = argv[i]; + if (arg == "--help" || arg == "-h") + { + // Usage text is terminal UX, not a diagnostic. + std::cout << kUsage << std::endl; + return 0; + } + if (auto value = flag_value(arg, "--collection-prefix")) + config.collection_prefix = *value; + else if (auto value = flag_value(arg, "--event-log")) + config.event_log_path = *value; + else if (auto value = flag_value(arg, "--wrist-source")) + { + config.wrist_source = wrist_source_mode(*value, "--wrist-source"); + wrist_source_flag = *value; + } + else if (auto value = flag_value(arg, "--aim-to-wrist-left")) + { + config.aim_to_wrist[static_cast(Hand::Left)] = parse_pose(*value, "--aim-to-wrist-left"); + aim_to_wrist_given[static_cast(Hand::Left)] = true; + } + else if (auto value = flag_value(arg, "--aim-to-wrist-right")) + { + config.aim_to_wrist[static_cast(Hand::Right)] = parse_pose(*value, "--aim-to-wrist-right"); + aim_to_wrist_given[static_cast(Hand::Right)] = true; + } + else if (auto value = flag_value(arg, "--stall-ms")) + config.pipeline.stall_ns = ms_to_ns(*value, "--stall-ms"); + else if (auto value = flag_value(arg, "--plugin-root-id")) + config.plugin_root_id = *value; + else + logger->warn("ignoring unknown argument '{}'", arg); + } + if (config.collection_prefix.empty()) + throw std::invalid_argument("--collection-prefix must not be empty"); + // The hand capture labels a take with this variable, so a flag that says otherwise is refused + // rather than recorded under the wrong source. + if (const char* env = std::getenv("REVOHUMAN_WRIST_SOURCE"); env != nullptr && *env != '\0') + { + const std::string value(env); + if (wrist_source_flag && *wrist_source_flag != value) + throw std::invalid_argument("--wrist-source=" + *wrist_source_flag + + " disagrees with REVOHUMAN_WRIST_SOURCE=" + value); + config.wrist_source = wrist_source_mode(value, "REVOHUMAN_WRIST_SOURCE"); + } + // Before the OpenXR session is created, so a build whose SDK lacks the hand points or the product + // hand models fails with this reason. + if (const auto reason = live_source_unavailable_reason(config.live)) + throw std::invalid_argument(*reason); + + // The offset applies only while a hand's wrist comes from a controller: every frame with + // `controller`, and with `auto` only when optical tracking has no pose for that hand. + if (config.wrist_source != plugin_utils::WristSourceMode::HandTracking) + { + const char* when = config.wrist_source == plugin_utils::WristSourceMode::Controller ? + "WRIST is" : + "when optical tracking has no pose for this hand and a controller does, WRIST is"; + for (std::size_t i = 0; i < aim_to_wrist_given.size(); ++i) + if (!aim_to_wrist_given[i]) + logger->warn( + "no --aim-to-wrist-{}: {} the controller aim pose with no offset", i == 0 ? "left" : "right", when); + } + + logger->info("RevoHuman (collections: {}__)", config.collection_prefix); + + std::signal(SIGINT, on_signal); + std::signal(SIGTERM, on_signal); + + RevoHumanPlugin plugin(std::move(config)); + + // Glove samples come at 200 Hz, so tick every millisecond and + // report device status at 100 Hz. + const auto tick = std::chrono::milliseconds(1); + const auto program_start = std::chrono::steady_clock::now(); + std::size_t tick_count = 0; + bool reported_finish = false; + + while (!g_stop) + { + const std::int64_t now_ns = core::os_monotonic_now_ns(); + plugin.update(now_ns); + if (tick_count % 10 == 0) + plugin.publish_status(core::os_monotonic_now_ns()); + if (plugin.finished() && !reported_finish) + { + // Keep the session up so status shows the gloves as gone rather than the plugin as dead. + logger->info("source finished; waiting for a stop signal"); + reported_finish = true; + } + tick_count++; + std::this_thread::sleep_until(program_start + tick * tick_count); + } + + logger->info("stopping"); + return 0; +} +catch (const std::exception& e) +{ + auto logger = isaaccapture::Logger::get("isaaccapture.plugins.revohuman.main"); + logger->error("{}: {}", argv[0], e.what()); + return 1; +} +catch (...) +{ + auto logger = isaaccapture::Logger::get("isaaccapture.plugins.revohuman.main"); + logger->error("{}: Unknown error", argv[0]); + return 1; +} diff --git a/src/plugins/revohuman/app/revohuman_plugin.cpp b/src/plugins/revohuman/app/revohuman_plugin.cpp new file mode 100644 index 0000000000..32cfc2aebe --- /dev/null +++ b/src/plugins/revohuman/app/revohuman_plugin.cpp @@ -0,0 +1,399 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "revohuman_plugin.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +namespace +{ + +constexpr std::array kHands = { Hand::Left, Hand::Right }; + +std::size_t index_of(Hand hand) +{ + return static_cast(hand); +} + +std::shared_ptr plugin_logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.revohuman.RevoHumanPlugin"); +} + +void add_unique(std::vector& extensions, const std::vector& more) +{ + for (const auto& extension : more) + { + if (std::find(extensions.begin(), extensions.end(), extension) == extensions.end()) + { + extensions.push_back(extension); + } + } +} + +std::vector required_extensions(bool monitoring, const plugin_utils::WristPoseSource::Requirements& wrist) +{ + std::vector extensions = core::DeviceIOSession::get_required_extensions(wrist.trackers); + add_unique(extensions, core::SchemaPusher::get_required_extensions()); + for (const char* extension : wrist.extensions) + { + add_unique(extensions, { extension }); + } + add_unique(extensions, core::XrTimeConverter::get_required_extensions()); + add_unique(extensions, { XR_NVX1_DEVICE_INTERFACE_BASE_EXTENSION_NAME }); + if (monitoring) + { + add_unique(extensions, plugin_utils::PluginDeviceStatusPublisher::get_required_extensions()); + } + return extensions; +} + +std::shared_ptr make_session(const RevoHumanPluginConfig& config, + const plugin_utils::WristPoseSource::Requirements& wrist) +{ + if (config.plugin_root_id && config.plugin_root_id->empty()) + { + throw std::invalid_argument("RevoHumanPlugin: --plugin-root-id must not be empty"); + } + return std::make_shared( + "RevoHumanPlugin", required_extensions(config.plugin_root_id.has_value(), wrist)); +} + +core::SchemaPusherConfig pusher_config(const std::string& collection_id, + std::size_t max_bytes, + const char* tensor_identifier, + const std::string& localized_name) +{ + return core::SchemaPusherConfig{ .collection_id = collection_id, + .max_flatbuffer_size = max_bytes, + .tensor_identifier = tensor_identifier, + .localized_name = localized_name, + .app_name = "RevoHumanPlugin" }; +} + +std::string side_name(Hand hand) +{ + return hand == Hand::Left ? "left" : "right"; +} + +plugin_utils::PluginDeviceStatusEntry entry(std::string path, + core::PluginDeviceState state, + core::PluginDeviceReason reason, + std::string error = {}) +{ + return plugin_utils::PluginDeviceStatusEntry{ + .path = std::move(path), .state = state, .reason = reason, .error = std::move(error) + }; +} + +} // namespace + +//! One hand's OpenXR outputs: three tensor collections and an injected hand. The hand and the two tactile +//! collections exist only while the pipeline says so. +class RevoHumanPlugin::HandOutput : public PushSink +{ +public: + HandOutput(const core::OpenXRSessionHandles& handles, + const core::XrTimeConverter& time_converter, + Hand hand, + const std::string& prefix, + std::size_t metadata_size) + : handles_(handles), + time_converter_(time_converter), + hand_(hand), + joint_state_(handles, + pusher_config(prefix + "_" + side_name(hand) + "_joint_state", + kJointStateMaxBytes, + "joint_state", + "RevoHuman " + side_name(hand) + " joint state")), + tactile_config_(pusher_config(prefix + "_" + side_name(hand) + "_tactile", + kTactileMaxBytes, + "tactile", + "RevoHuman " + side_name(hand) + " tactile")), + // The reader cap is an upper bound, not the size of every metadata tensor. + shape_config_(pusher_config(core::tracker_metadata_collection_id(prefix + "_" + side_name(hand) + "_tactile"), + metadata_size, + core::TRACKER_METADATA_TENSOR_IDENTIFIER.data(), + "RevoHuman " + side_name(hand) + " tactile metadata")) + { + } + + void push_joint_state(const std::vector& buffer, std::int64_t sample_ns, std::int64_t raw_ns) override + { + joint_state_.push_buffer(buffer.data(), buffer.size(), sample_ns, raw_ns); + } + void push_tactile(const std::vector& buffer, std::int64_t sample_ns, std::int64_t raw_ns) override + { + if (!tactile_) + tactile_ = std::make_unique(handles_, tactile_config_); + tactile_->push_buffer(buffer.data(), buffer.size(), sample_ns, raw_ns); + } + void clear_tactile() override + { + tactile_.reset(); + } + void push_shape(const std::vector& buffer, std::int64_t now_ns) override + { + if (!shape_config_.max_flatbuffer_size) + throw std::logic_error("metadata output was not configured"); + // Created with the first metadata, so a hand that never has a tactile glove has no tactile collections. + if (!shape_) + shape_ = std::make_unique(handles_, shape_config_); + shape_->push_buffer(buffer.data(), buffer.size(), now_ns, now_ns); + } + void clear_shape() override + { + shape_.reset(); + } + void hand_create() override + { + injector_ = std::make_unique( + handles_.instance, handles_.session, hand_ == Hand::Left ? XR_HAND_LEFT_EXT : XR_HAND_RIGHT_EXT, + handles_.space); + } + void hand_destroy() override + { + // Destroying the injector reports isActive = false to readers. + injector_.reset(); + } + void hand_push(const HandFrame& frame) override + { + if (!injector_) + { + return; + } + std::array joints{}; + for (std::size_t i = 0; i < joints.size(); ++i) + { + const Pose& pose = frame.joints[i]; + const bool position_valid = (frame.position_valid_mask & (1u << i)) != 0; + const bool orientation_valid = (frame.orientation_valid_mask & (1u << i)) != 0; + joints[i].locationFlags = + (position_valid ? XR_SPACE_LOCATION_POSITION_VALID_BIT : 0) | + (orientation_valid ? XR_SPACE_LOCATION_ORIENTATION_VALID_BIT : 0) | + (position_valid && frame.tracked ? XR_SPACE_LOCATION_POSITION_TRACKED_BIT : 0) | + (orientation_valid && frame.tracked ? XR_SPACE_LOCATION_ORIENTATION_TRACKED_BIT : 0); + joints[i].pose.position = XrVector3f{ static_cast(pose.p[0]), static_cast(pose.p[1]), + static_cast(pose.p[2]) }; + joints[i].pose.orientation = XrQuaternionf{ static_cast(pose.q[0]), static_cast(pose.q[1]), + static_cast(pose.q[2]), static_cast(pose.q[3]) }; + joints[i].radius = frame.radius[i]; + } + // Stamped with the glove sampling time, not the push time. + injector_->push(joints.data(), time_converter_.convert_monotonic_ns_to_xrtime(frame.sample_mono_ns)); + } + +private: + core::OpenXRSessionHandles handles_; + const core::XrTimeConverter& time_converter_; + Hand hand_; + core::SchemaPusher joint_state_; + core::SchemaPusherConfig tactile_config_; + core::SchemaPusherConfig shape_config_; + std::unique_ptr tactile_; + std::unique_ptr shape_; + std::unique_ptr injector_; +}; + +XrPosef to_xr(const Pose& pose) +{ + return XrPosef{ { static_cast(pose.q[0]), static_cast(pose.q[1]), static_cast(pose.q[2]), + static_cast(pose.q[3]) }, + { static_cast(pose.p[0]), static_cast(pose.p[1]), static_cast(pose.p[2]) } }; +} + +plugin_utils::WristSourceConfig wrist_config(const RevoHumanPluginConfig& plugin) +{ + plugin_utils::WristSourceConfig config; + config.mode = plugin.wrist_source; + config.left_aim_to_wrist = to_xr(plugin.aim_to_wrist[index_of(Hand::Left)]); + config.right_aim_to_wrist = to_xr(plugin.aim_to_wrist[index_of(Hand::Right)]); + return config; +} + +//! The host's wrist pose (WristPoseSource) at a glove sample time, in the stage. +class RevoHumanPlugin::Wrists : public WristProvider +{ +public: + Wrists(const plugin_utils::WristSourceConfig& config, + const core::OpenXRSessionHandles& handles, + core::DeviceIOSession* deviceio, + std::shared_ptr controller_tracker, + const core::XrTimeConverter& time_converter) + : source_(config, handles, deviceio, std::move(controller_tracker)), time_converter_(time_converter) + { + } + + WristPose wrist(Hand hand, std::int64_t sample_mono_ns) override + { + const plugin_utils::WristSample sample = + source_.query(hand == Hand::Left, time_converter_.convert_monotonic_ns_to_xrtime(sample_mono_ns)); + WristPose out; + out.valid = sample.valid; + out.tracked = sample.tracked; + out.pose_stage.p = { sample.pose.position.x, sample.pose.position.y, sample.pose.position.z }; + out.pose_stage.q = { sample.pose.orientation.x, sample.pose.orientation.y, sample.pose.orientation.z, + sample.pose.orientation.w }; + return out; + } + +private: + plugin_utils::WristPoseSource source_; + const core::XrTimeConverter& time_converter_; +}; + +RevoHumanPlugin::RevoHumanPlugin(RevoHumanPluginConfig config) + : config_(std::move(config)), + wrist_config_(wrist_config(config_)), + wrist_requirements_(plugin_utils::WristPoseSource::collect_requirements(wrist_config_.mode)), + session_(make_session(config_, wrist_requirements_)), + time_converter_(std::make_unique(session_->get_handles())), + event_log_(config_.event_log_path) +{ + auto logger = plugin_logger(); + const auto handles = session_->get_handles(); + deviceio_ = core::DeviceIOSession::run(wrist_requirements_.trackers, handles); + wrists_ = std::make_unique( + wrist_config_, handles, deviceio_.get(), std::move(wrist_requirements_.controller_tracker), *time_converter_); + EventLog* log = event_log_.enabled() ? &event_log_ : nullptr; + for (const Hand hand : kHands) + { + std::shared_ptr shape; + try + { + auto made = make_tactile_shape(hand); + logger->info("{}: TactileMetadata from the {} ({} taxels, {} bytes, sha256 {})", hand_name(hand), + made.source, made.shape->taxel_count(), made.shape->bytes().size(), made.shape->sha256()); + shape = std::move(made.shape); + } + catch (const std::exception& e) + { + logger->warn("{}: no TactileMetadata ({}); tactile is not pushed", hand_name(hand), e.what()); + } + auto& output = outputs_[index_of(hand)]; + output = std::make_unique( + handles, *time_converter_, hand, config_.collection_prefix, shape ? shape->bytes().size() : 0); + auto& pipeline = pipelines_[index_of(hand)]; + pipeline = std::make_unique(hand, config_.pipeline, *output, log); + pipeline->set_wrist_provider(wrists_.get()); + if (shape) + pipeline->set_shape(std::move(shape)); + } + source_ = make_live_source(config_.live); + logger->info("live gloves"); + if (config_.plugin_root_id) + { + status_ = std::make_unique(handles, *config_.plugin_root_id); + } +} + +RevoHumanPlugin::~RevoHumanPlugin() +{ + // Remove the hands before the session goes away. + pipelines_ = {}; + outputs_ = {}; + wrists_.reset(); + deviceio_.reset(); +} + +void RevoHumanPlugin::update(std::int64_t now_ns) +{ + // WristPoseSource reads the controllers through this session, on this thread. + deviceio_->update(); + if (source_ && !finished_) + { + SourceBatch batch = source_->poll(now_ns); + // SDK discovery and FK can take time; classify timestamps against the clock after receiving. + now_ns = core::os_monotonic_now_ns(); + for (const Hand hand : kHands) + { + // A glove opened again starts over: new ticks, maybe the same generation. + auto& pipeline = pipelines_[index_of(hand)]; + if (batch.hand_restarted[index_of(hand)] && pipeline) + { + pipeline->reset("glove_reopened"); + } + } + for (const SourceRecord& record : batch.records) + { + std::visit( + [&](const auto& r) + { + auto& pipeline = pipelines_[index_of(r.hand)]; + if (!pipeline) + { + return; + } + using T = std::decay_t; + if constexpr (std::is_same_v) + { + pipeline->set_meta(r); + } + else if constexpr (std::is_same_v) + { + pipeline->on_glove(r, now_ns); + } + }, + record); + } + if (batch.finished) + { + finished_ = true; + plugin_logger()->info("live source finished after {} records", source_->records_released()); + } + } + for (auto& pipeline : pipelines_) + { + if (pipeline) + { + pipeline->on_tick(now_ns); + } + } +} + +void RevoHumanPlugin::publish_status(std::int64_t now_ns) +{ + if (!status_) + { + return; + } + std::vector entries; + for (const Hand hand : kHands) + { + HandStatusInput input; + const auto& pipeline = pipelines_[index_of(hand)]; + if (pipeline) + { + input.health = pipeline->health(now_ns); + input.note = source_->glove_note(hand); + } + for (DeviceStatusRow& row : make_hand_status(hand, input)) + { + entries.push_back(entry(std::move(row.path), row.state, row.reason, std::move(row.error))); + } + } + status_->publish_if_changed(entries, now_ns); +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/app/revohuman_plugin.hpp b/src/plugins/revohuman/app/revohuman_plugin.hpp new file mode 100644 index 0000000000..0abfc8897f --- /dev/null +++ b/src/plugins/revohuman/app/revohuman_plugin.hpp @@ -0,0 +1,109 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace core +{ +class OpenXRSession; +class XrTimeConverter; +} // namespace core + +namespace plugins +{ +namespace revohuman +{ + +struct RevoHumanPluginConfig +{ + //! Gloves through the RevoHuman C SDK. It finds the plugged-in gloves by itself, at most one per + //! hand. + LiveSourceConfig live; + //! Collections are __, e.g. revohuman_left_tactile. + std::string collection_prefix = "revohuman"; + //! JSON-lines audit log of the run; disabled when empty. + std::string event_log_path; + PipelineConfig pipeline; + //! Where the WRIST pose comes from: optical hand tracking, a controller, or both (Auto). + plugin_utils::WristSourceMode wrist_source = plugin_utils::WristSourceMode::Auto; + //! Controller aim pose to WRIST, indexed by Hand. Identity until a controller mount for the + //! glove is calibrated. + std::array aim_to_wrist{}; + //! Set by the plugin launcher; enables device status reporting for monitoring. + std::optional plugin_root_id; +}; + +/*! + * @brief Pushes RevoHuman gloves into OpenXR: per hand an injected OpenXR hand (wrist from the + * host's WristPoseSource), the encoder joint states, and the taxel forces with their TactileMetadata. + */ +std::unique_ptr make_live_source(const LiveSourceConfig& config); +//! Why this build's C SDK cannot make a LiveSource for the config, or nullopt when it can. +std::optional live_source_unavailable_reason(const LiveSourceConfig& config); + +//! TactileMetadata of a glove and where it came from. +struct TactileShape +{ + std::shared_ptr shape; + std::string source; +}; +//! The TactileMetadata of @p hand, built from the RevoHuman C SDK's taxel layout; throws +//! std::runtime_error with the reason when the SDK has none (tactile is then not pushed). +TactileShape make_tactile_shape(Hand hand); + +class RevoHumanPlugin +{ +public: + explicit RevoHumanPlugin(RevoHumanPluginConfig config); + ~RevoHumanPlugin(); + + RevoHumanPlugin(const RevoHumanPlugin&) = delete; + RevoHumanPlugin& operator=(const RevoHumanPlugin&) = delete; + + //! Feeds what is due at now_ns and ticks both hands. + void update(std::int64_t now_ns); + //! Device status for monitoring; call at least once a second. + void publish_status(std::int64_t now_ns); + //! The source gives no more samples: both gloves were given up. + bool finished() const + { + return finished_; + } + +private: + class HandOutput; + + RevoHumanPluginConfig config_; + class Wrists; + + plugin_utils::WristSourceConfig wrist_config_; + plugin_utils::WristPoseSource::Requirements wrist_requirements_; + std::shared_ptr session_; + std::unique_ptr time_converter_; + std::unique_ptr deviceio_; + std::unique_ptr wrists_; + EventLog event_log_; + std::array, 2> outputs_; + std::array, 2> pipelines_; + std::unique_ptr source_; + std::unique_ptr status_; + bool finished_ = false; +}; + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/CMakeLists.txt b/src/plugins/revohuman/core/CMakeLists.txt new file mode 100644 index 0000000000..80580a2bd1 --- /dev/null +++ b/src/plugins/revohuman/core/CMakeLists.txt @@ -0,0 +1,41 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Pose math, clocks, wrist validity, shape checks, serialization and the per-hand push pipeline, +# kept apart from OpenXR so they can be unit-tested without a runtime or a device. +add_library(revohuman_core STATIC + device_clock.cpp + inc/revohuman_plugin/device_clock.hpp + device_status.cpp + inc/revohuman_plugin/device_status.hpp + event_log.cpp + inc/revohuman_plugin/event_log.hpp + pipeline.cpp + inc/revohuman_plugin/pipeline.hpp + pose_math.cpp + inc/revohuman_plugin/pose_math.hpp + serialize.cpp + inc/revohuman_plugin/serialize.hpp + inc/revohuman_plugin/source.hpp + sha256.cpp + inc/revohuman_plugin/sha256.hpp + tactile.cpp + inc/revohuman_plugin/tactile.hpp + types.cpp + inc/revohuman_plugin/types.hpp + wrist_tracking.cpp + inc/revohuman_plugin/wrist_tracking.hpp +) + +target_include_directories(revohuman_core + INTERFACE + $ +) + +target_link_libraries(revohuman_core + PUBLIC + isaacteleop_schema + log_bridge::log_bridge_core +) + +add_library(revohuman_plugin::revohuman_core ALIAS revohuman_core) diff --git a/src/plugins/revohuman/core/device_clock.cpp b/src/plugins/revohuman/core/device_clock.cpp new file mode 100644 index 0000000000..4b6f9f24bd --- /dev/null +++ b/src/plugins/revohuman/core/device_clock.cpp @@ -0,0 +1,39 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/device_clock.hpp" + +namespace plugins +{ +namespace revohuman +{ + +std::int64_t TickUnwrapper::unwrap(std::uint32_t raw) +{ + if (!anchored_) + { + anchored_ = true; + last_ = raw; + extended_ = static_cast(raw); + return extended_; + } + const auto delta = static_cast(raw - last_); + last_ = raw; + extended_ += delta; + return extended_; +} + +void TickUnwrapper::reset() +{ + anchored_ = false; + last_ = 0; + extended_ = 0; +} + +std::int64_t ms_ticks_to_ns(std::int64_t ticks) +{ + return ticks * 1'000'000; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/device_status.cpp b/src/plugins/revohuman/core/device_status.cpp new file mode 100644 index 0000000000..06f40508e7 --- /dev/null +++ b/src/plugins/revohuman/core/device_status.cpp @@ -0,0 +1,144 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/device_status.hpp" + +#include + +namespace plugins +{ +namespace revohuman +{ + +namespace +{ + +using core::PluginDeviceReason; +using core::PluginDeviceReason_CALIBRATION_FAILED; +using core::PluginDeviceReason_DEVICE_ERROR; +using core::PluginDeviceReason_DISABLED_BY_CONFIGURATION; +using core::PluginDeviceReason_NO_CURRENT_DATA; +using core::PluginDeviceReason_NO_HARDWARE_SIGNAL; +using core::PluginDeviceReason_NONE; +using core::PluginDeviceState; +using core::PluginDeviceState_CONNECTED; +using core::PluginDeviceState_DEGRADED; +using core::PluginDeviceState_DISABLED; +using core::PluginDeviceState_DISCONNECTED; +using core::PluginDeviceState_FAILED; +using core::PluginDeviceState_UNKNOWN; + +DeviceStatusRow row(std::string path, PluginDeviceState state, PluginDeviceReason reason, std::string error = {}) +{ + return DeviceStatusRow{ .path = std::move(path), .state = state, .reason = reason, .error = std::move(error) }; +} + +//! A device that has given data before and stopped is DISCONNECTED; one that never did is UNKNOWN. +PluginDeviceState lost_or_unknown(bool seen) +{ + return seen ? PluginDeviceState_DISCONNECTED : PluginDeviceState_UNKNOWN; +} + +DeviceStatusRow hand_row(std::string path, const HandHealth& h) +{ + if (h.hand_active && h.wrist_tracked) + { + return row(std::move(path), PluginDeviceState_CONNECTED, PluginDeviceReason_NONE); + } + if (h.hand_active) + { + return row(std::move(path), PluginDeviceState_DEGRADED, PluginDeviceReason_NO_CURRENT_DATA, + "no tracked wrist pose from the host"); + } + if (h.kinematics_missing) + { + return row(std::move(path), PluginDeviceState_DEGRADED, PluginDeviceReason_NO_CURRENT_DATA, + "glove samples carry no joint poses"); + } + return row(std::move(path), lost_or_unknown(h.glove_seen), PluginDeviceReason_NO_HARDWARE_SIGNAL); +} + +DeviceStatusRow glove_row(std::string path, const HandHealth& h, const std::optional& note) +{ + // What the source says about the glove overrides a recently cached healthy sample. + if (note) + { + switch (note->kind) + { + case Source::GloveNote::Kind::Ended: + return row(std::move(path), PluginDeviceState_FAILED, PluginDeviceReason_DEVICE_ERROR, note->text); + case Source::GloveNote::Kind::Waiting: + // A glove that streamed before and is looked for again was lost, not never there. + return row(std::move(path), lost_or_unknown(h.glove_seen), PluginDeviceReason_NO_HARDWARE_SIGNAL, note->text); + case Source::GloveNote::Kind::NotReady: + return row(std::move(path), h.glove_seen ? PluginDeviceState_DEGRADED : PluginDeviceState_UNKNOWN, + PluginDeviceReason_NO_CURRENT_DATA, note->text); + } + } + if (h.glove_live) + { + return row(std::move(path), PluginDeviceState_CONNECTED, PluginDeviceReason_NONE); + } + if (!h.glove_seen) + { + return row(std::move(path), PluginDeviceState_UNKNOWN, PluginDeviceReason_NO_HARDWARE_SIGNAL); + } + return row(std::move(path), PluginDeviceState_DISCONNECTED, PluginDeviceReason_NO_HARDWARE_SIGNAL, + "no recent glove sample"); +} + +DeviceStatusRow tactile_row(std::string path, const HandHealth& h) +{ + if (h.tactile_live) + { + return row(std::move(path), PluginDeviceState_CONNECTED, PluginDeviceReason_NONE); + } + switch (h.tactile_skip) + { + case TactileSkip::NoShape: + return row(std::move(path), PluginDeviceState_DISABLED, PluginDeviceReason_DISABLED_BY_CONFIGURATION, + "no TactileMetadata: the C SDK gave no taxel layout (see the start-up log)"); + case TactileSkip::NoTactile: + return row(std::move(path), PluginDeviceState_DISABLED, PluginDeviceReason_DISABLED_BY_CONFIGURATION, + "this glove has no tactile sensors"); + case TactileSkip::Uncalibrated: + if (h.glove_live) + { + return row(std::move(path), PluginDeviceState_DEGRADED, PluginDeviceReason_CALIBRATION_FAILED, + tactile_skip_name(h.tactile_skip)); + } + break; + case TactileSkip::CountMismatch: + if (h.glove_live) + { + return row(std::move(path), PluginDeviceState_FAILED, PluginDeviceReason_DEVICE_ERROR, + "taxel count differs from the TactileMetadata"); + } + break; + case TactileSkip::None: + break; + default: + if (h.glove_live) + { + return row(std::move(path), PluginDeviceState_DEGRADED, PluginDeviceReason_NO_CURRENT_DATA, + tactile_skip_name(h.tactile_skip)); + } + break; + } + return row(std::move(path), lost_or_unknown(h.tactile_seen), PluginDeviceReason_NO_HARDWARE_SIGNAL); +} + +} // namespace + +std::array make_hand_status(Hand hand, const HandStatusInput& input) +{ + const std::string side = hand == Hand::Left ? "left" : "right"; + std::string hand_path = "/hand/" + side; + std::string glove_path = "/glove/revohuman_" + side; + std::string tactile_path = "/tactile/revohuman_" + side; + return { hand_row(std::move(hand_path), input.health), glove_row(std::move(glove_path), input.health, input.note), + tactile_row(std::move(tactile_path), input.health) }; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/event_log.cpp b/src/plugins/revohuman/core/event_log.cpp new file mode 100644 index 0000000000..2dc4f93b5c --- /dev/null +++ b/src/plugins/revohuman/core/event_log.cpp @@ -0,0 +1,116 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/event_log.hpp" + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +EventLog::EventLog(const std::string& path) +{ + if (path.empty()) + { + return; + } + out_.open(path, std::ios::out | std::ios::app); + if (!out_) + { + throw std::runtime_error("event log: cannot open " + path); + } +} + +std::string EventLog::quote(const std::string& s) +{ + std::string out = "\""; + for (const unsigned char c : s) + { + switch (c) + { + case '"': + out += "\\\""; + break; + case '\\': + out += "\\\\"; + break; + case '\n': + out += "\\n"; + break; + case '\r': + out += "\\r"; + break; + case '\t': + out += "\\t"; + break; + default: + if (c < 0x20) + { + char buf[8]; + std::snprintf(buf, sizeof(buf), "\\u%04x", c); + out += buf; + } + else + { + out += static_cast(c); + } + } + } + return out + "\""; +} + +void EventLog::write(const char* event, std::initializer_list fields) +{ + if (!enabled()) + { + return; + } + std::ostringstream line; + line.precision(17); + line << "{\"event\":" << quote(event); + for (const auto& [key, value] : fields) + { + line << "," << quote(key) << ":"; + std::visit( + [&line](const auto& v) + { + using T = std::decay_t; + if constexpr (std::is_same_v) + { + line << quote(v); + } + else if constexpr (std::is_same_v) + { + line << (v ? "true" : "false"); + } + else if constexpr (std::is_same_v) + { + if (std::isfinite(v)) + { + line << v; + } + else + { + line << "null"; + } + } + else + { + line << v; + } + }, + value); + } + line << "}\n"; + out_ << line.str(); + out_.flush(); +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/device_clock.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/device_clock.hpp new file mode 100644 index 0000000000..01a0a7a68e --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/device_clock.hpp @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace plugins +{ +namespace revohuman +{ + +//! Unwraps a free-running uint32 device counter into a monotonic int64 by signed 32-bit deltas. +//! The first value anchors the output at the raw value itself, so the unwrapped count equals the +//! device count until the first wrap. A step backwards is kept as a step backwards. +class TickUnwrapper +{ +public: + std::int64_t unwrap(std::uint32_t raw); + void reset(); + bool anchored() const + { + return anchored_; + } + +private: + bool anchored_ = false; + std::uint32_t last_ = 0; + std::int64_t extended_ = 0; +}; + +//! Glove and tactile ticks are 1 ms. +std::int64_t ms_ticks_to_ns(std::int64_t ticks); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/device_status.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/device_status.hpp new file mode 100644 index 0000000000..e3fb6bc8ac --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/device_status.hpp @@ -0,0 +1,48 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "pipeline.hpp" +#include "source.hpp" +#include "types.hpp" + +#include + +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! One device path in plugin.yaml and what it reports. +struct DeviceStatusRow +{ + std::string path; + core::PluginDeviceState state = core::PluginDeviceState_UNKNOWN; + core::PluginDeviceReason reason = core::PluginDeviceReason_NONE; + std::string error; +}; + +//! What the plugin knows about one hand when it reports device status. +struct HandStatusInput +{ + HandHealth health; + std::optional note; //!< From the source, for a glove that gives no samples. +}; + +/*! + * @brief The /hand, /glove and /tactile rows of one hand, in that order. + * + * Only the state/reason pairs that TeleopSession accepts are used + * (teleop_session_manager/status_monitor.py). A device that has not given data yet is UNKNOWN, + * not DISCONNECTED, so a plugin started before its gloves are plugged in is not reported as having + * lost them. + */ +std::array make_hand_status(Hand hand, const HandStatusInput& input); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/event_log.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/event_log.hpp new file mode 100644 index 0000000000..3a2a1b9e11 --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/event_log.hpp @@ -0,0 +1,42 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! One JSON object per line, flushed per event, so a run can be audited offline +//! (pushed timestamps, injector transitions, shape hashes). Disabled when the path is empty. +class EventLog +{ +public: + using Value = std::variant; + using Field = std::pair; + + explicit EventLog(const std::string& path); + + bool enabled() const + { + return out_.is_open(); + } + void write(const char* event, std::initializer_list fields); + + //! JSON string literal with escapes, quotes included. + static std::string quote(const std::string& s); + +private: + std::ofstream out_; +}; + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/pipeline.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/pipeline.hpp new file mode 100644 index 0000000000..c6327e8ba9 --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/pipeline.hpp @@ -0,0 +1,234 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "device_clock.hpp" +#include "event_log.hpp" +#include "tactile.hpp" +#include "types.hpp" +#include "wrist_tracking.hpp" + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! The 26 OpenXR joints of one hand in the stage, ready for a hand injector. +struct HandFrame +{ + std::array joints{}; + std::array radius{}; + std::uint32_t position_valid_mask = 0; + std::uint32_t orientation_valid_mask = 0; + bool tracked = false; //!< TRACKED bits as well as VALID bits. + std::int64_t sample_mono_ns = 0; +}; + +//! Where one hand's pipeline delivers its output. The app maps each call onto OpenXR; tests +//! record the calls. Times are CLOCK_MONOTONIC ns. Raw times are the sampling time on the device +//! clock in ns where the device reports one (tactile); joint states pass their monotonic time. +class PushSink +{ +public: + virtual ~PushSink() = default; + virtual void push_joint_state(const std::vector& buffer, std::int64_t sample_ns, std::int64_t raw_ns) = 0; + virtual void push_tactile(const std::vector& buffer, std::int64_t sample_ns, std::int64_t raw_ns) = 0; + //! Remove retained force data when its validity ends; metadata stays available. + virtual void clear_tactile() = 0; + virtual void push_shape(const std::vector& buffer, std::int64_t now_ns) = 0; + //! Remove the TactileMetadata: the glove now in use has no tactile sensors. + virtual void clear_shape() = 0; + virtual void hand_create() = 0; + virtual void hand_destroy() = 0; + virtual void hand_push(const HandFrame& frame) = 0; +}; + +//! WRIST pose of one hand in the OpenXR stage, as the host's wrist source reports it. +struct WristPose +{ + Pose pose_stage; + bool valid = false; //!< pose_stage holds a pose; without one the hand is placed at the origin. + bool tracked = false; //!< The runtime tracks the pose now, not only holds the last one. +}; + +//! Where the hand pipeline takes the WRIST pose from (plugin_utils::WristPoseSource in the plugin). +class WristProvider +{ +public: + virtual ~WristProvider() = default; + virtual WristPose wrist(Hand hand, std::int64_t sample_mono_ns) = 0; +}; + +struct PipelineConfig +{ + //! No hand push for this long destroys the injector. + std::int64_t stall_ns = 50'000'000; + //! The TactileMetadata is pushed once a glove with tactile sensors is identified, then again at this period. + std::int64_t shape_resend_ns = 1'000'000'000; + //! Data older than this counts as absent in health(). + std::int64_t health_timeout_ns = 500'000'000; + //! Maximum age of a tactile force sample; also used for tactile health. + std::int64_t tactile_max_age_ns = 100'000'000; + //! Period of the tactile_rate event in the event log; 0 turns it off. + std::int64_t tactile_rate_period_ns = 1'000'000'000; +}; + +//! Why a tactile sample was not pushed. Without a matching shape no force direction exists, so +//! nothing is pushed rather than a guess. +enum class TactileSkip +{ + None, + NoShape, + NoTactile, //!< The glove's product has no tactile sensors (P1). + Invalid, + Uncalibrated, + CountMismatch, + NoSampleTime, + StaleSample, + NonMonotonicSample, +}; + +const char* tactile_skip_name(TactileSkip skip); + +struct PipelineStats +{ + std::uint64_t glove_samples = 0; + std::uint64_t glove_before_meta = 0; + std::uint64_t joint_states_pushed = 0; + std::uint64_t glove_without_kinematics = 0; + //! Glove samples stamped after the time they were received, pushed at the receive time. + std::uint64_t glove_future_clamped = 0; + std::uint64_t hands_pushed = 0; + std::uint64_t hands_tracked = 0; + std::uint64_t injector_creates = 0; + std::uint64_t injector_destroys = 0; + std::uint64_t tactile_samples = 0; + std::uint64_t tactile_pushed = 0; + std::uint64_t tactile_skipped = 0; + //! Tactile samples with the tick of the last pushed one, e.g. repeated on every glove sample. + std::uint64_t tactile_repeats = 0; + //! The tactile_repeats marked invalid: the firmware re-sends the last snapshot that way when one + //! read of the tactile board fails. + std::uint64_t tactile_invalid_repeats = 0; + //! StaleSample skips, by side: sampled after the time it was received, or older than + //! tactile_max_age_ns. + std::uint64_t tactile_stale_future = 0; + std::uint64_t tactile_stale_old = 0; + std::uint64_t shapes_pushed = 0; + std::uint64_t generation_changes = 0; +}; + +struct HandHealth +{ + bool hand_active = false; + bool glove_live = false; + bool glove_seen = false; //!< A glove sample has been accepted since the pipeline was created. + //! The glove is live but its samples carry no joint poses, so no hand can be injected. + bool kinematics_missing = false; + bool wrist_tracked = false; //!< The last hand pushed had a tracked wrist. + bool tactile_live = false; + bool tactile_seen = false; //!< Tactile has been pushed since the pipeline was created. + TactileSkip tactile_skip = TactileSkip::None; //!< Last reason, while tactile is not live. +}; + +//! Everything one glove needs between the SDK and OpenXR: joint states at the +//! encoder sampling time, the hand injected at the glove sample time with the wrist taken from +//! the WristProvider at that same time, and tactile forces in taxel-local axes at their own sampling time. +//! Single-threaded: call from one thread. +class HandPipeline +{ +public: + HandPipeline(Hand hand, PipelineConfig config, PushSink& sink, EventLog* log = nullptr); + + //! Identity and joint names; glove samples before it are dropped. A glove without tactile sensors + //! (HandMeta::has_tactile) gets neither tactile nor TactileMetadata; its joints and hand go out as usual. + void set_meta(const HandMeta& meta); + //! TactileMetadata for gloves with tactile sensors; without it tactile is never pushed. + void set_shape(std::shared_ptr shape); + + void on_glove(const GloveSample& sample, std::int64_t now_ns); + //! Without a provider the hand is placed at the stage origin, untracked. + void set_wrist_provider(WristProvider* provider) + { + wrist_ = provider; + } + //! Detects stalls and keeps the shape fresh. + void on_tick(std::int64_t now_ns); + + //! The glove's stream started over (reopened): drops the hand, history and clock anchors. + void reset(const char* reason); + + HandHealth health(std::int64_t now_ns) const; + const PipelineStats& stats() const + { + return stats_; + } + Hand hand() const + { + return hand_; + } + +private: + bool has_tactile() const + { + return meta_ && meta_->has_tactile; + } + void clear_tactile(); + void clear_shape(); + void push_shape_if_due(std::int64_t now_ns); + void accept_tactile(const GloveSample& glove, std::int64_t now_ns); + void write_tactile_rate(std::int64_t now_ns); + void inject(const GloveSample& sample, std::int64_t t_ns, std::int64_t now_ns); + void apply(const InjectorLifecycle::Step& step, std::int64_t t_ns); + void note_tactile_skip(TactileSkip skip, std::int64_t now_ns); + + Hand hand_; + PipelineConfig config_; + PushSink& sink_; + EventLog* log_; + + std::optional meta_; + std::shared_ptr shape_; + //! Unset while no TactileMetadata is published, so the next push is the first one. + std::optional last_shape_push_ns_; + + std::optional generation_; + TickUnwrapper tactile_ticks_; + + WristProvider* wrist_ = nullptr; + InjectorLifecycle lifecycle_; + + std::optional last_glove_ns_; + //! Unlike last_glove_ns_, kept by reset(): a reopened glove is not unseen. + bool glove_seen_ = false; + std::optional last_tactile_push_ns_; + //! Device time of the last pushed tactile sample: each tactile tick is pushed once. + std::optional last_tactile_raw_ns_; + std::optional last_tactile_sample_ns_; + bool wrist_tracked_ = false; + TactileSkip tactile_skip_ = TactileSkip::None; + //! The latest tactile sample was an invalid repeat, so an expiry now reports Invalid. + bool tactile_invalid_held_ = false; + bool kinematics_missing_ = false; + + bool tactile_published_ = false; + bool shape_published_ = false; + std::optional last_tactile_warning_ns_; + PipelineStats stats_; + + //! Start of the current tactile_rate window, its counters, and + //! the largest push latency (now minus sampling time) in it; -1 without a push. + std::optional rate_window_ns_; + PipelineStats rate_base_; + std::int64_t rate_lag_max_ns_ = -1; +}; + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/pose_math.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/pose_math.hpp new file mode 100644 index 0000000000..25d894128b --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/pose_math.hpp @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "types.hpp" + +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +using Vec3 = std::array; +using Quat = std::array; // x, y, z, w +using Mat3 = std::array, 3>; + +Quat quat_mul(const Quat& a, const Quat& b); +Quat quat_conj(const Quat& q); +Quat quat_normalize(const Quat& q); +Vec3 quat_rotate(const Quat& q, const Vec3& v); +Mat3 quat_to_mat(const Quat& q); +//! Shortest-arc spherical interpolation, alpha in [0, 1]. +Quat quat_slerp(const Quat& a, const Quat& b, double alpha); + +double dot(const Vec3& a, const Vec3& b); +double norm(const Vec3& v); +Vec3 normalize(const Vec3& v); + +//! a * b: b expressed in a's frame, mapped to a's parent. +Pose compose(const Pose& a, const Pose& b); +Pose inverse(const Pose& a); +Vec3 transform_point(const Pose& a, const Vec3& v); +Pose interpolate(const Pose& a, const Pose& b, double alpha); + +//! A pose written as "px,py,pz,qx,qy,qz,qw" (meters, quaternion x y z w), for command line flags. The +//! quaternion is normalized. Throws std::invalid_argument naming @p flag for anything but seven finite +//! numbers with a non-zero quaternion. +Pose parse_pose(std::string_view text, std::string_view flag); + +//! The 26 OpenXR joints in the stage, from the WRIST pose and the WRIST-frame joint poses. +std::array joints_in_stage(const Pose& wrist_stage, const HandKinematics& kinematics); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/serialize.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/serialize.hpp new file mode 100644 index 0000000000..b0899900ed --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/serialize.hpp @@ -0,0 +1,31 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "types.hpp" + +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! JointStateOutput with the 21 encoder joints in SDK order (not sorted by name). +//! position is static_cast(joints_rad[i]); valid is bit i of joints_valid_mask. +//! Throws std::length_error above kJointStateMaxBytes. +std::vector serialize_joint_state(const std::string& device_id, + const std::array& names, + const std::array& joints_rad, + std::uint32_t joints_valid_mask); + +//! TactileDataArray, one force vector per taxel in TactileMetadata order. +//! Throws std::length_error above kTactileMaxBytes. +std::vector serialize_tactile(const std::vector>& forces); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/sha256.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/sha256.hpp new file mode 100644 index 0000000000..439430a949 --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/sha256.hpp @@ -0,0 +1,20 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! FIPS 180-4 SHA-256 of a byte range, as 64 lowercase hex digits. +std::string sha256_hex(const std::uint8_t* data, std::size_t size); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/source.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/source.hpp new file mode 100644 index 0000000000..b04cc3a5f8 --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/source.hpp @@ -0,0 +1,104 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "types.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! What a source hands to the per-hand pipelines. +using SourceRecord = std::variant; + +struct SourceBatch +{ + //! Indexed by Hand: the hand's glove was opened again before these records, so everything + //! derived from its previous records (tick anchors, injected hand) must be dropped first. + std::array hand_restarted{ false, false }; + //! No more records will come. + bool finished = false; + std::vector records; +}; + +//! Feeds the plugin, polled from its tick loop. Sample times are CLOCK_MONOTONIC ns. A source +//! delivers a hand's HandMeta before that hand's first glove sample, and again after the hand +//! restarted. +class Source +{ +public: + virtual ~Source() = default; + + //! The records due at now_ns. + virtual SourceBatch poll(std::int64_t now_ns) = 0; + + //! What the source knows about a glove that gives no samples, for device status. + struct GloveNote + { + enum class Kind + { + Waiting, //!< Not open; the source keeps looking for it. + NotReady, //!< Open but not delivering, for example while the capture gate is closed. + Ended, //!< Gone for good. + }; + Kind kind = Kind::NotReady; + std::string text; + }; + //! nullopt when the source has nothing to say about this hand. + virtual std::optional glove_note(Hand hand) const + { + static_cast(hand); + return std::nullopt; + } + + virtual std::uint64_t records_released() const = 0; + +protected: + Source() = default; + Source(const Source&) = default; + Source(Source&&) = default; + Source& operator=(const Source&) = default; + Source& operator=(Source&&) = default; +}; + +/*! + * @brief Hand geometry provider for live glove angles. + * + * The plugin-local implementation maps the glove angles to the 26 OpenXR joints, with its own FK + * or the SDK's mechanical poses. + * Without a provider, live glove samples carry no kinematics: joint states and tactile skip + * reasons are still pushed, the hand is not injected. + */ +class KinematicsProvider +{ +public: + virtual ~KinematicsProvider() = default; + + //! Identifies the hand model and the OpenXR joint layout, for the logs. + virtual std::string model_id() const = 0; + + //! The 26 OpenXR joints in the WRIST frame for these joint angles (SDK order, rad), or + //! nullopt if calculation failed. Per-point masks preserve partial validity. + virtual std::optional solve(Hand hand, + const std::array& joints_rad, + std::uint32_t joints_valid_mask) const = 0; +}; + +//! Makes the provider for a glove of the product the SDK identified on it ("P1", "PTV1", ...), on +//! whichever thread opens the glove. Throws when that product has no usable provider; the glove is +//! then given up. +using KinematicsFactory = std::function(const std::string& product_code)>; + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/tactile.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/tactile.hpp new file mode 100644 index 0000000000..7d75ce97bc --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/tactile.hpp @@ -0,0 +1,100 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "types.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! Match the tracker metadata reader's IPC cap. +constexpr std::size_t kShapeMaxBytes = core::TRACKER_METADATA_MAX_SIZE; +constexpr std::size_t kTactileMaxBytes = 16384; +constexpr std::size_t kJointStateMaxBytes = 4096; + +//! Three tables and six vectors, including 29 bytes per segment and 60 per taxel. +constexpr std::size_t shape_size_bound(std::size_t joints, std::size_t taxels) +{ + return 256 + 29 * joints + 60 * taxels; +} + +//! Upper bound of a serialized TactileDataArray: table, one Point vector, slack. +constexpr std::size_t tactile_size_bound(std::size_t taxels) +{ + return 40 + 12 * taxels + 32; +} + +//! LBS bind poses in WRIST space. Segment slots index joint_ids, not OpenXR ordinals. +struct ShapeSpec +{ + std::vector bind_poses; + std::vector joint_ids; + std::vector taxel_bind_poses; + std::vector> joint_indices; + std::vector> joint_weights; +}; + +//! Serializes a bare TactileMetadata, without a Record wrapper. +std::vector build_shape_buffer(const ShapeSpec& spec); + +//! Validates LBS/HandSkeletonRef semantics and finite, unit bind poses. +std::vector validate_shape(const core::TactileMetadata& shape); + +//! TactileMetadata of a glove, verified and ready to publish and skin. +class LoadedShape +{ +public: + //! Requires valid metadata within the IPC cap and one rigid segment per taxel. + explicit LoadedShape(std::vector bytes); + + const std::vector& bytes() const + { + return bytes_; + } + //! SHA-256 of bytes(), hex, to name the metadata in logs. + const std::string& sha256() const + { + return sha256_; + } + std::uint32_t taxel_count() const + { + return taxel_count_; + } + const core::TactileMetadata& shape() const; + + //! Skinning of the bind positions with the current WRIST-frame joint poses. With rigid + //! binding each taxel moves as T_current[j] * inverse(T_bind[j]) * p_bind. + std::vector> skin_positions(const HandKinematics& current) const; + //! Every positively weighted joint must have a finite, valid unit orientation. + bool has_valid_orientations(const HandKinematics& current) const; + //! Taxel normals at the current pose, unit length, in the WRIST frame. + std::vector> current_normals(const HandKinematics& current) const; + +private: + std::vector bytes_; + std::string sha256_; + std::uint32_t taxel_count_ = 0; + std::vector bind_inverse_; +}; + +//! The SDK's calibrated reading, unchanged, along each taxel's outward +Z: the normal force the skin exerts on +//! what it touches. +std::vector> taxel_forces(const std::vector& raw); + +//! The SDK reading recovered from a force vector: round(|f|). +long force_to_counts(const std::array& force); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/types.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/types.hpp new file mode 100644 index 0000000000..32e6d5b00d --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/types.hpp @@ -0,0 +1,89 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! Encoder joints per glove, in SDK order (RH_JOINT_COUNT). +constexpr std::size_t kGloveJointCount = 21; +//! OpenXR hand joints (XR_HAND_JOINT_COUNT_EXT); PALM = 0, WRIST = 1. +constexpr std::size_t kHandJointCount = 26; +constexpr std::uint32_t kPalmJoint = 0; +constexpr std::uint32_t kWristJoint = 1; +constexpr std::uint32_t kMappedHandMask = + ((1u << kHandJointCount) - 1u) & ~((1u << 6) | (1u << 11) | (1u << 16) | (1u << 21)); +//! Protocol upper bound on taxels per glove (RH_TACTILE_MAX_POINTS). +constexpr std::size_t kMaxTaxels = 350; + +enum class Hand : std::uint8_t +{ + Left = 0, + Right = 1, +}; + +const char* hand_name(Hand hand); + +//! Rigid transform. q is (x, y, z, w), unit length. +struct Pose +{ + std::array p{ 0.0, 0.0, 0.0 }; + std::array q{ 0.0, 0.0, 0.0, 1.0 }; +}; + +//! The 26 OpenXR hand joints in the WRIST frame (joints[kWristJoint] is identity), with radii. +//! Produced by the selected pose provider, after mapping and reviewed corrections. +struct HandKinematics +{ + std::array joints{}; + std::array radius{}; + std::uint32_t position_valid_mask = 0; + std::uint32_t orientation_valid_mask = 0; +}; + +struct TactileSample +{ + std::uint32_t device_tick = 0; //!< Tactile sampling tick, 1 ms, wraps at 2^32. + std::int64_t sample_mono_ns = 0; //!< CLOCK_MONOTONIC sampling time; valid when has_sample_time. + bool has_sample_time = false; + bool valid = false; + bool calibrated = false; + std::vector values; //!< The SDK reading, one per taxel. +}; + +struct GloveSample +{ + Hand hand = Hand::Left; + std::uint32_t generation = 0; //!< Changes on every (re)start of the device stream. + std::uint32_t device_tick = 0; //!< Device send tick, 1 ms, wraps at 2^32; not a sampling time. + std::uint32_t joints_valid_mask = 0; //!< Bit i set when joints_rad[i] is valid. + bool has_sample_time = false; + std::int64_t sample_mono_ns = 0; //!< CLOCK_MONOTONIC encoder sampling time. + std::array joints_rad{}; + std::optional tactile; + //! Joint poses at sample_mono_ns, in the WRIST frame. + std::optional kinematics; +}; + +struct HandMeta +{ + Hand hand = Hand::Left; + std::string product_code; + std::string device_id; + std::array joint_names; + //! The product has tactile sensors. A P1 glove has none: it streams joints only. + bool has_tactile = true; +}; + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/wrist_tracking.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/wrist_tracking.hpp new file mode 100644 index 0000000000..00210f352f --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/wrist_tracking.hpp @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! When the OpenXR hand injector of one hand must exist: from the first hand frame until no frame +//! was pushed for stall_ns. The caller owns the injector and does what each Step says, in order: +//! destroy, then create. +class InjectorLifecycle +{ +public: + struct Step + { + bool destroy = false; + bool create = false; + std::string reason; //!< Why the injector was destroyed or created. + }; + + explicit InjectorLifecycle(std::int64_t stall_ns); + + //! A hand frame is about to be pushed; now_ns is the current monotonic time. + Step on_frame(std::int64_t now_ns); + //! Periodic check: destroys the injector when nothing was pushed for stall_ns. + Step on_tick(std::int64_t now_ns); + + bool active() const + { + return active_; + } + +private: + std::int64_t stall_ns_; + bool active_ = false; + std::int64_t last_push_ns_ = 0; +}; + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/pipeline.cpp b/src/plugins/revohuman/core/pipeline.cpp new file mode 100644 index 0000000000..93478c2a61 --- /dev/null +++ b/src/plugins/revohuman/core/pipeline.cpp @@ -0,0 +1,470 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/pipeline.hpp" + +#include "inc/revohuman_plugin/pose_math.hpp" +#include "inc/revohuman_plugin/serialize.hpp" + +#include + +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +std::shared_ptr pipeline_logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.revohuman.HandPipeline"); +} + +} // namespace + +const char* tactile_skip_name(TactileSkip skip) +{ + switch (skip) + { + case TactileSkip::None: + return "none"; + case TactileSkip::NoShape: + return "no_shape"; + case TactileSkip::NoTactile: + return "no_tactile"; + case TactileSkip::Invalid: + return "invalid"; + case TactileSkip::Uncalibrated: + return "uncalibrated"; + case TactileSkip::CountMismatch: + return "count_mismatch"; + case TactileSkip::NoSampleTime: + return "no_sample_time"; + case TactileSkip::StaleSample: + return "stale_sample"; + case TactileSkip::NonMonotonicSample: + return "nonmonotonic_sample"; + } + return "unknown"; +} + +HandPipeline::HandPipeline(Hand hand, PipelineConfig config, PushSink& sink, EventLog* log) + : hand_(hand), config_(config), sink_(sink), log_(log), lifecycle_(config.stall_ns) +{ +} + +void HandPipeline::set_meta(const HandMeta& meta) +{ + if (meta.hand != hand_) + { + return; + } + const bool had_tactile = !meta_ || meta_->has_tactile; + meta_ = meta; + if (!meta.has_tactile) + { + // Another glove may have used this hand before: its forces and layout do not belong to this one. + clear_tactile(); + clear_shape(); + if (had_tactile) + { + pipeline_logger()->info( + "{}: product {} has no tactile sensors; tactile is not pushed", hand_name(hand_), meta.product_code); + } + } + if (log_) + { + log_->write("meta", { { "hand", std::string(hand_name(hand_)) }, + { "product_code", meta.product_code }, + { "device_id", meta.device_id }, + { "has_tactile", meta.has_tactile } }); + } +} + +void HandPipeline::set_shape(std::shared_ptr shape) +{ + clear_tactile(); + shape_ = std::move(shape); + // The new shape replaces the published one at the next push. + last_shape_push_ns_.reset(); +} + +void HandPipeline::on_glove(const GloveSample& sample, std::int64_t now_ns) +{ + ++stats_.glove_samples; + if (!meta_) + { + if (stats_.glove_before_meta++ == 0) + { + pipeline_logger()->warn("{}: glove samples before the device metadata are dropped", hand_name(hand_)); + } + return; + } + if (generation_ && *generation_ != sample.generation) + { + // A new stream generation can reuse device ticks and must not inherit tactile freshness. + clear_tactile(); + ++stats_.generation_changes; + tactile_ticks_.reset(); + last_tactile_raw_ns_.reset(); + last_tactile_sample_ns_.reset(); + last_tactile_push_ns_.reset(); + tactile_invalid_held_ = false; + if (log_) + { + log_->write("generation", { { "hand", std::string(hand_name(hand_)) }, + { "from", static_cast(*generation_) }, + { "to", static_cast(sample.generation) } }); + } + } + generation_ = sample.generation; + last_glove_ns_ = now_ns; + glove_seen_ = true; + + // A sample cannot be stamped after the time it became available; a device clock mapped slightly + // ahead of CLOCK_MONOTONIC would otherwise put joint states and the hand in the future. + std::int64_t t_ns = now_ns; + if (sample.has_sample_time) + { + t_ns = sample.sample_mono_ns; + if (t_ns > now_ns) + { + ++stats_.glove_future_clamped; + t_ns = now_ns; + } + } + // The device reports when it sent a glove frame, not when it sampled the encoders, so the joint + // state has no sample time on the device clock; the monotonic sample time stands in for it. + sink_.push_joint_state( + serialize_joint_state(meta_->device_id, meta_->joint_names, sample.joints_rad, sample.joints_valid_mask), t_ns, + t_ns); + ++stats_.joint_states_pushed; + + if (sample.tactile && has_tactile()) + { + // The TactileMetadata goes out before the first forces that need it. + push_shape_if_due(now_ns); + accept_tactile(sample, now_ns); + } + + kinematics_missing_ = !sample.kinematics.has_value(); + if (kinematics_missing_) + { + if (stats_.glove_without_kinematics++ == 0) + { + pipeline_logger()->warn("{}: glove samples carry no joint poses; the hand is not injected", hand_name(hand_)); + } + return; + } + inject(sample, t_ns, now_ns); +} + +void HandPipeline::accept_tactile(const GloveSample& glove, std::int64_t now_ns) +{ + ++stats_.tactile_samples; + const TactileSample& tactile = *glove.tactile; + const std::int64_t t_ns = tactile.sample_mono_ns; + // Unwrap every sample, pushed or not, so the device clock stays continuous. + const std::int64_t raw_ns = ms_ticks_to_ns(tactile_ticks_.unwrap(tactile.device_tick)); + const bool repeat = last_tactile_raw_ns_ && *last_tactile_raw_ns_ == raw_ns; + tactile_invalid_held_ = false; + TactileSkip skip = TactileSkip::None; + if (!shape_) + { + skip = TactileSkip::NoShape; + } + else if (!tactile.valid) + { + // When one read of the tactile board fails, the firmware re-sends the last snapshot with the + // valid bit cleared, and the next read normally recovers. Downstream still holds that sample, + // so the re-send is a repeat until the sample's maximum age, and Invalid after it. + if (repeat && tactile_published_ && now_ns >= *last_tactile_sample_ns_ && + now_ns - *last_tactile_sample_ns_ <= config_.tactile_max_age_ns) + { + ++stats_.tactile_repeats; + ++stats_.tactile_invalid_repeats; + tactile_invalid_held_ = true; + return; + } + skip = TactileSkip::Invalid; + } + else if (!tactile.calibrated) + { + skip = TactileSkip::Uncalibrated; + } + else if (tactile.values.size() != shape_->taxel_count()) + { + skip = TactileSkip::CountMismatch; + } + else if (!tactile.has_sample_time) + { + skip = TactileSkip::NoSampleTime; + } + else if (t_ns > now_ns || now_ns - t_ns > config_.tactile_max_age_ns) + { + skip = TactileSkip::StaleSample; + ++(t_ns > now_ns ? stats_.tactile_stale_future : stats_.tactile_stale_old); + } + else if (repeat) + { + // Checked last, so a repeated tick cannot hide bad metadata or an expired sampling time. The + // invalid re-send above is the one exception, and it is held only until the maximum age. + ++stats_.tactile_repeats; + return; + } + else if (last_tactile_sample_ns_ && (t_ns <= *last_tactile_sample_ns_ || raw_ns <= *last_tactile_raw_ns_)) + { + skip = TactileSkip::NonMonotonicSample; + } + note_tactile_skip(skip, now_ns); + if (skip != TactileSkip::None) + { + // The last pushed forces keep their own sample time and expire in on_tick(). Removing them + // here would recreate the collection whenever skipped and valid samples alternate. + ++stats_.tactile_skipped; + return; + } + // Taxel-local pressure needs no encoder pose or skinning. Geometry consumers must + // independently validate the hand pose before rotating these forces into another frame. + sink_.push_tactile(serialize_tactile(taxel_forces(tactile.values)), t_ns, raw_ns); + tactile_published_ = true; + ++stats_.tactile_pushed; + rate_lag_max_ns_ = std::max(rate_lag_max_ns_, now_ns - t_ns); + last_tactile_push_ns_ = now_ns; + last_tactile_raw_ns_ = raw_ns; + last_tactile_sample_ns_ = t_ns; +} + +void HandPipeline::clear_tactile() +{ + if (tactile_published_) + { + // SchemaTracker retains the last buffer: silence alone cannot invalidate force data. + sink_.clear_tactile(); + tactile_published_ = false; + } + last_tactile_push_ns_.reset(); +} + +void HandPipeline::clear_shape() +{ + if (shape_published_) + { + // Like the forces, the retained metadata stays until it is removed. + sink_.clear_shape(); + shape_published_ = false; + } + last_shape_push_ns_.reset(); +} + +void HandPipeline::push_shape_if_due(std::int64_t now_ns) +{ + if (!shape_ || !has_tactile() || (last_shape_push_ns_ && now_ns - *last_shape_push_ns_ < config_.shape_resend_ns)) + { + return; + } + if (!last_shape_push_ns_ && log_) + { + log_->write("shape", { { "hand", std::string(hand_name(hand_)) }, + { "sha256", shape_->sha256() }, + { "taxels", static_cast(shape_->taxel_count()) }, + { "bytes", static_cast(shape_->bytes().size()) } }); + } + sink_.push_shape(shape_->bytes(), now_ns); + shape_published_ = true; + ++stats_.shapes_pushed; + last_shape_push_ns_ = now_ns; +} + +void HandPipeline::note_tactile_skip(TactileSkip skip, std::int64_t now_ns) +{ + if (skip == tactile_skip_) + { + return; + } + if (skip == TactileSkip::None) + { + pipeline_logger()->debug("{}: tactile pushed again", hand_name(hand_)); + } + else if (!last_tactile_warning_ns_ || now_ns - *last_tactile_warning_ns_ >= 1'000'000'000) + { + pipeline_logger()->warn("{}: tactile not pushed ({})", hand_name(hand_), tactile_skip_name(skip)); + last_tactile_warning_ns_ = now_ns; + } + if (log_) + { + log_->write( + "tactile", { { "hand", std::string(hand_name(hand_)) }, { "skip", std::string(tactile_skip_name(skip)) } }); + } + tactile_skip_ = skip; +} + +void HandPipeline::on_tick(std::int64_t now_ns) +{ + apply(lifecycle_.on_tick(now_ns), now_ns); + if (tactile_published_ && (!last_tactile_sample_ns_ || now_ns < *last_tactile_sample_ns_ || + now_ns - *last_tactile_sample_ns_ > config_.tactile_max_age_ns)) + { + clear_tactile(); + // An invalid repeat that outlasts its sample is an invalid sample, not just an old one. + note_tactile_skip(tactile_invalid_held_ ? TactileSkip::Invalid : TactileSkip::StaleSample, now_ns); + } + + push_shape_if_due(now_ns); + + if (log_ && config_.tactile_rate_period_ns > 0) + { + if (!rate_window_ns_) + { + rate_window_ns_ = now_ns; + rate_base_ = stats_; + } + else if (now_ns - *rate_window_ns_ >= config_.tactile_rate_period_ns) + { + write_tactile_rate(now_ns); + } + } +} + +void HandPipeline::write_tactile_rate(std::int64_t now_ns) +{ + // Counts over the window since the last event, plus running totals so that a reader can join + // any two events and tell a restarted plugin (totals going down) from a missed line. + const PipelineStats& s = stats_; + const PipelineStats& b = rate_base_; + const std::uint64_t skipped = s.tactile_skipped - b.tactile_skipped; + const std::uint64_t stale_future = s.tactile_stale_future - b.tactile_stale_future; + const std::uint64_t stale_old = s.tactile_stale_old - b.tactile_stale_old; + log_->write("tactile_rate", { { "hand", std::string(hand_name(hand_)) }, + { "t_ns", now_ns }, + { "window_s", static_cast(now_ns - *rate_window_ns_) * 1e-9 }, + { "samples", s.tactile_samples - b.tactile_samples }, + { "pushed", s.tactile_pushed - b.tactile_pushed }, + { "repeats", s.tactile_repeats - b.tactile_repeats }, + { "invalid_repeats", s.tactile_invalid_repeats - b.tactile_invalid_repeats }, + { "skipped", skipped }, + { "skip_stale_future", stale_future }, + { "skip_stale_old", stale_old }, + { "skip_other", skipped - stale_future - stale_old }, + { "lag_max_ms", rate_lag_max_ns_ < 0 ? std::numeric_limits::quiet_NaN() : + static_cast(rate_lag_max_ns_) * 1e-6 }, + { "samples_total", s.tactile_samples }, + { "pushed_total", s.tactile_pushed }, + { "skipped_total", s.tactile_skipped }, + { "invalid_repeats_total", s.tactile_invalid_repeats } }); + rate_window_ns_ = now_ns; + rate_base_ = stats_; + rate_lag_max_ns_ = -1; +} + +void HandPipeline::inject(const GloveSample& sample, std::int64_t t_ns, std::int64_t now_ns) +{ + // The wrist comes from the host (WristPoseSource, like the other hand plugins). Without a pose + // the hand stays at the stage origin and is pushed untracked, so its joints still show. + const WristPose wrist = wrist_ ? wrist_->wrist(hand_, t_ns) : WristPose{}; + const bool tracked = wrist.valid && wrist.tracked; + apply(lifecycle_.on_frame(now_ns), t_ns); + const HandKinematics& kinematics = *sample.kinematics; + HandFrame frame; + frame.joints = joints_in_stage(wrist.valid ? wrist.pose_stage : Pose{}, kinematics); + frame.radius = kinematics.radius; + frame.position_valid_mask = kinematics.position_valid_mask & kMappedHandMask; + frame.orientation_valid_mask = kinematics.orientation_valid_mask & kMappedHandMask; + frame.tracked = tracked; + frame.sample_mono_ns = t_ns; + sink_.hand_push(frame); + wrist_tracked_ = tracked; + ++stats_.hands_pushed; + if (tracked) + { + ++stats_.hands_tracked; + } +} + +void HandPipeline::apply(const InjectorLifecycle::Step& step, std::int64_t t_ns) +{ + if (step.destroy) + { + sink_.hand_destroy(); + ++stats_.injector_destroys; + pipeline_logger()->info("{}: hand removed ({})", hand_name(hand_), step.reason); + if (log_) + { + log_->write("injector", { { "hand", std::string(hand_name(hand_)) }, + { "action", std::string("destroy") }, + { "reason", step.reason }, + { "t_ns", t_ns } }); + } + } + if (step.create) + { + sink_.hand_create(); + ++stats_.injector_creates; + pipeline_logger()->info("{}: hand injected ({})", hand_name(hand_), step.reason); + if (log_) + { + log_->write("injector", { { "hand", std::string(hand_name(hand_)) }, + { "action", std::string("create") }, + { "reason", step.reason }, + { "t_ns", t_ns } }); + } + } +} + +void HandPipeline::reset(const char* reason) +{ + if (lifecycle_.active()) + { + InjectorLifecycle::Step step; + step.destroy = true; + step.reason = reason; + apply(step, 0); + } + clear_tactile(); + last_glove_ns_.reset(); + kinematics_missing_ = false; + lifecycle_ = InjectorLifecycle(config_.stall_ns); + tactile_ticks_.reset(); + last_tactile_raw_ns_.reset(); + last_tactile_sample_ns_.reset(); + last_tactile_push_ns_.reset(); + tactile_skip_ = TactileSkip::None; + tactile_invalid_held_ = false; + generation_.reset(); + wrist_tracked_ = false; + if (log_) + { + log_->write("reset", { { "hand", std::string(hand_name(hand_)) }, { "reason", std::string(reason) } }); + } +} + +HandHealth HandPipeline::health(std::int64_t now_ns) const +{ + HandHealth h; + h.hand_active = lifecycle_.active(); + h.glove_seen = glove_seen_; + h.glove_live = last_glove_ns_ && now_ns >= *last_glove_ns_ && now_ns - *last_glove_ns_ <= config_.health_timeout_ns; + h.kinematics_missing = h.glove_live && kinematics_missing_; + h.wrist_tracked = h.hand_active && wrist_tracked_; + h.tactile_live = tactile_skip_ == TactileSkip::None && last_tactile_push_ns_ && last_tactile_sample_ns_ && + now_ns >= *last_tactile_sample_ns_ && + now_ns - *last_tactile_sample_ns_ <= config_.tactile_max_age_ns; + h.tactile_seen = stats_.tactile_pushed > 0; + if (meta_ && !meta_->has_tactile) + { + h.tactile_skip = TactileSkip::NoTactile; + } + else if (!h.tactile_live) + { + h.tactile_skip = shape_ ? tactile_skip_ : TactileSkip::NoShape; + } + return h; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/pose_math.cpp b/src/plugins/revohuman/core/pose_math.cpp new file mode 100644 index 0000000000..d6a37fa76c --- /dev/null +++ b/src/plugins/revohuman/core/pose_math.cpp @@ -0,0 +1,203 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/pose_math.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +Quat quat_mul(const Quat& a, const Quat& b) +{ + return { a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1], + a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0], a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3], + a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2] }; +} + +Quat quat_conj(const Quat& q) +{ + return { -q[0], -q[1], -q[2], q[3] }; +} + +Quat quat_normalize(const Quat& q) +{ + const double n = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + return { q[0] / n, q[1] / n, q[2] / n, q[3] / n }; +} + +Vec3 quat_rotate(const Quat& q, const Vec3& v) +{ + // v' = v + 2w (u x v) + 2 u x (u x v), u = (x, y, z). + const Vec3 u{ q[0], q[1], q[2] }; + const Vec3 t{ 2.0 * (u[1] * v[2] - u[2] * v[1]), 2.0 * (u[2] * v[0] - u[0] * v[2]), + 2.0 * (u[0] * v[1] - u[1] * v[0]) }; + return { v[0] + q[3] * t[0] + (u[1] * t[2] - u[2] * t[1]), v[1] + q[3] * t[1] + (u[2] * t[0] - u[0] * t[2]), + v[2] + q[3] * t[2] + (u[0] * t[1] - u[1] * t[0]) }; +} + +Mat3 quat_to_mat(const Quat& q) +{ + const double x = q[0], y = q[1], z = q[2], w = q[3]; + return { { { 1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w) }, + { 2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w) }, + { 2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y) } } }; +} + +Quat quat_slerp(const Quat& a, const Quat& b_in, double alpha) +{ + Quat b = b_in; + double c = a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; + if (c < 0.0) + { + b = { -b[0], -b[1], -b[2], -b[3] }; + c = -c; + } + double wa = 1.0 - alpha; + double wb = alpha; + if (c < 0.9995) + { + const double theta = std::acos(c); + const double s = std::sin(theta); + wa = std::sin((1.0 - alpha) * theta) / s; + wb = std::sin(alpha * theta) / s; + } + return quat_normalize({ wa * a[0] + wb * b[0], wa * a[1] + wb * b[1], wa * a[2] + wb * b[2], wa * a[3] + wb * b[3] }); +} + +double dot(const Vec3& a, const Vec3& b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; +} + +double norm(const Vec3& v) +{ + return std::sqrt(dot(v, v)); +} + +Vec3 normalize(const Vec3& v) +{ + const double n = norm(v); + return { v[0] / n, v[1] / n, v[2] / n }; +} + +Pose compose(const Pose& a, const Pose& b) +{ + const Vec3 r = quat_rotate(a.q, b.p); + return Pose{ { a.p[0] + r[0], a.p[1] + r[1], a.p[2] + r[2] }, quat_normalize(quat_mul(a.q, b.q)) }; +} + +Pose inverse(const Pose& a) +{ + const Quat qi = quat_conj(a.q); + const Vec3 r = quat_rotate(qi, a.p); + return Pose{ { -r[0], -r[1], -r[2] }, qi }; +} + +Vec3 transform_point(const Pose& a, const Vec3& v) +{ + const Vec3 r = quat_rotate(a.q, v); + return { a.p[0] + r[0], a.p[1] + r[1], a.p[2] + r[2] }; +} + +Pose interpolate(const Pose& a, const Pose& b, double alpha) +{ + Pose out; + for (int i = 0; i < 3; ++i) + { + out.p[i] = a.p[i] + alpha * (b.p[i] - a.p[i]); + } + out.q = quat_slerp(a.q, b.q, alpha); + return out; +} + +Pose parse_pose(std::string_view text, std::string_view flag) +{ + const auto fail = [&](const std::string& why) + { + return std::invalid_argument(std::string(flag) + " needs px,py,pz,qx,qy,qz,qw (" + why + "), got '" + + std::string(text) + "'"); + }; + std::array values{}; + std::size_t count = 0; + std::size_t begin = 0; + while (true) + { + const std::size_t end = std::min(text.find(',', begin), text.size()); + if (count == values.size()) + { + throw fail("more than 7 numbers"); + } + const std::string field(text.substr(begin, end - begin)); + std::size_t used = 0; + double value = 0.0; + // std::stod skips leading blanks; a field must be the number alone. + if (!field.empty() && !std::isspace(static_cast(field.front()))) + { + try + { + value = std::stod(field, &used); + } + catch (const std::exception&) + { + used = 0; + } + } + if (used == 0 || used != field.size()) + { + throw fail("'" + field + "' is not a number"); + } + if (!std::isfinite(value)) + { + throw fail("values must be finite"); + } + values[count++] = value; + if (end == text.size()) + { + break; + } + begin = end + 1; + } + if (count != values.size()) + { + throw fail(std::to_string(count) + " numbers"); + } + Pose pose; + pose.p = { values[0], values[1], values[2] }; + for (double v : pose.p) + if (std::abs(v) > std::numeric_limits::max()) + throw fail("position exceeds OpenXR float range"); + const Quat q = { values[3], values[4], values[5], values[6] }; + // Scale before squaring: finite CLI values can still overflow the norm. + const double scale = std::max({ std::abs(q[0]), std::abs(q[1]), std::abs(q[2]), std::abs(q[3]) }); + if (scale == 0) + throw fail("the quaternion is zero"); + Quat scaled{ q[0] / scale, q[1] / scale, q[2] / scale, q[3] / scale }; + const double n = + std::sqrt(scaled[0] * scaled[0] + scaled[1] * scaled[1] + scaled[2] * scaled[2] + scaled[3] * scaled[3]); + if (scale <= 1e-6 / n) + throw fail("the quaternion is zero"); + pose.q = { scaled[0] / n, scaled[1] / n, scaled[2] / n, scaled[3] / n }; + return pose; +} + +std::array joints_in_stage(const Pose& wrist_stage, const HandKinematics& kinematics) +{ + std::array out{}; + for (std::size_t i = 0; i < kHandJointCount; ++i) + { + out[i] = compose(wrist_stage, kinematics.joints[i]); + } + return out; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/serialize.cpp b/src/plugins/revohuman/core/serialize.cpp new file mode 100644 index 0000000000..51fb1fc527 --- /dev/null +++ b/src/plugins/revohuman/core/serialize.cpp @@ -0,0 +1,72 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/serialize.hpp" + +#include "inc/revohuman_plugin/tactile.hpp" + +#include +#include + +#include +#include + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +static_assert(tactile_size_bound(kMaxTaxels) <= kTactileMaxBytes, "350 taxels must fit one TactileDataArray push"); + +std::vector finish(flatbuffers::FlatBufferBuilder& builder, std::size_t cap, const char* what) +{ + if (builder.GetSize() > cap) + { + throw std::length_error(std::string(what) + ": " + std::to_string(builder.GetSize()) + " bytes exceeds " + + std::to_string(cap)); + } + return std::vector(builder.GetBufferPointer(), builder.GetBufferPointer() + builder.GetSize()); +} + +} // namespace + +std::vector serialize_joint_state(const std::string& device_id, + const std::array& names, + const std::array& joints_rad, + std::uint32_t joints_valid_mask) +{ + core::JointStateOutputT out; + out.device_id = device_id; + out.has_velocity = false; + out.has_effort = false; + out.ee_pose_valid = false; + for (std::size_t i = 0; i < kGloveJointCount; ++i) + { + auto joint = std::make_shared(); + joint->name = names[i]; + joint->position = static_cast(joints_rad[i]); + joint->valid = ((joints_valid_mask >> i) & 1u) != 0; + out.joints.push_back(std::move(joint)); + } + flatbuffers::FlatBufferBuilder builder(kJointStateMaxBytes); + builder.Finish(core::JointStateOutput::Pack(builder, &out)); + return finish(builder, kJointStateMaxBytes, "joint_state"); +} + +std::vector serialize_tactile(const std::vector>& forces) +{ + std::vector points; + points.reserve(forces.size()); + for (const auto& f : forces) + { + points.emplace_back(f[0], f[1], f[2]); + } + flatbuffers::FlatBufferBuilder builder(kTactileMaxBytes); + builder.Finish(core::CreateTactileDataArrayDirect(builder, &points)); + return finish(builder, kTactileMaxBytes, "tactile"); +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/sha256.cpp b/src/plugins/revohuman/core/sha256.cpp new file mode 100644 index 0000000000..e315850050 --- /dev/null +++ b/src/plugins/revohuman/core/sha256.cpp @@ -0,0 +1,113 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/sha256.hpp" + +#include + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +constexpr std::array kRound = { + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, + 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, + 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, + 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, + 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2, +}; + +std::uint32_t rotr(std::uint32_t x, int n) +{ + return (x >> n) | (x << (32 - n)); +} + +void compress(std::array& h, const std::uint8_t* block) +{ + std::array w{}; + for (int i = 0; i < 16; ++i) + { + w[i] = (static_cast(block[4 * i]) << 24) | (static_cast(block[4 * i + 1]) << 16) | + (static_cast(block[4 * i + 2]) << 8) | static_cast(block[4 * i + 3]); + } + for (int i = 16; i < 64; ++i) + { + const std::uint32_t s0 = rotr(w[i - 15], 7) ^ rotr(w[i - 15], 18) ^ (w[i - 15] >> 3); + const std::uint32_t s1 = rotr(w[i - 2], 17) ^ rotr(w[i - 2], 19) ^ (w[i - 2] >> 10); + w[i] = w[i - 16] + s0 + w[i - 7] + s1; + } + std::uint32_t a = h[0], b = h[1], c = h[2], d = h[3], e = h[4], f = h[5], g = h[6], k = h[7]; + for (int i = 0; i < 64; ++i) + { + const std::uint32_t s1 = rotr(e, 6) ^ rotr(e, 11) ^ rotr(e, 25); + const std::uint32_t ch = (e & f) ^ (~e & g); + const std::uint32_t t1 = k + s1 + ch + kRound[i] + w[i]; + const std::uint32_t s0 = rotr(a, 2) ^ rotr(a, 13) ^ rotr(a, 22); + const std::uint32_t maj = (a & b) ^ (a & c) ^ (b & c); + const std::uint32_t t2 = s0 + maj; + k = g; + g = f; + f = e; + e = d + t1; + d = c; + c = b; + b = a; + a = t1 + t2; + } + h[0] += a; + h[1] += b; + h[2] += c; + h[3] += d; + h[4] += e; + h[5] += f; + h[6] += g; + h[7] += k; +} + +} // namespace + +std::string sha256_hex(const std::uint8_t* data, std::size_t size) +{ + std::array h = { 0x6a09e667, 0xbb67ae85, 0x3c6ef372, 0xa54ff53a, + 0x510e527f, 0x9b05688c, 0x1f83d9ab, 0x5be0cd19 }; + std::size_t full = size / 64; + for (std::size_t i = 0; i < full; ++i) + { + compress(h, data + 64 * i); + } + std::vector tail(data + 64 * full, data + size); + tail.push_back(0x80); + while (tail.size() % 64 != 56) + { + tail.push_back(0); + } + const std::uint64_t bits = static_cast(size) * 8; + for (int i = 7; i >= 0; --i) + { + tail.push_back(static_cast(bits >> (8 * i))); + } + for (std::size_t i = 0; i < tail.size(); i += 64) + { + compress(h, tail.data() + i); + } + static const char* kHex = "0123456789abcdef"; + std::string out; + out.reserve(64); + for (const std::uint32_t word : h) + { + for (int i = 28; i >= 0; i -= 4) + { + out.push_back(kHex[(word >> i) & 0xF]); + } + } + return out; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/tactile.cpp b/src/plugins/revohuman/core/tactile.cpp new file mode 100644 index 0000000000..3963f6e4a1 --- /dev/null +++ b/src/plugins/revohuman/core/tactile.cpp @@ -0,0 +1,206 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/tactile.hpp" + +#include "inc/revohuman_plugin/pose_math.hpp" +#include "inc/revohuman_plugin/sha256.hpp" + +#include +#include + +#include +#include + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +Pose to_pose(const core::Pose& p) +{ + const auto& v = p.position(); + const auto& q = p.orientation(); + return { { v.x(), v.y(), v.z() }, { q.x(), q.y(), q.z(), q.w() } }; +} + +std::vector encode_poses(const std::vector& poses) +{ + std::vector out; + for (const auto& p : poses) + out.emplace_back(core::Point(p.p[0], p.p[1], p.p[2]), core::Quaternion(p.q[0], p.q[1], p.q[2], p.q[3])); + return out; +} + +bool valid_pose(const core::Pose& encoded) +{ + const auto p = to_pose(encoded); + double norm2 = 0; + for (const auto v : p.p) + if (!std::isfinite(v)) + return false; + for (const auto v : p.q) + norm2 += v * v; + return std::isfinite(norm2) && std::abs(norm2 - 1) <= 1e-5; +} + +} // namespace + +std::vector build_shape_buffer(const ShapeSpec& spec) +{ + core::LBST lbs; + lbs.segment_bind_poses = encode_poses(spec.bind_poses); + lbs.taxel_bind_poses = encode_poses(spec.taxel_bind_poses); + for (const auto& slots : spec.joint_indices) + lbs.taxel_segment_indices.emplace_back(flatbuffers::span(slots)); + for (const auto& slots : spec.joint_weights) + lbs.taxel_segment_weights.emplace_back(flatbuffers::span(slots)); + core::HandSkeletonRefT hand; + hand.joint_ids = spec.joint_ids; + core::TactileMetadataT metadata; + metadata.geometry_mapping.Set(std::move(lbs)); + metadata.segment_source.Set(std::move(hand)); + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::TactileMetadata::Pack(builder, &metadata)); + return { builder.GetBufferPointer(), builder.GetBufferPointer() + builder.GetSize() }; +} + +std::vector validate_shape(const core::TactileMetadata& shape) +{ + try + { + core::verify_tactile_metadata(shape); + } + catch (const std::runtime_error& error) + { + return { std::string("invalid LBS/HandSkeletonRef metadata: ") + error.what() }; + } + const auto& lbs = *shape.geometry_mapping_as_LBS(); + if (lbs.segment_bind_poses()->size() == 0 || lbs.taxel_bind_poses()->size() == 0) + return { "metadata must contain segments and taxels" }; + for (const auto* poses : { lbs.segment_bind_poses(), lbs.taxel_bind_poses() }) + for (const auto* p : *poses) + if (!valid_pose(*p)) + return { "bind pose position must be finite and orientation must be unit" }; + return {}; +} + +LoadedShape::LoadedShape(std::vector bytes) + : bytes_(std::move(bytes)), sha256_(sha256_hex(bytes_.data(), bytes_.size())) +{ + if (bytes_.size() > kShapeMaxBytes) + throw std::runtime_error("metadata exceeds the tracker IPC cap"); + flatbuffers::Verifier verifier(bytes_.data(), bytes_.size()); + if (!verifier.VerifyBuffer(nullptr)) + throw std::runtime_error("metadata: buffer does not verify as TactileMetadata"); + const auto issues = validate_shape(shape()); + if (!issues.empty()) + throw std::runtime_error("metadata: " + issues.front()); + const auto& lbs = *shape().geometry_mapping_as_LBS(); + taxel_count_ = lbs.taxel_bind_poses()->size(); + if (taxel_count_ > kMaxTaxels) + throw std::runtime_error("metadata taxel count exceeds the glove protocol limit"); + for (const auto* weights : *lbs.taxel_segment_weights()) + { + int nonzero = 0; + for (const auto w : *weights->values()) + nonzero += w > 0 ? 1 : 0; + if (nonzero != 1) + throw std::runtime_error("metadata: only rigid binding (one segment, weight 1) is supported"); + } + for (const auto* p : *lbs.segment_bind_poses()) + bind_inverse_.push_back(inverse(to_pose(*p))); +} + +const core::TactileMetadata& LoadedShape::shape() const +{ + return *flatbuffers::GetRoot(bytes_.data()); +} + +std::vector> LoadedShape::skin_positions(const HandKinematics& current) const +{ + const auto& lbs = *shape().geometry_mapping_as_LBS(); + const auto* ids = shape().segment_source_as_HandSkeletonRef()->joint_ids(); + std::vector out(taxel_count_, { 0, 0, 0 }); + for (std::uint32_t i = 0; i < taxel_count_; ++i) + { + const auto bind = to_pose(*lbs.taxel_bind_poses()->Get(i)); + for (int k = 0; k < 4; ++k) + { + const double w = lbs.taxel_segment_weights()->Get(i)->values()->Get(k); + if (w == 0) + continue; + const auto segment = lbs.taxel_segment_indices()->Get(i)->values()->Get(k); + const auto joint = ids->Get(segment); + const auto moved = transform_point(current.joints[joint], transform_point(bind_inverse_[segment], bind.p)); + for (int c = 0; c < 3; ++c) + out[i][c] += w * moved[c]; + } + } + return out; +} + +bool LoadedShape::has_valid_orientations(const HandKinematics& current) const +{ + const auto& lbs = *shape().geometry_mapping_as_LBS(); + const auto* ids = shape().segment_source_as_HandSkeletonRef()->joint_ids(); + for (std::uint32_t i = 0; i < taxel_count_; ++i) + for (int k = 0; k < 4; ++k) + { + if (lbs.taxel_segment_weights()->Get(i)->values()->Get(k) == 0) + continue; + const auto joint = ids->Get(lbs.taxel_segment_indices()->Get(i)->values()->Get(k)); + if (!(current.orientation_valid_mask & (1u << joint))) + return false; + double norm2 = 0; + for (const auto v : current.joints[joint].q) + norm2 += v * v; + if (!std::isfinite(norm2) || std::abs(norm2 - 1) > 1e-5) + return false; + } + return true; +} + +std::vector> LoadedShape::current_normals(const HandKinematics& current) const +{ + const auto& lbs = *shape().geometry_mapping_as_LBS(); + const auto* ids = shape().segment_source_as_HandSkeletonRef()->joint_ids(); + std::vector out(taxel_count_, { 0, 0, 0 }); + for (std::uint32_t i = 0; i < taxel_count_; ++i) + { + const auto normal = quat_rotate(to_pose(*lbs.taxel_bind_poses()->Get(i)).q, { 0, 0, 1 }); + for (int k = 0; k < 4; ++k) + { + const double w = lbs.taxel_segment_weights()->Get(i)->values()->Get(k); + if (w == 0) + continue; + const auto segment = lbs.taxel_segment_indices()->Get(i)->values()->Get(k); + const auto moved = + quat_rotate(current.joints[ids->Get(segment)].q, quat_rotate(bind_inverse_[segment].q, normal)); + for (int c = 0; c < 3; ++c) + out[i][c] += w * moved[c]; + } + out[i] = normalize(out[i]); + } + return out; +} + +std::vector> taxel_forces(const std::vector& raw) +{ + std::vector> out; + out.reserve(raw.size()); + for (const auto value : raw) + out.push_back({ 0, 0, static_cast(value) }); + return out; +} + +long force_to_counts(const std::array& force) +{ + const double x = force[0], y = force[1], z = force[2]; + return std::lround(std::sqrt(x * x + y * y + z * z)); +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/types.cpp b/src/plugins/revohuman/core/types.cpp new file mode 100644 index 0000000000..ff2c2e5248 --- /dev/null +++ b/src/plugins/revohuman/core/types.cpp @@ -0,0 +1,17 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/types.hpp" + +namespace plugins +{ +namespace revohuman +{ + +const char* hand_name(Hand hand) +{ + return hand == Hand::Left ? "left" : "right"; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/wrist_tracking.cpp b/src/plugins/revohuman/core/wrist_tracking.cpp new file mode 100644 index 0000000000..8f4427c8bf --- /dev/null +++ b/src/plugins/revohuman/core/wrist_tracking.cpp @@ -0,0 +1,41 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/wrist_tracking.hpp" + +namespace plugins +{ +namespace revohuman +{ + +InjectorLifecycle::InjectorLifecycle(std::int64_t stall_ns) : stall_ns_(stall_ns) +{ +} + +InjectorLifecycle::Step InjectorLifecycle::on_frame(std::int64_t now_ns) +{ + Step step; + if (!active_) + { + active_ = true; + step.create = true; + step.reason = "first_frame"; + } + last_push_ns_ = now_ns; + return step; +} + +InjectorLifecycle::Step InjectorLifecycle::on_tick(std::int64_t now_ns) +{ + Step step; + if (active_ && now_ns - last_push_ns_ > stall_ns_) + { + active_ = false; + step.destroy = true; + step.reason = "stall"; + } + return step; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/install.sh b/src/plugins/revohuman/install.sh new file mode 100755 index 0000000000..29853dd86b --- /dev/null +++ b/src/plugins/revohuman/install.sh @@ -0,0 +1,232 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Install the RevoHuman glove plugin: download the pinned RevoHuman C SDK release +# (or use the one given with --sdk-root), verify it against a pinned per-arch +# SHA-256, then configure, build, and install the plugin with the SDK's shared +# library next to it. Run with --help for the options. + +set -euo pipefail + +REVOHUMAN_SDK_C_VERSION="0.1.3" +REVOHUMAN_SDK_C_REPO="BrainCoTech/brainco-revohuman-sdk" +REVOHUMAN_SDK_C_TAG="c-sdk-v0.1.3" +# Pinned SHA-256 per release asset; update together with the version/tag above +# (every SDK version bump needs a matching commit here, same tradeoff as +# wuji_glove's install.sh). +declare -A REVOHUMAN_SDK_C_SHA256=( + ["x86_64"]="773dc63fd5daf62e4acceb90278e7bca493dc7fada3aad4febe45fb8934759dc" + ["aarch64"]="bdd4a9ae3c1777f4c2dccf6c2be35d3a6ccba2098e289ff1677527042cc0f642" +) + +usage() { + cat <] [--build-dir ] + [--install-prefix ] [] + [-- ...] + +Builds and installs the RevoHuman glove plugin with the RevoHuman C SDK library +next to it. Without --sdk-root (or REVOHUMAN_SDK_ROOT), downloads the pinned +C SDK release instead. + +Options: + --sdk-root Already installed C SDK prefix with include/revohuman.h + and lib/librevohuman.so (env: REVOHUMAN_SDK_ROOT). + --build-dir CMake build directory (default: /build). + --install-prefix + Install prefix for the plugin, passed to + \`cmake --install --prefix\` (default: the build's + CMAKE_INSTALL_PREFIX, i.e. /install). + -h, --help Show this help. + -- ... Pass the remaining arguments to the CMake configure step, + e.g. \`-- -DBUILD_VIZ=OFF\` when the Televiz build tools + (glslangValidator, wayland-scanner) are not installed. + +The option takes precedence over the environment variable. + +Needs CMake 3.24 or newer first on PATH (the top-level CMakeLists.txt requires +it). The build runs \`cmake --build --parallel\` without a job count; set +CMAKE_BUILD_PARALLEL_LEVEL to limit the number of jobs. +EOF +} + +die() { + echo "error: $*" >&2 + exit 1 +} + +# Absolute form of a path; the directory need not exist yet. +abspath() { + if [[ "$1" == /* ]]; then + echo "$1" + else + echo "$PWD/$1" + fi +} + +build_dir="" +isaac_root="" +sdk_root="${REVOHUMAN_SDK_ROOT:-}" +root_from="REVOHUMAN_SDK_ROOT" +install_prefix="" +cmake_args=() +while [[ $# -gt 0 ]]; do + case "$1" in + --build-dir) + build_dir="${2:?--build-dir requires a non-empty path}" + shift 2 + ;; + --sdk-root) + sdk_root="${2:?--sdk-root requires a non-empty path}" + root_from="--sdk-root" + shift 2 + ;; + --install-prefix) + install_prefix="${2:?--install-prefix requires a non-empty path}" + shift 2 + ;; + -h | --help) + usage + exit 0 + ;; + --) + shift + cmake_args=("$@") + break + ;; + -*) + echo "Unknown option: $1" >&2 + exit 1 + ;; + *) + if [[ -n "$isaac_root" ]]; then + echo "Only one Isaac Teleop root may be specified" >&2 + exit 1 + fi + isaac_root="$1" + shift + ;; + esac +done + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +ISAAC_ROOT="${isaac_root:-$(cd "$SCRIPT_DIR/../../.." && pwd)}" +BUILD_DIR="$(abspath "${build_dir:-$ISAAC_ROOT/build}")" + +download_work="" +trap '[[ -z "$download_work" ]] || rm -rf "$download_work"' EXIT + +# 1. RevoHuman C SDK (include/revohuman.h + lib/librevohuman.so). +if [[ -n "$sdk_root" ]]; then + [[ -d "$sdk_root" ]] || die "$root_from: no such directory: $sdk_root" + sdk_root="$(cd "$sdk_root" && pwd)" + echo "==> Using installed RevoHuman C SDK: $sdk_root" +else + # No SDK root given: download the pinned release and use it as the SDK root. + command -v curl >/dev/null 2>&1 || die "curl is required to download the pinned RevoHuman C SDK release" + case "$(uname -m)" in + aarch64 | arm64) rh_arch="aarch64" ;; + x86_64) rh_arch="x86_64" ;; + *) die "unsupported arch: $(uname -m)" ;; + esac + expected_sha256="${REVOHUMAN_SDK_C_SHA256[$rh_arch]:-}" + [[ -n "$expected_sha256" ]] \ + || die "no pinned RevoHuman C SDK release asset for arch '$rh_arch' yet" \ + "(see the SHA-256 map at the top of this script); pass --sdk-root instead" + asset_base="revohuman-c-sdk-${REVOHUMAN_SDK_C_VERSION}-linux-${rh_arch}" + asset_name="${asset_base}.tar.gz" + sdk_root="$SCRIPT_DIR/$asset_base" + # Keyed on the verified SHA-256, not just the directory name: a re-published + # release that keeps the same version must not silently reuse a tree + # downloaded from the earlier one. + stamp_file="$sdk_root/.archive-sha256" + if [[ -f "$sdk_root/include/revohuman.h" && -e "$sdk_root/lib/librevohuman.so" \ + && "$(cat "$stamp_file" 2>/dev/null)" == "$expected_sha256" ]]; then + echo "==> Reusing extracted RevoHuman C SDK: $sdk_root" + else + download_work="$(mktemp -d)" + archive="$download_work/$asset_name" + echo "==> Downloading $asset_name (${REVOHUMAN_SDK_C_REPO}@${REVOHUMAN_SDK_C_TAG})" + curl -fL --retry 3 \ + "https://github.com/${REVOHUMAN_SDK_C_REPO}/releases/download/${REVOHUMAN_SDK_C_TAG}/${asset_name}" \ + -o "$archive" \ + || die "download failed: $asset_name from ${REVOHUMAN_SDK_C_REPO}@${REVOHUMAN_SDK_C_TAG}" + actual_sha256="$(sha256sum "$archive" | awk '{print $1}')" + if [[ "$actual_sha256" != "$expected_sha256" ]]; then + echo "SHA-256 mismatch for $asset_name" >&2 + echo " expected: $expected_sha256" >&2 + echo " actual: $actual_sha256" >&2 + exit 1 + fi + rm -rf "$sdk_root" + tar -xzf "$archive" -C "$SCRIPT_DIR" + echo "$expected_sha256" >"$stamp_file" + fi +fi +[[ -f "$sdk_root/include/revohuman.h" && -e "$sdk_root/lib/librevohuman.so" ]] || { + echo "RevoHuman C SDK not found under $sdk_root (need include/revohuman.h and lib/librevohuman.so)" >&2 + exit 1 +} + +# 2. Configure, build, and install the plugin target. +echo "==> Building revohuman_plugin" +# The arguments after -- come after the build type, so they can change it, and before the +# plugin options, so they cannot turn the plugin off or point it at another SDK. +cmake -B "$BUILD_DIR" -S "$ISAAC_ROOT" -DCMAKE_BUILD_TYPE=Release \ + "${cmake_args[@]}" \ + -DBUILD_PLUGINS=ON \ + -DBUILD_PLUGIN_REVOHUMAN=ON \ + -DREVOHUMAN_SDK_ROOT="$sdk_root" \ + || die "configuring $ISAAC_ROOT failed, see the output above; configure options such as" \ + "-DBUILD_VIZ=OFF go after -- on this script's command line" + +# The plugin CMake records whether the live source links against this SDK; without it there +# is no plugin executable to install. +live_available="$(awk '/^REVOHUMAN_LIVE_AVAILABLE:INTERNAL=/{sub(/^[^=]*=/, ""); print; exit}' \ + "$BUILD_DIR/CMakeCache.txt")" +if [[ "$live_available" != "ON" ]]; then + echo "RevoHuman live source unavailable with the C SDK in $sdk_root" \ + "(REVOHUMAN_LIVE_AVAILABLE=${live_available:-unset}): the library is missing or lacks" \ + "functions the plugin calls; see the configure output above. There is no plugin to install." >&2 + exit 1 +fi + +# Hand poses come only from the SDK hand points, and every glove takes its hand model from the +# SDK's product table, so an SDK that links but lacks the hand-point or product-model API would +# still pass the check above and only fail later, at runtime. Catch that here instead. +for cache_var in REVOHUMAN_HAS_SDK_POINTS REVOHUMAN_HAS_SDK_VARIANT_MODEL; do + case "$cache_var" in + REVOHUMAN_HAS_SDK_POINTS) needed_by="live hand poses come only from rh_hand_points" ;; + REVOHUMAN_HAS_SDK_VARIANT_MODEL) needed_by="no glove has a hand model without it" ;; + esac + value="$(awk -v v="$cache_var" ' + $0 ~ "^" v ":INTERNAL=" { sub(/^[^=]*=/, ""); print; found=1; exit } + END { if (!found) print "__MISSING__" } + ' "$BUILD_DIR/CMakeCache.txt")" + if [[ "$value" != "1" ]]; then + if [[ "$value" == "__MISSING__" ]]; then + detail="not present in CMakeCache.txt" + else + detail="configure check failed" + fi + echo "RevoHuman C SDK in $sdk_root does not provide $cache_var" \ + "($detail): $needed_by" \ + "and live would fail at runtime instead. See kinematics/CMakeLists.txt for the exact" \ + "symbols checked. Refusing the install." >&2 + exit 1 + fi +done + +cmake --build "$BUILD_DIR" --target revohuman_plugin --parallel + +# The plugin, plugin.yaml and every librevohuman.so name land in plugins/revohuman/; the +# plugin's $ORIGIN RPATH finds the library there, so any prefix works. +if [[ -n "$install_prefix" ]]; then + install_prefix="$(abspath "$install_prefix")" + cmake --install "$BUILD_DIR" --component revohuman --prefix "$install_prefix" +else + cmake --install "$BUILD_DIR" --component revohuman + install_prefix="$(awk '/^CMAKE_INSTALL_PREFIX:PATH=/{sub(/^[^=]*=/, ""); print; exit}' "$BUILD_DIR/CMakeCache.txt")" +fi +echo "==> Done: $install_prefix/plugins/revohuman/revohuman_plugin" diff --git a/src/plugins/revohuman/kinematics/CMakeLists.txt b/src/plugins/revohuman/kinematics/CMakeLists.txt new file mode 100644 index 0000000000..f066225aa7 --- /dev/null +++ b/src/plugins/revohuman/kinematics/CMakeLists.txt @@ -0,0 +1,65 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_library(revohuman_hand_kinematics STATIC + hand_geometry.cpp + hand_geometry.hpp + openxr_model.inc + hand_kinematics.cpp + inc/revohuman_plugin/hand_kinematics.hpp +) +target_include_directories(revohuman_hand_kinematics INTERFACE + $) +target_link_libraries(revohuman_hand_kinematics PUBLIC + revohuman_plugin::revohuman_core + revohuman_plugin::sdk_headers) +add_library(revohuman_plugin::revohuman_hand_kinematics ALIAS revohuman_hand_kinematics) + +# Optional SDK entry points: an SDK without them still builds, and the plugin stops at startup naming the +# missing one. +include(CheckCXXSourceCompiles) +include(CMakePushCheckState) +cmake_push_check_state(RESET) +set(CMAKE_REQUIRED_INCLUDES "${REVOHUMAN_SDK_INCLUDE_DIR}") +set(CMAKE_REQUIRED_LIBRARIES "${REVOHUMAN_SDK_LIBRARY}") +# Model-selected point set, the only source of hand poses: without it the plugin fails at startup. +unset(REVOHUMAN_HAS_SDK_POINTS CACHE) +check_cxx_source_compiles([=[ +#include +int main() { + double q[RH_JOINT_COUNT]{}; + RhHandModelInfo info = RH_STRUCT_INIT(RhHandModelInfo); + RhHandPoints points = RH_STRUCT_INIT(RhHandPoints); + static_assert(RH_HAND_POINT_COUNT == 27); + return rh_hand_model_info(RH_HAND_MODEL_DV1, &info) + (info.points_version != RH_HAND_POINTS_VERSION) + + rh_hand_points(RH_HAND_MODEL_P, RH_HAND_LEFT, q, 0, &points); +} +]=] REVOHUMAN_HAS_SDK_POINTS) +# The SDK's product -> hand model table: without it live refuses to start, it never assumes a model. +unset(REVOHUMAN_HAS_SDK_VARIANT_MODEL CACHE) +check_cxx_source_compiles([=[ +#include +int main() { + RhHandModel model = 0; + return rh_variant_hand_model("PTV1", &model) + (model != RH_HAND_MODEL_DV1); +} +]=] REVOHUMAN_HAS_SDK_VARIANT_MODEL) +# Where each taxel sits on the hand, the only source of the TactileMetadata: without it tactile is not pushed. +unset(REVOHUMAN_HAS_SDK_TAXEL_LAYOUT CACHE) +check_cxx_source_compiles([=[ +#include +static RhTaxelLayout layout = RH_STRUCT_INIT(RhTaxelLayout); +int main() { + static_assert(RH_TAXEL_LAYOUT_VERSION == 1); + return rh_hand_model_taxel_layout(RH_HAND_MODEL_DV1, RH_HAND_LEFT, RH_TACTILE_POINTS_TYPE_B, &layout) + + (layout.taxels[0].link >= RH_JOINT_COUNT); +} +]=] REVOHUMAN_HAS_SDK_TAXEL_LAYOUT) +cmake_pop_check_state() +target_compile_definitions(revohuman_hand_kinematics PRIVATE + REVOHUMAN_HAS_SDK_POINTS=$ + REVOHUMAN_HAS_SDK_VARIANT_MODEL=$ + REVOHUMAN_HAS_SDK_TAXEL_LAYOUT=$) +if(REVOHUMAN_HAS_SDK_POINTS OR REVOHUMAN_HAS_SDK_VARIANT_MODEL OR REVOHUMAN_HAS_SDK_TAXEL_LAYOUT) + target_link_libraries(revohuman_hand_kinematics PRIVATE revohuman_plugin::sdk) +endif() diff --git a/src/plugins/revohuman/kinematics/hand_geometry.cpp b/src/plugins/revohuman/kinematics/hand_geometry.cpp new file mode 100644 index 0000000000..3afb00aeef --- /dev/null +++ b/src/plugins/revohuman/kinematics/hand_geometry.cpp @@ -0,0 +1,347 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Keep fitting and reviewed geometry in the plugin; the external C SDK is an unmodified dependency. +#include "hand_geometry.hpp" + +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ +namespace +{ +using V = std::array; +using M = std::array; +constexpr M kIdentity{ 1, 0, 0, 0, 1, 0, 0, 0, 1 }; +constexpr V kDorsal{ -1, 0, 0 }; +struct Height +{ + int driver; + V inward; + double amount; +}; +struct PointCorrection +{ + Height lateral; + Height pad; +}; +struct Model +{ + M wrist; + PointCorrection correction[26]; +}; +#include "openxr_model.inc" + +struct FingerMap +{ + int base_angle; + int anchor_angle; + int proximal_point; + int intermediate_point; + int distal_point; + int tip_point; + int placeholder_point; + int orientation_angle; +}; +// SDK angle slots and OpenXR output slots are independent index spaces. A non-thumb proximal point is +// oriented by its MCP link (base + 2), which carries it; the thumb's keeps its reviewed link. +constexpr std::array kFingerMap = { { + { 0, 3, 7, 8, 9, 10, 6, 2 }, + { 4, 7, 12, 13, 14, 15, 11, 6 }, + { 8, 11, 17, 18, 19, 20, 16, 10 }, + { 12, 15, 22, 23, 24, 25, 21, 14 }, + { 16, 20, 2, 3, 4, 5, -1, 19 }, +} }; + +V add(V a, V b) +{ + for (int i = 0; i < 3; ++i) + a[i] += b[i]; + return a; +} +V scale(V a, double s) +{ + for (double& v : a) + v *= s; + return a; +} +V sub(V a, V b) +{ + return add(a, scale(b, -1)); +} +double dot(V a, V b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; +} +V cross(V a, V b) +{ + return { a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0] }; +} +V unit(V a) +{ + double n = std::sqrt(dot(a, a)); + return scale(a, n > 1e-12 ? 1 / n : std::numeric_limits::quiet_NaN()); +} +V mv(M a, V b) +{ + V o{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + o[i] += a[3 * i + j] * b[j]; + return o; +} +M mul(M a, M b) +{ + M o{}; + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + for (int k = 0; k < 3; ++k) + o[3 * i + j] += a[3 * i + k] * b[3 * k + j]; + return o; +} +M transpose(M a) +{ + return { a[0], a[3], a[6], a[1], a[4], a[7], a[2], a[5], a[8] }; +} +M frame(V z, V y) +{ + z = unit(z); + y = sub(y, scale(z, dot(y, z))); + if (dot(y, y) < 1e-14) + y = cross(z, std::abs(z[0]) < .8 ? V{ 1, 0, 0 } : V{ 0, 1, 0 }); + y = unit(y); + V x = unit(cross(y, z)); + y = unit(cross(z, x)); + return { x[0], y[0], z[0], x[1], y[1], z[1], x[2], y[2], z[2] }; +} +std::array quaternion(M a) +{ + std::array q{}; + double trace = a[0] + a[4] + a[8]; + if (trace > 0) + { + double s = 2 * std::sqrt(trace + 1); + q = { (a[7] - a[5]) / s, (a[2] - a[6]) / s, (a[3] - a[1]) / s, s / 4 }; + } + else + { + int i = a[4] > a[0] ? 1 : 0; + if (a[8] > a[3 * i + i]) + i = 2; + int j = (i + 1) % 3, k = (i + 2) % 3; + double s = 2 * std::sqrt(1 + a[3 * i + i] - a[3 * j + j] - a[3 * k + k]); + q[i] = s / 4; + q[j] = (a[3 * i + j] + a[3 * j + i]) / s; + q[k] = (a[3 * i + k] + a[3 * k + i]) / s; + q[3] = (a[3 * k + j] - a[3 * j + k]) / s; + } + double n = 0; + for (double v : q) + n += v * v; + n = std::sqrt(n); + for (double& v : q) + v /= n; + return q; +} +const Model& model(RhHand hand) +{ + if (hand == RH_HAND_LEFT) + return kModel0; + if (hand == RH_HAND_RIGHT) + return kModel1; + throw std::invalid_argument("hand must be left or right"); +} +bool finite(V p) +{ + return std::all_of(p.begin(), p.end(), [](double x) { return std::isfinite(x); }); +} +void write_point(HandPoint& dst, V p, M r, bool mapped) +{ + auto q = quaternion(r); + dst = {}; + dst.radius = .01f; + dst.orientation[3] = 1; + bool pf = finite(p), qf = std::all_of(q.begin(), q.end(), [](double x) { return std::isfinite(x); }); + if (pf) + std::copy(p.begin(), p.end(), dst.position); + if (qf) + std::copy(q.begin(), q.end(), dst.orientation); + dst.flags = mapped ? ((pf ? kHandPositionValid : 0u) | (qf ? kHandOrientationValid : 0u)) : 0u; +} +// Palm-relative links, TIPs and tracker origin to OpenXR points, with the reviewed corrections. +void assemble(RhHand hand, + const Model& m, + const std::array& p, + const std::array& r, + const std::array& tips, + const std::array& tip_directions, + V tracker, + HandPoint out[26]) +{ + std::array points{}; + std::array frames{}; + points[0] = tracker; + frames[0] = frames[1] = m.wrist; + for (int f = 0; f < 5; ++f) + { + const auto& mapping = kFingerMap[f]; + const int b = mapping.base_angle; + if (mapping.placeholder_point >= 0) + { + // Placeholder offsets are along the OpenXR wrist axes from the palm_link origin, not from the shifted + // WRIST; mirror the lateral X axis. + V placeholder{ kPlaceholder[f][0], kPlaceholder[f][1], kPlaceholder[f][2] }; + if (hand == RH_HAND_RIGHT) + placeholder[0] = -placeholder[0]; + points[mapping.placeholder_point] = mv(m.wrist, placeholder); + } + points[mapping.proximal_point] = p[mapping.anchor_angle]; + points[mapping.intermediate_point] = p[b + 1]; + points[mapping.distal_point] = p[b]; + points[mapping.tip_point] = tips[f]; + } + // Each output slot owns both corrections, including explicit zero values. + for (int i = 0; i < 26; ++i) + { + for (const Height* shift : { &m.correction[i].lateral, &m.correction[i].pad }) + { + if (shift->amount != 0) + points[i] = add(points[i], scale(mv(shift->driver < 0 ? kIdentity : r[shift->driver], shift->inward), + shift->amount)); + } + } + // A non-thumb proximal point is placed, pad correction included, in the frame of the MPR link (the anchor), + // but the finger bends about the MCP origin 19.5 mm further out (22.5 mm from the corrected point). Carry it + // with the MCP flexion so that it is rigid on the MCP link, like the PIP point it aims at: the bone keeps its + // length, turns by the encoder angle, and the taxels bound to it stay on the SDK's taxels. MCP and MPR agree at + // zero flexion, so the bind pose does not move; the knuckle point does, 21.7 mm at 1.2 rad. + for (const auto& mapping : kFingerMap) + { + if (mapping.placeholder_point < 0) + continue; + const int mcp = mapping.base_angle + 2; + const M flexion = mul(r[mcp], transpose(r[mapping.anchor_angle])); + V& proximal = points[mapping.proximal_point]; + proximal = add(p[mcp], mv(flexion, sub(proximal, p[mcp]))); + } + for (int f = 0; f < 5; ++f) + { + const auto& mapping = kFingerMap[f]; + const int b = mapping.base_angle; + const int proximal = mapping.proximal_point, intermediate = mapping.intermediate_point, + distal = mapping.distal_point, tip = mapping.tip_point; + if (mapping.placeholder_point >= 0) + frames[mapping.placeholder_point] = m.wrist; + frames[proximal] = frame(sub(points[proximal], points[intermediate]), mv(r[mapping.orientation_angle], kDorsal)); + frames[intermediate] = frame(sub(points[intermediate], points[distal]), mv(r[b + 1], kDorsal)); + frames[distal] = frames[tip] = frame(scale(tip_directions[f], -1), mv(r[b], kDorsal)); + } + HandPoint result[26]{}; + M inv = transpose(m.wrist); + for (int i = 0; i < 26; ++i) + { + bool mapped = i != 6 && i != 11 && i != 16 && i != 21; + // Every pose, the placeholders included, is relative to WRIST (point 1). + write_point(result[i], mv(inv, sub(points[i], points[1])), mul(inv, frames[i]), mapped); + } + std::copy(std::begin(result), std::end(result), out); +} +} // namespace + +const char* hand_points_mapping_id() +{ + return kMappingId; +} +const char* hand_points_reviewed_sha256(const char* model_name) +{ + if (!model_name) + return nullptr; + for (const auto& reviewed : kReviewedModels) + { + if (std::strcmp(reviewed.name, model_name) == 0) + return reviewed.sha256; + } + return nullptr; +} +// wrist_from_sdk_points() has only palm_link, so the WRIST corrections must not name a driving link. +static_assert(kModel0.correction[1].lateral.driver < 0 && kModel0.correction[1].pad.driver < 0 && + kModel1.correction[1].lateral.driver < 0 && kModel1.correction[1].pad.driver < 0, + "WRIST corrections must stay fixed in palm_link"); +RhStatus wrist_from_sdk_points(RhHand hand, const LinkPose sdk[27], LinkPose* wrist) +{ + if (!sdk || !wrist || (hand != RH_HAND_LEFT && hand != RH_HAND_RIGHT)) + return RH_ERR_INVALID_ARG; + M palm_rotation{}; + std::copy_n(sdk[26].r, 9, palm_rotation.begin()); + const M rotation = mul(palm_rotation, model(hand).wrist); + V origin{}; + std::copy_n(sdk[26].p, 3, origin.begin()); + // WRIST corrections are fixed in palm_link (see the static_assert above). Share this origin with taxels. + for (const Height* shift : { &model(hand).correction[1].lateral, &model(hand).correction[1].pad }) + origin = add(origin, mv(palm_rotation, scale(shift->inward, shift->amount))); + std::copy(origin.begin(), origin.end(), wrist->p); + std::copy(rotation.begin(), rotation.end(), wrist->r); + return RH_OK; +} +int openxr_joint_of_sdk_link(std::uint32_t link) +{ + for (const auto& mapping : kFingerMap) + { + const auto b = static_cast(mapping.base_angle); + if (link == b) + return mapping.distal_point; + if (link == b + 1) + return mapping.intermediate_point; + // A non-thumb proximal point is carried by its MCP link (see assemble()); the thumb's MCP link has + // no point of its own. + if (link == b + 2 && mapping.placeholder_point >= 0) + return mapping.proximal_point; + } + return -1; +} +RhStatus hand_points_from_sdk_points(RhHand hand, const LinkPose sdk[27], HandPoint out[26]) +{ + if (!sdk || !out || (hand != RH_HAND_LEFT && hand != RH_HAND_RIGHT)) + return RH_ERR_INVALID_ARG; + // Re-express tracker_link poses relative to palm_link (point 26); the mount is not applied again. + V palm{}; + M palm_rotation{}; + std::copy_n(sdk[26].p, 3, palm.begin()); + std::copy_n(sdk[26].r, 9, palm_rotation.begin()); + const M to_palm = transpose(palm_rotation); + const auto position = [&](const LinkPose& pose) + { + V v{}; + std::copy_n(pose.p, 3, v.begin()); + return mv(to_palm, sub(v, palm)); + }; + std::array p{}; + std::array r{}; + for (int i = 0; i < 21; ++i) + { + M link_rotation{}; + std::copy_n(sdk[i].r, 9, link_rotation.begin()); + p[i] = position(sdk[i]); + r[i] = mul(to_palm, link_rotation); + } + std::array tips{}; + std::array tip_directions{}; + for (int f = 0; f < 5; ++f) + { + // SDK TIPs follow the finger order of kFingerMap and are rigid on that finger's DIP link. + tips[f] = position(sdk[21 + f]); + tip_directions[f] = unit(sub(tips[f], p[kFingerMap[f].base_angle])); + } + assemble(hand, model(hand), p, r, tips, tip_directions, mv(to_palm, scale(palm, -1)), out); + return RH_OK; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/kinematics/hand_geometry.hpp b/src/plugins/revohuman/kinematics/hand_geometry.hpp new file mode 100644 index 0000000000..4df23158c7 --- /dev/null +++ b/src/plugins/revohuman/kinematics/hand_geometry.hpp @@ -0,0 +1,62 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +constexpr int kHandPointCount = 26; +constexpr std::uint32_t kHandPositionValid = 1u; +constexpr std::uint32_t kHandOrientationValid = 2u; + +// OpenXR joint order, wrist-relative meters and normalized (x,y,z,w). +// Flags describe calculated data validity, not spatial tracking quality. +// Four non-thumb metacarpals are preview placeholders with flags == 0. +// Radius is a nominal 10 mm visualization value, not an anatomical measurement. +struct HandPoint +{ + double position[3]; + double orientation[4]; + float radius; + std::uint32_t flags; +}; + +// SDK link poses, as rh_hand_points gives them. +struct LinkPose +{ + double p[3]; + double r[9]; // Row-major rotation matrix, same convention as the SDK FK API. +}; + +constexpr int kSdkPointCount = 27; +constexpr std::uint32_t kSdkPointsVersion = 1u; + +// SDK hand point set (rh_hand_points, points version 1) in its own order and {hand}_tracker_link frame: +// 21 joint child links, five TIPs (index, middle, ring, little, thumb) and the palm_link origin. TIPs are +// taken as given; the tracker_link origin supplies PALM and the corrected palm_link origin supplies WRIST. Invalid +// quantities must be NaN, as the SDK writes them. +RhStatus hand_points_from_sdk_points(RhHand hand, const LinkPose points[kSdkPointCount], HandPoint out[kHandPointCount]); + +// WRIST in the {hand}_tracker_link frame of an SDK point set: the frame the OpenXR points above are written +// in, palm_link (point 26) shifted toward the palm and turned by the reviewed WRIST rotation. +RhStatus wrist_from_sdk_points(RhHand hand, const LinkPose points[kSdkPointCount], LinkPose* wrist); + +// The OpenXR point that moves with SDK link @p link (rh_hand_model_fk order): DIP and PIP links carry the +// finger's distal and intermediate points, a non-thumb MCP link its proximal point. -1 for a link no OpenXR +// point follows (MPR, thumb MCP/CMR/CMP) or out of range. +int openxr_joint_of_sdk_link(std::uint32_t link); + +// Id of the reviewed mapping: its corrections and the rules that apply them. +const char* hand_points_mapping_id(); +// SHA-256 (rh_hand_model_info urdf_sha256) of the SDK hand model named @p model_name on which the mapping +// was reviewed, or nullptr for a model it was not reviewed on. +const char* hand_points_reviewed_sha256(const char* model_name); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/kinematics/hand_kinematics.cpp b/src/plugins/revohuman/kinematics/hand_kinematics.cpp new file mode 100644 index 0000000000..da0722fc72 --- /dev/null +++ b/src/plugins/revohuman/kinematics/hand_kinematics.cpp @@ -0,0 +1,341 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/hand_kinematics.hpp" + +#include "hand_geometry.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#if REVOHUMAN_HAS_SDK_POINTS || REVOHUMAN_HAS_SDK_VARIANT_MODEL || REVOHUMAN_HAS_SDK_TAXEL_LAYOUT +# include +#endif + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +#if REVOHUMAN_HAS_SDK_POINTS +RhHand sdk_hand(Hand hand) +{ + return hand == Hand::Left ? RH_HAND_LEFT : RH_HAND_RIGHT; +} + +#endif + +#if REVOHUMAN_HAS_SDK_VARIANT_MODEL || REVOHUMAN_HAS_SDK_TAXEL_LAYOUT +// The SDK's error text is "E : ". Keep the code and class. +std::string sdk_error_text() +{ + char buf[512]; + if (rh_last_error(buf, sizeof(buf)) <= 0) + return "E" + std::to_string(rh_last_error_code()); + const std::string text(buf); + return text.substr(0, text.find(':')); +} +#endif + +#if REVOHUMAN_HAS_SDK_POINTS +static_assert(RH_HAND_POINT_COUNT == kSdkPointCount); +// A new points version may reorder points or change TIP rules: review the mapping before accepting it. +static_assert(RH_HAND_POINTS_VERSION == kSdkPointsVersion); + +RhHandModel sdk_model(HandModel model) +{ + return model == HandModel::P ? RH_HAND_MODEL_P : RH_HAND_MODEL_DV1; +} + +template +std::string field(const char (&text)[N]) +{ + return std::string(text, std::find(text, text + N, '\0')); +} + +using SdkLinks = std::array; + +// The SDK applies the mask itself and writes NaN for every invalid quantity. +RhStatus sdk_points(HandModel model, + Hand hand, + const std::array& angles, + std::uint32_t valid_mask, + SdkLinks& links) +{ + RhHandPoints sdk = RH_STRUCT_INIT(RhHandPoints); + const auto status = rh_hand_points(sdk_model(model), sdk_hand(hand), angles.data(), valid_mask, &sdk); + if (status != RH_OK) + return status; + for (std::size_t i = 0; i < links.size(); ++i) + { + std::copy_n(sdk.points[i].p, 3, links[i].p); + std::copy_n(sdk.points[i].r, 9, links[i].r); + } + return RH_OK; +} +#endif + +class HandKinematicsSolver final : public KinematicsProvider +{ +public: + explicit HandKinematicsSolver(HandModel model) : model_(model) + { +#if REVOHUMAN_HAS_SDK_POINTS + RhHandModelInfo info = RH_STRUCT_INIT(RhHandModelInfo); + if (rh_hand_model_info(sdk_model(model_), &info) != RH_OK) + throw std::runtime_error("the C SDK does not provide the selected hand model"); + if (info.points_version != kSdkPointsVersion) + throw std::runtime_error("SDK hand point set version " + std::to_string(info.points_version) + + " differs from the reviewed mapping; use a C SDK with points version " + + std::to_string(kSdkPointsVersion)); + // The reviewed corrections require the same mechanical model and numeric joint order. + const std::string name = field(info.name); + const char* reviewed = hand_points_reviewed_sha256(name.c_str()); + const std::string sha = field(info.urdf_sha256); + if (reviewed == nullptr || sha != reviewed) + throw std::runtime_error("SDK hand model '" + name + + "' differs from the reviewed models; use a matching C SDK"); + model_id_ = "sdk-points:" + name + ":" + hand_points_mapping_id() + "-" + sha; +#else + throw std::runtime_error("SDK hand point API unavailable in this build; use a C SDK with rh_hand_points"); +#endif + } + + std::string model_id() const override + { + return model_id_; + } + + std::optional solve(Hand hand, + const std::array& angles, + std::uint32_t valid_mask) const override + { + static_assert(kHandPointCount == kHandJointCount); + static_assert(RH_JOINT_COUNT == kGloveJointCount); + std::array points{}; + if (calculate(hand, angles, valid_mask, points.data()) != RH_OK) + { + return std::nullopt; + } + HandKinematics out; + for (std::size_t i = 0; i < points.size(); ++i) + { + const auto& point = points[i]; + auto& pose = out.joints[i]; + const bool finite_position = std::all_of( + std::begin(point.position), std::end(point.position), [](double v) { return std::isfinite(v); }); + double norm2 = 0.0; + for (double v : point.orientation) + { + norm2 += v * v; + } + if ((point.flags & kHandPositionValid) && finite_position && std::isfinite(point.radius) && + point.radius > 0.0f) + { + std::copy(std::begin(point.position), std::end(point.position), pose.p.begin()); + out.position_valid_mask |= 1u << i; + } + if ((point.flags & kHandOrientationValid) && std::isfinite(norm2) && norm2 > 1e-12) + { + for (std::size_t k = 0; k < pose.q.size(); ++k) + { + pose.q[k] = point.orientation[k] / std::sqrt(norm2); + } + out.orientation_valid_mask |= 1u << i; + } + out.radius[i] = std::isfinite(point.radius) && point.radius > 0.0f ? point.radius : 0.0f; + } + out.position_valid_mask &= kMappedHandMask; + out.orientation_valid_mask &= kMappedHandMask; + return out; + } + +private: + RhStatus calculate(Hand hand, + const std::array& angles, + std::uint32_t valid_mask, + HandPoint* points) const + { +#if REVOHUMAN_HAS_SDK_POINTS + SdkLinks links{}; + const auto status = sdk_points(model_, hand, angles, valid_mask, links); + if (status != RH_OK) + return status; + return hand_points_from_sdk_points(sdk_hand(hand), links.data(), points); +#else + static_cast(hand); + static_cast(angles); + static_cast(valid_mask); + static_cast(points); + return RH_ERR_INVALID_ARG; +#endif + } + HandModel model_; + std::string model_id_; +}; + +#if REVOHUMAN_HAS_SDK_POINTS && REVOHUMAN_HAS_SDK_TAXEL_LAYOUT +// A new layout version may reorder taxels or change how positions and normals are taken: review first. +static_assert(RH_TAXEL_LAYOUT_VERSION == 1); +static_assert(RH_TACTILE_MAX_POINTS == kMaxTaxels); + +constexpr std::uint32_t kAllGloveJoints = (1u << kGloveJointCount) - 1u; + +Vec3 rotate(const double (&r)[9], const double (&v)[3]) +{ + return { r[0] * v[0] + r[1] * v[1] + r[2] * v[2], r[3] * v[0] + r[4] * v[1] + r[5] * v[2], + r[6] * v[0] + r[7] * v[1] + r[8] * v[2] }; +} + +Vec3 rotate_back(const double (&r)[9], const Vec3& v) +{ + return { r[0] * v[0] + r[3] * v[1] + r[6] * v[2], r[1] * v[0] + r[4] * v[1] + r[7] * v[2], + r[2] * v[0] + r[5] * v[1] + r[8] * v[2] }; +} + +// Shortest-arc rotation taking +Z to the unit vector @p n; a half turn about X for -Z. +Quat outward_z(const Vec3& n) +{ + const double w = 1 + n[2]; + if (w < 1e-12) + return { 1, 0, 0, 0 }; + return quat_normalize({ -n[1], n[0], 0, w }); +} + +// The taxels at @p angles with the SDK points they were computed from (tracker_link frame). +SdkTaxels taxels_at(HandModel model, Hand hand, const std::array& angles, SdkLinks& links) +{ + // Same model checks as the poses the shape is skinned with. + const HandKinematicsSolver checked(model); + static_cast(checked); + if (sdk_points(model, hand, angles, kAllGloveJoints, links) != RH_OK) + throw std::runtime_error("the C SDK cannot compute the hand points: " + sdk_error_text()); + LinkPose wrist{}; + if (wrist_from_sdk_points(sdk_hand(hand), links.data(), &wrist) != RH_OK) + throw std::runtime_error("cannot place the WRIST in the SDK hand points"); + // About 19 KB: keep it off the stack. + auto layout = std::make_unique(); + layout->struct_size = sizeof(RhTaxelLayout); + if (rh_hand_model_taxel_layout(sdk_model(model), sdk_hand(hand), RH_TACTILE_POINTS_TYPE_B, layout.get()) != RH_OK) + throw std::runtime_error("the C SDK has no taxel layout for this glove: " + sdk_error_text()); + if (layout->layout_version != RH_TAXEL_LAYOUT_VERSION) + throw std::runtime_error("the C SDK gives taxel layout version " + std::to_string(layout->layout_version) + + ", which differs from the reviewed version " + std::to_string(RH_TAXEL_LAYOUT_VERSION)); + if (layout->count != RH_TACTILE_POINTS_TYPE_B) + throw std::runtime_error("the C SDK gives " + std::to_string(layout->count) + " taxels instead of " + + std::to_string(RH_TACTILE_POINTS_TYPE_B)); + SdkTaxels out; + out.source = field(layout->source); + for (std::uint32_t i = 0; i < layout->count; ++i) + { + const auto& taxel = layout->taxels[i]; + if (taxel.link >= kGloveJointCount) + throw std::runtime_error("the C SDK puts taxel " + std::to_string(i) + " on unknown link " + + std::to_string(taxel.link)); + const auto& link = links[taxel.link]; + Vec3 at = rotate(link.r, taxel.p); + for (int k = 0; k < 3; ++k) + at[k] += link.p[k] - wrist.p[k]; + out.positions.push_back(rotate_back(wrist.r, at)); + out.normals.push_back(normalize(rotate_back(wrist.r, rotate(link.r, taxel.n)))); + out.links.push_back(taxel.link); + } + return out; +} +#endif + +} // namespace + +HandModel hand_model_of_product(const std::string& product_code) +{ +#if REVOHUMAN_HAS_SDK_VARIANT_MODEL + RhHandModel model = 0; + if (rh_variant_hand_model(product_code.c_str(), &model) != RH_OK) + throw std::runtime_error("the C SDK has no hand model for product '" + product_code + "': " + sdk_error_text()); + if (model == RH_HAND_MODEL_DV1) + return HandModel::Dv1; + if (model == RH_HAND_MODEL_P) + return HandModel::P; + throw std::runtime_error("the C SDK gives product '" + product_code + "' hand model " + std::to_string(model) + + ", which this build does not know"); +#else + static_cast(product_code); + throw std::runtime_error( + "this build's C SDK cannot tell the hand model of a product; use a C SDK with rh_variant_hand_model"); +#endif +} + +bool sdk_has_product_hand_models() +{ + return REVOHUMAN_HAS_SDK_VARIANT_MODEL; +} + +std::shared_ptr make_hand_kinematics(HandModel model) +{ + return std::make_shared(model); +} + +SdkTaxels sdk_taxels(HandModel model, Hand hand, const std::array& angles) +{ +#if REVOHUMAN_HAS_SDK_POINTS && REVOHUMAN_HAS_SDK_TAXEL_LAYOUT + SdkLinks links{}; + return taxels_at(model, hand, angles, links); +#else + static_cast(hand); + static_cast(angles); + // Same refusal as the poses for a build without points. + make_hand_kinematics(model); + throw std::runtime_error( + "this build's C SDK has no taxel layout; use a C SDK with rh_hand_model_taxel_layout (0.1.3 or later)"); +#endif +} + +SdkTactileShape make_sdk_tactile_shape(HandModel model, Hand hand) +{ +#if REVOHUMAN_HAS_SDK_POINTS && REVOHUMAN_HAS_SDK_TAXEL_LAYOUT + SdkLinks links{}; + const SdkTaxels taxels = taxels_at(model, hand, {}, links); + // Bind every OpenXR joint as the mapping places it at all-zero angles, placeholders included. + std::array points{}; + if (hand_points_from_sdk_points(sdk_hand(hand), links.data(), points.data()) != RH_OK) + throw std::runtime_error("cannot map the SDK hand points to OpenXR joints"); + ShapeSpec spec; + for (std::size_t j = 0; j < points.size(); ++j) + { + Pose bind; + std::copy(std::begin(points[j].position), std::end(points[j].position), bind.p.begin()); + bind.q = quat_normalize( + { points[j].orientation[0], points[j].orientation[1], points[j].orientation[2], points[j].orientation[3] }); + spec.bind_poses.push_back(bind); + spec.joint_ids.push_back(static_cast(j)); + } + for (std::size_t i = 0; i < taxels.positions.size(); ++i) + { + const int joint = openxr_joint_of_sdk_link(taxels.links[i]); + if (joint < 0) + throw std::runtime_error("the C SDK puts taxel " + std::to_string(i) + " on link " + + std::to_string(taxels.links[i]) + ", which no OpenXR joint follows"); + spec.taxel_bind_poses.push_back({ taxels.positions[i], outward_z(taxels.normals[i]) }); + spec.joint_indices.push_back({ static_cast(joint), 0, 0, 0 }); + spec.joint_weights.push_back({ 1, 0, 0, 0 }); + } + return { build_shape_buffer(spec), taxels.source }; +#else + static_cast(hand); + make_hand_kinematics(model); + throw std::runtime_error( + "this build's C SDK has no taxel layout; use a C SDK with rh_hand_model_taxel_layout (0.1.3 or later)"); +#endif +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/kinematics/inc/revohuman_plugin/hand_kinematics.hpp b/src/plugins/revohuman/kinematics/inc/revohuman_plugin/hand_kinematics.hpp new file mode 100644 index 0000000000..4ecfb8ff14 --- /dev/null +++ b/src/plugins/revohuman/kinematics/inc/revohuman_plugin/hand_kinematics.hpp @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! Mechanical hand model. +enum class HandModel +{ + Dv1, //!< PTV1 gloves. + P, //!< P1 gloves. The SDK has no taxel layout for it: they have no tactile sensors. +}; + +//! The hand model of a product as the SDK identified it ("P1", "PTV1", ...), from the SDK's own table +//! (rh_variant_hand_model). Throws std::runtime_error for a product the SDK has no model for, and when +//! the build or the loaded SDK has no such table; there is no default model. +HandModel hand_model_of_product(const std::string& product_code); +//! Whether this build's C SDK has that table; without it no product has a hand model. +bool sdk_has_product_hand_models(); + +//! OpenXR hand joints from the SDK's point set for @p model (rh_hand_points), through the reviewed +//! OpenXR mapping. Throws std::runtime_error when the build or the loaded SDK has no point set matching +//! the reviewed mapping; there is no fallback to another model. +std::shared_ptr make_hand_kinematics(HandModel model); + +//! Where the taxels of a glove are, from the SDK's taxel layout (rh_hand_model_taxel_layout). +struct SdkTaxels +{ + std::vector positions; //!< Meters, in the WRIST frame of make_hand_kinematics(model). + std::vector normals; //!< Outward unit normals, same frame. + std::vector links; //!< SDK link each taxel is fixed to (rh_hand_model_fk order). + std::string source; //!< The layout's source note, e.g. "cuff-surface-v2@4e7da46". +}; + +//! The SDK's taxels at joint angles @p angles (all valid), one per value of a tactile frame. +//! Throws std::runtime_error when make_hand_kinematics(model) would, for a model without a layout (P), +//! and when the build or the loaded SDK has no taxel layout. +SdkTaxels sdk_taxels(HandModel model, Hand hand, const std::array& angles); + +//! TactileMetadata built from the SDK's taxel layout, ready for LoadedShape. +struct SdkTactileShape +{ + std::vector metadata; + std::string layout_source; +}; + +//! TactileMetadata of a glove: one segment per OpenXR joint (joint_ids 0..25) bound at all-zero joint +//! angles, and each taxel rigidly on the OpenXR joint that moves with its SDK link, with +Z along its +//! outward normal. Skinned with make_hand_kinematics(model) it follows the SDK's taxels at any joint +//! angles: every OpenXR joint that carries taxels is rigid on their SDK link. Throws as sdk_taxels() does. +SdkTactileShape make_sdk_tactile_shape(HandModel model, Hand hand); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/kinematics/openxr_model.inc b/src/plugins/revohuman/kinematics/openxr_model.inc new file mode 100644 index 0000000000..f03db0bdfa --- /dev/null +++ b/src/plugins/revohuman/kinematics/openxr_model.inc @@ -0,0 +1,115 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Reviewed OpenXR mapping, per hand (kModel0 left, kModel1 right): the WRIST rotation relative to palm_link +// (row-major) and the per-point side and height corrections, in meters. They were fitted on the SDK DV1 hand +// model. The P model differs from DV1 in link positions and, by 2.5e-4 rad, in its MCP flexion axes, so it +// takes the same corrections: a lateral shift then leaves its bones at most 3.3 um off rigid. The plugin refuses +// an SDK whose models differ from kReviewedModels; changing the corrections or the rules that apply them needs a +// new kMappingId. +constexpr const char* kMappingId = "reviewed-openxr-symmetric-v6"; +struct ReviewedModel +{ + const char* name; // rh_hand_model_info name + const char* sha256; // rh_hand_model_info urdf_sha256 +}; +constexpr ReviewedModel kReviewedModels[] = { + { "dv1", "9e343e1f1ffccee7bf8dbfcd24bf05b085ff400668c9bc3b071fd174b7d55592" }, + { "p", "b400ebf2df5abcbf1d8c88fd66ded8ec73cd5c35bb62cf28c92eb153e007edc2" }, +}; +// WRIST height along palm_link +X: the depth of PALM and of the middle and ring proximal joints, not of the +// tilted thumb anchor. +constexpr double kWristPadM = 0.01259782883756545; +constexpr double kPlaceholder[4][3] = { + { .018, .003, -.055 }, { .005, .002, -.055 }, { -.008, .003, -.055 }, { -.022, .005, -.05 } +}; +constexpr Model kModel0 = { + { -4.336808689942018e-19, -0.9996343707598901, 0.027039319434451838, -0.9959269287673426, 0.0024379740461849446, + 0.090131064781233, -0.09016403138751444, -0.026929186360312732, -0.9955627887611725 }, + { { { -1, { 0, 0, 0 }, 0 }, { -1, { 1, 0, 0 }, 0.018274150003565453 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 1, 0, 0 }, kWristPadM } }, + { { -1, { 0, 0, 0 }, 0 }, { 19, { 1, 0, 0 }, 0.014553057085492309 } }, + { { 19, { 0.0047418328189518235, 0.9850609625533788, 0.1721406839620662 }, 0.01180176096585353 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 19, { 0.0047418328189518235, 0.9850609625533788, 0.1721406839620662 }, 0.01180176096585353 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 19, { 0.0047418328189518235, 0.9850609625533788, 0.1721406839620662 }, 0.01180176096585353 }, + { -1, { 0, 0, 0 }, 0 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 0.9397675076223737, 0.3418143233061359, 0 }, 0.016408002819149843 } }, + { { -1, { 0, 0, 0 }, 0 }, { 3, { 0.9397675076223737, 0.3418143233061359, 0 }, 0.015901207797603602 } }, + { { 3, { -0.3274952970420892, 0.9003731890532454, 0.28648726123407803 }, -0.011798590037946272 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 3, { -0.3274952970420892, 0.9003731890532454, 0.28648726123407803 }, -0.011798590037946272 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 3, { -0.3274952970420892, 0.9003731890532454, 0.28648726123407803 }, -0.011798590037946272 }, + { -1, { 0, 0, 0 }, 0 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 1, 0, 0 }, 0.016084260147980083 } }, + { { -1, { 0, 0, 0 }, 0 }, { 7, { 1, 0, 0 }, 0.015897828909095454 } }, + { { 7, { 0.0001840165710279162, 0.9986812390005452, 0.05133954622157662 }, -0.01179821503258675 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 7, { 0.0001840165710279162, 0.9986812390005452, 0.05133954622157662 }, -0.01179821503258675 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 7, { 0.0001840165710279162, 0.9986812390005452, 0.05133954622157662 }, -0.01179821503258675 }, + { -1, { 0, 0, 0 }, 0 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 1, 0, 0 }, 0.01708389449829436 } }, + { { -1, { 0, 0, 0 }, 0 }, { 11, { 1, 0, 0 }, 0.015897828888649837 } }, + { { 11, { 0.00018401830736306681, 0.9896463406153593, -0.14352730278205209 }, -0.011798214814995772 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 11, { 0.00018401830736306681, 0.9896463406153593, -0.14352730278205209 }, -0.011798214814995772 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 11, { 0.00018401830736306681, 0.9896463406153593, -0.14352730278205209 }, -0.011798214814995772 }, + { -1, { 0, 0, 0 }, 0 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 0.9509901516808972, -0.3092211690779666, 0 }, 0.015727599194206895 } }, + { { -1, { 0, 0, 0 }, 0 }, { 15, { 0.9509901516808972, -0.3092211690779666, 0 }, 0.015894196511516582 } }, + { { 15, { 0.28925767822902526, 0.8896298104990777, -0.35339608919303084 }, -0.01179833398465048 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 15, { 0.28925767822902526, 0.8896298104990777, -0.35339608919303084 }, -0.01179833398465048 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 15, { 0.28925767822902526, 0.8896298104990777, -0.35339608919303084 }, -0.01179833398465048 }, + { -1, { 0, 0, 0 }, 0 } } } +}; +constexpr Model kModel1 = { + { 4.336808689942018e-19, -0.9996343707598901, 0.027039319434451838, -0.9959269287673426, -0.0024379740461849446, + -0.090131064781233, 0.09016403138751444, -0.026929186360312732, -0.9955627887611725 }, + { { { -1, { 0, 0, 0 }, 0 }, { -1, { 1, 0, 0 }, 0.018274150003565453 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 1, 0, 0 }, kWristPadM } }, + { { -1, { 0, 0, 0 }, 0 }, { 19, { 1, 0, 0 }, 0.014553057085492309 } }, + { { 19, { -0.0047418328189518235, 0.9850609625533788, -0.1721406839620662 }, -0.01180176096585353 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 19, { -0.0047418328189518235, 0.9850609625533788, -0.1721406839620662 }, -0.01180176096585353 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 19, { -0.0047418328189518235, 0.9850609625533788, -0.1721406839620662 }, -0.01180176096585353 }, + { -1, { 0, 0, 0 }, 0 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 0.9397675076223737, -0.3418143233061359, 0 }, 0.016408002819149843 } }, + { { -1, { 0, 0, 0 }, 0 }, { 3, { 0.9397675076223737, -0.3418143233061359, 0 }, 0.015901207797603602 } }, + { { 3, { 0.3274952970420892, 0.9003731890532454, -0.28648726123407803 }, 0.011798590037946272 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 3, { 0.3274952970420892, 0.9003731890532454, -0.28648726123407803 }, 0.011798590037946272 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 3, { 0.3274952970420892, 0.9003731890532454, -0.28648726123407803 }, 0.011798590037946272 }, + { -1, { 0, 0, 0 }, 0 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 1, 0, 0 }, 0.016084260147980083 } }, + { { -1, { 0, 0, 0 }, 0 }, { 7, { 1, 0, 0 }, 0.015897828909095454 } }, + { { 7, { -0.0001840165710279162, 0.9986812390005452, -0.05133954622157662 }, 0.01179821503258675 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 7, { -0.0001840165710279162, 0.9986812390005452, -0.05133954622157662 }, 0.01179821503258675 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 7, { -0.0001840165710279162, 0.9986812390005452, -0.05133954622157662 }, 0.01179821503258675 }, + { -1, { 0, 0, 0 }, 0 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 1, 0, 0 }, 0.01708389449829436 } }, + { { -1, { 0, 0, 0 }, 0 }, { 11, { 1, 0, 0 }, 0.015897828888649837 } }, + { { 11, { -0.00018401830736306681, 0.9896463406153593, 0.14352730278205209 }, 0.011798214814995772 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 11, { -0.00018401830736306681, 0.9896463406153593, 0.14352730278205209 }, 0.011798214814995772 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 11, { -0.00018401830736306681, 0.9896463406153593, 0.14352730278205209 }, 0.011798214814995772 }, + { -1, { 0, 0, 0 }, 0 } }, + { { -1, { 0, 0, 0 }, 0 }, { -1, { 0.9509901516808972, 0.3092211690779666, 0 }, 0.015727599194206895 } }, + { { -1, { 0, 0, 0 }, 0 }, { 15, { 0.9509901516808972, 0.3092211690779666, 0 }, 0.015894196511516582 } }, + { { 15, { -0.28925767822902526, 0.8896298104990777, 0.35339608919303084 }, 0.01179833398465048 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 15, { -0.28925767822902526, 0.8896298104990777, 0.35339608919303084 }, 0.01179833398465048 }, + { -1, { 0, 0, 0 }, 0 } }, + { { 15, { -0.28925767822902526, 0.8896298104990777, 0.35339608919303084 }, 0.01179833398465048 }, + { -1, { 0, 0, 0 }, 0 } } } +}; diff --git a/src/plugins/revohuman/live/CMakeLists.txt b/src/plugins/revohuman/live/CMakeLists.txt new file mode 100644 index 0000000000..65a4e731e1 --- /dev/null +++ b/src/plugins/revohuman/live/CMakeLists.txt @@ -0,0 +1,25 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Glove samples from the RevoHuman C SDK. Built from the header alone: whoever links it also links +# the SDK library (the plugin when it was found, the tests a fake of it). +add_library(revohuman_live STATIC + live_source.cpp + inc/revohuman_plugin/live_source.hpp + rh_frame.cpp + inc/revohuman_plugin/rh_frame.hpp +) + +target_include_directories(revohuman_live + INTERFACE + $ +) + +target_link_libraries(revohuman_live + PUBLIC + revohuman_plugin::revohuman_core + revohuman_plugin::sdk_headers + log_bridge::log_bridge_core +) + +add_library(revohuman_plugin::revohuman_live ALIAS revohuman_live) diff --git a/src/plugins/revohuman/live/inc/revohuman_plugin/live_source.hpp b/src/plugins/revohuman/live/inc/revohuman_plugin/live_source.hpp new file mode 100644 index 0000000000..345ce61568 --- /dev/null +++ b/src/plugins/revohuman/live/inc/revohuman_plugin/live_source.hpp @@ -0,0 +1,225 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +struct LiveSourceConfig +{ + double rate_hz = 200.0; + //! Frames the SDK buffers per glove between two polls (about 1.3 s at 200 Hz). + std::uint32_t queue_size = 256; + //! Open and close gloves on a worker thread, so that plugging in one glove never holds up the + //! other glove's samples. false does it inside poll(), for tests. + bool background_open = true; +}; + +/*! + * @brief Glove samples from the RevoHuman C SDK: at most one left and one right glove, found without + * configuration. Discovery, two-hand kits, retries and give-up follow the plugin README ("Real devices"). + * Samples come from the SDK's gated subscription, each with its CLOCK_MONOTONIC sampling time. A reopened + * glove starts over with a new HandMeta and SourceBatch::hand_restarted; the SDK cannot take one glove out + * of a two-hand kit, so the end of a paired glove's stream restarts both hands. + */ +class LiveSource final : public Source +{ +public: + struct HandStats + { + std::uint64_t frames = 0; //!< Frames pulled from the SDK. + std::uint64_t rejected = 0; //!< Frames that could not be converted. + std::uint64_t without_kinematics = 0; //!< Samples the provider gave no joint poses for. + std::uint64_t sdk_dropped = 0; //!< Frames the SDK dropped because its queue was full. + std::uint64_t opens = 0; //!< Times the glove was opened. + std::uint64_t ends = 0; //!< Times its subscription ended. + }; + + //! Tries to open each glove once. Throws std::invalid_argument for a bad config, and + //! std::runtime_error with the SDK error when the SDK cannot start or neither glove can ever be + //! opened (bad options, both gloves of a product without a provider, ...). Without a factory the + //! samples carry no kinematics. + explicit LiveSource(LiveSourceConfig config, KinematicsFactory kinematics = nullptr); + ~LiveSource() override; + + LiveSource(const LiveSource&) = delete; + LiveSource& operator=(const LiveSource&) = delete; + + SourceBatch poll(std::int64_t now_ns) override; + std::optional glove_note(Hand hand) const override; + std::uint64_t records_released() const override + { + return released_; + } + + //! Whether the hand's glove is open and streaming. + bool opened(Hand hand) const; + //! Whether the hand's open glove shares a two-hand kit with the other glove, sampled on the same + //! USB frame. + bool paired(Hand hand) const; + //! The product code the SDK identified on the open glove ("" while the glove is not open). + std::string product_code(Hand hand) const; + //! The model_id() of the open glove's provider ("" while the glove is not open or has none). + std::string model_id(Hand hand) const; + HandStats stats(Hand hand) const; + +private: + //! rh_init and the log callback; undone (rh_shutdown, callback removed) once every handle is closed. + struct SdkRuntime + { + SdkRuntime(); + ~SdkRuntime(); + SdkRuntime(const SdkRuntime&) = delete; + SdkRuntime& operator=(const SdkRuntime&) = delete; + }; + + struct HandStream + { + RhKit* kit = nullptr; + RhGlove* glove = nullptr; //!< Borrowed from the kit. + RhSubscription* sub = nullptr; + HandMeta meta; + std::shared_ptr kinematics; //!< For the identified product. + bool gate_ok = true; + RhSyncCause gate_cause = RH_CAUSE_NONE; + std::int64_t last_gate_check_ns = 0; + std::int64_t last_drop_report_ns = 0; + std::uint64_t reported_dropped = 0; + //! The kit holds both gloves: release() leaves it to release_pair(), which closes it once. + bool paired = false; + }; + + enum class OpenResult + { + Opened, + Retry, + Fatal + }; + + //! What one rh_kit_open attempt gave, made on whichever thread ran it. + struct OpenOutcome + { + OpenResult result = OpenResult::Fatal; + RhStatus status = RH_OK; + RhErrorCode code = RH_E_OK; + std::string error; + std::optional stream; + }; + + //! What one attempt to open both gloves gave: both from one two-hand kit, or else each from a + //! kit of its own, tried in the same attempt. + struct PairOutcome + { + std::array hands; //!< Indexed by Hand. + std::string error; //!< Why the two-hand kit did not open ("" when it did). + }; + + struct HandSlot + { + enum class State + { + Closed, //!< No glove; looked for at next_open_ns. + Opening, //!< An attempt is on the worker. + Open, + Failed, //!< Given up. + }; + State state = State::Closed; + std::optional stream; + HandStats stats; + std::uint64_t attempts = 0; + std::optional next_open_ns; //!< Set on the first poll() without a glove. + RhStatus open_status = RH_OK; + std::string error; //!< Why the last open failed, the stream ended, or the hand was given up. + bool meta_due = false; //!< The open glove's HandMeta has not been delivered yet. + bool streamed = false; //!< A HandMeta was delivered: the next one restarts the hand. + std::uint64_t dropped_before = 0; //!< sdk_dropped of the earlier streams. + }; + + //! One rh_kit_open attempt for the hand, and its subscription when the kit opens. Any thread. + OpenOutcome open_hand(Hand hand) const; + //! One rh_kit_open attempt for both gloves in one kit; when that fails, one for each on its own. + //! Any thread. + PairOutcome open_pair() const; + void open_stream(Hand hand, HandStream& stream) const; + //! Whether the SDK finds a glove for each hand on the USB bus, without opening them. + bool both_plugged_in() const; + void request_open(Hand hand, HandSlot& slot); + void request_pair(); + void adopt(Hand hand, HandSlot& slot, OpenOutcome outcome); + void adopt_pair(PairOutcome outcome); + //! Takes what the worker opened since the last poll(). + void take_opened(); + //! Looks again for the hands without a glove whose time has come: both together when neither + //! has one and the SDK finds both plugged in, else each on its own. + void look_for_gloves(std::int64_t now_ns); + //! Reopens both gloves in one kit when both stream on their own and are the same product. + void consider_pairing(std::int64_t now_ns); + //! Closes the subscription here and the kit on the worker (here once the worker stopped). + void release(HandStream& stream); + //! Closes the two-hand kit, once neither glove streams from it any more. + void release_pair(); + void close_kit(RhKit* kit); + void start_worker(); + void worker_loop(); + + void service(Hand hand, HandSlot& slot, std::int64_t now_ns, SourceBatch& batch); + //! Moves the queued samples into out; returns why the subscription ended, if it did. + std::optional pull(Hand hand, HandSlot& slot, std::vector& out); + void reject(Hand hand, HandSlot& slot, const std::string& reason); + void check_gate(Hand hand, HandStream& stream, std::int64_t now_ns); + void check_dropped(Hand hand, HandSlot& slot, std::int64_t now_ns); + void end_stream(Hand hand, HandSlot& slot, const std::string& reason, std::int64_t now_ns); + //! Releases the open glove and looks for it again in two seconds. + void close_stream(HandSlot& slot, std::string error, std::int64_t now_ns); + + //! First, so that it is destroyed last, after the destructor closed every handle. + std::optional runtime_; + LiveSourceConfig config_; + KinematicsFactory kinematics_; //!< Called from open_stream(), on either thread. + std::array slots_; //!< Indexed by Hand; poll() thread only. + RhKit* pair_kit_ = nullptr; //!< The two-hand kit the paired gloves stream from; poll() thread only. + //! The open gloves streamed on their own after they could not be opened together, so they are + //! not reopened together until either stream ends; poll() thread only. + bool unpairable_ = false; + std::string pair_error_; //!< Why the last attempt to open both gloves together failed. + std::uint64_t pair_failures_ = 0; + bool finished_ = false; + std::uint64_t released_ = 0; + + // Shared with the worker, guarded by mutex_. + std::mutex mutex_; + std::condition_variable wake_; + std::array open_wanted_{ false, false }; + bool pair_wanted_ = false; + std::array, 2> opened_; + std::optional pair_opened_; + std::vector to_close_; + bool stop_ = false; //!< Written only by the destructor. + std::thread worker_; //!< Started by the first open or close that goes to the background. +}; + +//! "E1101 TransportDisconnected (the glove's USB link dropped)" for the calling thread's last SDK error: +//! the error code, the SDK's exception class name and, for errors a user can act on, an English hint. +//! The SDK's own message after the class name is logged at debug level only. +std::string rh_error_text(); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/live/inc/revohuman_plugin/rh_frame.hpp b/src/plugins/revohuman/live/inc/revohuman_plugin/rh_frame.hpp new file mode 100644 index 0000000000..c341c1d17a --- /dev/null +++ b/src/plugins/revohuman/live/inc/revohuman_plugin/rh_frame.hpp @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! Converts one SDK glove frame. joints_rad comes from rh_joints_to_radians for the same frame; +//! the plugin never maps degrees itself. Throws std::invalid_argument when struct_size is smaller +//! than this build's RhGloveFrame, the hand is unknown, or the tactile point count is out of range. +//! Kinematics are left empty; the KinematicsProvider fills them in. +GloveSample from_rh_frame(const RhGloveFrame& frame, const std::array& joints_rad); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/live/live_source.cpp b/src/plugins/revohuman/live/live_source.cpp new file mode 100644 index 0000000000..de3520d19f --- /dev/null +++ b/src/plugins/revohuman/live/live_source.cpp @@ -0,0 +1,1159 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/live_source.hpp" + +#include "inc/revohuman_plugin/rh_frame.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +//! The SDK's capture gate is polled at most this often. +constexpr std::int64_t kGateCheckPeriodNs = 100'000'000; +//! SDK queue overflows are reported at most this often. +constexpr std::int64_t kDropReportPeriodNs = 1'000'000'000; +//! A hand without an open glove is looked for this often. +constexpr std::int64_t kOpenRetryPeriodNs = 2'000'000'000; + +std::shared_ptr live_logger() +{ + return isaaccapture::Logger::get("isaaccapture.plugins.revohuman.LiveSource"); +} + +const char* level_name(RhLogLevel level) +{ + if (level >= RH_LOG_ERROR) + { + return "error"; + } + if (level >= RH_LOG_WARNING) + { + return "warning"; + } + return level >= RH_LOG_INFO ? "info" : "debug"; +} + +// The SDK may log from any of its threads; spdlog loggers are thread-safe. The SDK's log lines are +// kept at debug level: everything the plugin acts on is reported by the plugin itself, with the +// SDK error code. +void forward_sdk_log(RhLogLevel level, const char* message, void*) +{ + static const auto logger = isaaccapture::Logger::get("isaaccapture.plugins.revohuman.sdk"); + logger->debug("SDK {}: {}", level_name(level), message != nullptr ? message : ""); +} + +const char* status_name(RhStatus status) +{ + switch (status) + { + case RH_OK: + return "RH_OK"; + case RH_ERR_INVALID_ARG: + return "RH_ERR_INVALID_ARG"; + case RH_ERR_NOT_INITIALIZED: + return "RH_ERR_NOT_INITIALIZED"; + case RH_ERR_NOT_FOUND: + return "RH_ERR_NOT_FOUND"; + case RH_ERR_TIMEOUT: + return "RH_ERR_TIMEOUT"; + case RH_ERR_PROTOCOL: + return "RH_ERR_PROTOCOL"; + case RH_ERR_DISCONNECTED: + return "RH_ERR_DISCONNECTED"; + case RH_ERR_BUFFER_TOO_SMALL: + return "RH_ERR_BUFFER_TOO_SMALL"; + case RH_ERR_UNSUPPORTED: + return "RH_ERR_UNSUPPORTED"; + case RH_ERR_CANCELLED: + return "RH_ERR_CANCELLED"; + case RH_ERR_INVALID_DATA: + return "RH_ERR_INVALID_DATA"; + case RH_ERR_BUSY: + return "RH_ERR_BUSY"; + case RH_ERR_STATE: + return "RH_ERR_STATE"; + case RH_ERR_CONFLICT: + return "RH_ERR_CONFLICT"; + case RH_ERR_IO: + return "RH_ERR_IO"; + case RH_ERR_INTERNAL: + return "RH_ERR_INTERNAL"; + default: + return "unknown status"; + } +} + +//! What the user can do about the errors a glove open or stream meets; nullptr where the class name says it. +const char* error_hint(RhErrorCode code) +{ + switch (code) + { + case RH_E_DISCONNECTED: + return "the glove's USB link dropped"; + case RH_E_PORT_BUSY: + return "another process holds the glove's serial port"; + case RH_E_FIRMWARE_INCOMPATIBLE: + case RH_E_TIME_SYNC_INCOMPATIBLE: + case RH_E_TIME_SYNC_UNSUPPORTED: + return "the glove firmware is not supported by this SDK; update the firmware"; + case RH_E_UNREGISTERED_GLOVE: + return "a glove is plugged in, but its MCU serial number does not say which hand it is"; + case RH_E_STREAM_RATE_CONFLICT: + return "the glove already streams at another rate"; + case RH_E_VARIANT_MISMATCH: + case RH_E_PRODUCT_UNIDENTIFIED: + case RH_E_PRODUCT_MISMATCH: + return "the SDK cannot identify the glove product"; + default: + return nullptr; + } +} + +//! An absent glove, as opposed to a plugged-in glove the SDK cannot assign to a hand (E1400), which +//! the SDK reports with the same status. +bool glove_absent(RhStatus status, RhErrorCode code) +{ + return status == RH_ERR_NOT_FOUND && code != RH_E_UNREGISTERED_GLOVE; +} + +//! A failed SDK call with the status and error code it left, so a failed open can be retried. +struct SdkError : std::runtime_error +{ + SdkError(RhStatus status, RhErrorCode code, const std::string& what) + : std::runtime_error(what), status(status), code(code) + { + } + RhStatus status; + RhErrorCode code; +}; + +void check(RhStatus status, const char* what) +{ + if (status != RH_OK) + { + const RhErrorCode code = rh_last_error_code(); + throw SdkError(status, code, + std::string("LiveSource: ") + what + " failed (" + status_name(status) + "): " + rh_error_text()); + } +} + +template +std::string field(const char (&chars)[N]) +{ + return std::string(chars, strnlen(chars, N)); +} + +RhHand to_rh(Hand hand) +{ + return hand == Hand::Left ? RH_HAND_LEFT : RH_HAND_RIGHT; +} + +std::size_t slot_index(Hand hand) +{ + return static_cast(hand); +} + +const char* cause_name(RhSyncCause cause) +{ + switch (cause) + { + case RH_CAUSE_NONE: + return "none"; + case RH_CAUSE_CLOCK_NOT_READY: + return "clock_not_ready"; + case RH_CAUSE_CLOCK_DEGRADED: + return "clock_degraded"; + case RH_CAUSE_HOST_CLOCK_EVENT: + return "host_clock_event"; + case RH_CAUSE_DEVICE_RESET: + return "device_reset"; + case RH_CAUSE_LINK_UNSTABLE: + return "link_unstable"; + default: + return "unknown"; + } +} + +//! The glove's firmware SN, which every RevoHuman glove has. The SDK's unit id names the USB port the +//! glove is plugged into and the USB serial is not unique to a glove, so neither identifies it; both are +//! logged when it opens. The hand name stands in only if the SDK reports an empty SN, which open_stream() +//! warns about. +std::string device_id_of(const RhGloveInfo& info, Hand hand) +{ + std::string id = field(info.sn); + if (id.empty()) + { + id = std::string("revohuman_") + hand_name(hand); + } + return id; +} + +//! Whether opening a glove may succeed later without a configuration change: no glove plugged in yet, +//! a port held by another process, or a link that dropped while the kit identified the glove. A glove +//! the SDK cannot assign to a hand (E1400) is retried too, since plugging in the other glove or +//! replugging this one can settle it, but it is reported as a problem, not as an absent glove. +//! A wrong product or variant, incompatible firmware and bad options stay wrong. +bool open_retryable(RhStatus status, RhErrorCode code) +{ + switch (status) + { + case RH_ERR_NOT_FOUND: + case RH_ERR_TIMEOUT: + case RH_ERR_DISCONNECTED: + case RH_ERR_BUSY: + case RH_ERR_STATE: + case RH_ERR_IO: + return true; + case RH_ERR_PROTOCOL: + return code != RH_E_FIRMWARE_INCOMPATIBLE && code != RH_E_TIME_SYNC_INCOMPATIBLE && + code != RH_E_TIME_SYNC_UNSUPPORTED && code != RH_E_UNSUPPORTED_FEATURE; + case RH_ERR_CONFLICT: + // The SDK asks for a retry when a port re-enumerated during identification. + return code != RH_E_PRODUCT_MISMATCH && code != RH_E_VARIANT_MISMATCH && code != RH_E_STREAM_RATE_CONFLICT; + default: + return false; + } +} + +//! True for 1, 10, 100, ...: repeated problems are logged with decreasing frequency. +bool worth_logging(std::uint64_t count) +{ + while (count >= 10 && count % 10 == 0) + { + count /= 10; + } + return count == 1; +} + +} // namespace + +std::string rh_error_text() +{ + const RhErrorCode code = rh_last_error_code(); + // "E1200 StreamOptionsRejected: ": the class name is the actual exception class, which may be a + // subclass without a code of its own; the text after it goes to the debug log only. + std::string head; + char buf[512]; + if (rh_last_error(buf, sizeof(buf)) > 0) + { + const std::string full(buf); + live_logger()->debug("SDK error: {}", full); + head = full.substr(0, full.find(':')); + } + if (head.empty()) + { + const char* class_name = rh_error_class_name(code); + head = "E" + std::to_string(code); + if (class_name != nullptr) + { + head += std::string(" ") + class_name; + } + } + const char* hint = error_hint(code); + return hint != nullptr ? head + " (" + hint + ")" : head; +} + +namespace +{ +// rh_shutdown is process-wide: only the last live source may call it. +std::mutex g_runtime_mutex; +int g_runtime_users = 0; +} // namespace + +LiveSource::SdkRuntime::SdkRuntime() +{ + std::lock_guard lock(g_runtime_mutex); + if (g_runtime_users == 0) + { + RhInitOptions init; + rh_init_options_init(&init); + check(rh_init(&init), "rh_init"); + rh_set_log_callback(&forward_sdk_log, nullptr); + } + ++g_runtime_users; +} + +LiveSource::SdkRuntime::~SdkRuntime() +{ + std::lock_guard lock(g_runtime_mutex); + if (--g_runtime_users == 0) + { + // Every handle is closed by now; rh_shutdown closes (and warns about) any the plugin leaked. Once the + // callback is removed the SDK no longer calls into the plugin, which may be about to unload. + rh_shutdown(); + rh_set_log_callback(nullptr, nullptr); + } +} + +LiveSource::LiveSource(LiveSourceConfig config, KinematicsFactory kinematics) + : config_(std::move(config)), kinematics_(std::move(kinematics)) +{ + if (config_.queue_size == 0) + { + throw std::invalid_argument("LiveSource: queue_size must be positive"); + } + runtime_.emplace(); + if (kinematics_) + { + live_logger()->info("RevoHuman C SDK {}", rh_version()); + } + else + { + live_logger()->info("RevoHuman C SDK {}, no hand kinematics (the hands are not injected)", rh_version()); + } + + // Nothing streams yet, so the first attempts may block here. + if (both_plugged_in()) + { + for (HandSlot& slot : slots_) + { + ++slot.attempts; + } + adopt_pair(open_pair()); + } + else + { + for (const Hand hand : { Hand::Left, Hand::Right }) + { + HandSlot& slot = slots_[slot_index(hand)]; + ++slot.attempts; + adopt(hand, slot, open_hand(hand)); + } + } + const HandSlot& left = slots_[slot_index(Hand::Left)]; + const HandSlot& right = slots_[slot_index(Hand::Right)]; + if (left.state == HandSlot::State::Failed && right.state == HandSlot::State::Failed) + { + throw std::runtime_error("LiveSource: cannot open either glove: left: " + left.error + "; right: " + right.error); + } +} + +LiveSource::OpenOutcome LiveSource::open_hand(Hand hand) const +{ + OpenOutcome out; + RhKitOptions options; + rh_kit_options_init(&options); + options.hands = hand == Hand::Left ? RH_HANDS_LEFT : RH_HANDS_RIGHT; + // The SDK identifies the product; the hand model follows from it. + options.product = "auto"; + options.rate_hz = config_.rate_hz; + // No SDK tracker: the wrist pose comes from the host, like the other hand plugins. + options.tracker_config_json = nullptr; + RhKit* kit = nullptr; + out.status = rh_kit_open(&options, &kit); + if (out.status != RH_OK) + { + // The SDK's last error belongs to this thread, so it is read here. + out.code = rh_last_error_code(); + out.error = std::string(status_name(out.status)) + ": " + rh_error_text(); + out.result = open_retryable(out.status, out.code) ? OpenResult::Retry : OpenResult::Fatal; + return out; + } + HandStream stream; + stream.kit = kit; + try + { + open_stream(hand, stream); + } + catch (const std::runtime_error& e) + { + if (stream.sub != nullptr) + { + rh_sub_close(stream.sub); + } + rh_kit_close(kit); + // A glove unplugged between rh_kit_open and rh_glove_subscribe is looked for again. + const auto* sdk = dynamic_cast(&e); + if (sdk != nullptr) + { + out.status = sdk->status; + out.code = sdk->code; + } + out.result = sdk != nullptr && open_retryable(sdk->status, sdk->code) ? OpenResult::Retry : OpenResult::Fatal; + out.error = e.what(); + return out; + } + out.result = OpenResult::Opened; + out.stream = std::move(stream); + return out; +} + +LiveSource::PairOutcome LiveSource::open_pair() const +{ + PairOutcome out; + RhKitOptions options; + rh_kit_options_init(&options); + options.hands = RH_HANDS_BOTH; + options.product = "auto"; + // Both gloves join one sync session and are sampled on the same USB frame; the open fails when + // they cannot be synced. + options.sync = RH_SYNC_ON; + options.rate_hz = config_.rate_hz; + options.tracker_config_json = nullptr; + RhKit* kit = nullptr; + if (rh_kit_open(&options, &kit) == RH_OK) + { + std::array streams; + try + { + for (const Hand hand : { Hand::Left, Hand::Right }) + { + HandStream& stream = streams[slot_index(hand)]; + stream.kit = kit; + stream.paired = true; + open_stream(hand, stream); + } + for (const Hand hand : { Hand::Left, Hand::Right }) + { + OpenOutcome& hand_out = out.hands[slot_index(hand)]; + hand_out.result = OpenResult::Opened; + hand_out.stream = std::move(streams[slot_index(hand)]); + } + return out; + } + catch (const std::runtime_error& e) + { + for (const HandStream& stream : streams) + { + if (stream.sub != nullptr) + { + rh_sub_close(stream.sub); + } + } + rh_kit_close(kit); + out.error = e.what(); + } + } + else + { + out.error = rh_error_text(); + } + // A glove that cannot be opened with the other may still stream on its own. + for (const Hand hand : { Hand::Left, Hand::Right }) + { + out.hands[slot_index(hand)] = open_hand(hand); + } + return out; +} + +bool LiveSource::both_plugged_in() const +{ + RhScanOptions options; + rh_scan_options_init(&options); + // Only reads sysfs, so no port is opened and a glove another process holds still counts. + options.probe = 0; + RhScanList* list = nullptr; + if (rh_scan(&options, &list) != RH_OK) + { + live_logger()->debug("cannot scan for gloves: {}", rh_error_text()); + return false; + } + std::array found{ false, false }; + const std::size_t count = rh_scan_count(list); + for (std::size_t i = 0; i < count; ++i) + { + RhDiscoveredGlove glove = RH_STRUCT_INIT(RhDiscoveredGlove); + // Without a hand (USB serial collision) or a port, rh_kit_open cannot find the glove either. + if (rh_scan_get(list, i, &glove) != RH_OK || glove.port[0] == '\0') + { + continue; + } + if (glove.hand == RH_HAND_LEFT) + { + found[slot_index(Hand::Left)] = true; + } + else if (glove.hand == RH_HAND_RIGHT) + { + found[slot_index(Hand::Right)] = true; + } + } + rh_scan_free(list); + return found[0] && found[1]; +} + +void LiveSource::open_stream(Hand hand, HandStream& stream) const +{ + auto logger = live_logger(); + HandMeta& meta = stream.meta; + meta.hand = hand; + // Joint mapping and hand model both follow the identified product; there is no default to fall back to. + RhProductIdentity identity = RH_STRUCT_INIT(RhProductIdentity); + check(rh_kit_identity(stream.kit, &identity), "rh_kit_identity"); + meta.product_code = field(identity.variant_code); + if (meta.product_code.empty()) + { + throw std::runtime_error("LiveSource: the SDK opened the glove without identifying its product"); + } + // A product without tactile sensors (P1) still streams joints and the hand. + meta.has_tactile = (identity.variant_sensors & RH_SENSOR_TACTILE) != 0; + if (kinematics_) + { + try + { + stream.kinematics = kinematics_(meta.product_code); + } + catch (const std::exception& e) + { + throw std::runtime_error("LiveSource: no hand kinematics for product '" + meta.product_code + + "': " + e.what()); + } + } + + stream.glove = rh_kit_glove(stream.kit, to_rh(hand)); + if (stream.glove == nullptr) + { + throw std::runtime_error(std::string("LiveSource: the kit opened no ") + hand_name(hand) + " glove"); + } + RhGloveInfo info = RH_STRUCT_INIT(RhGloveInfo); + check(rh_glove_info(stream.glove, &info), "rh_glove_info"); + meta.device_id = device_id_of(info, hand); + if (field(info.sn).empty()) + { + logger->warn("{}: the SDK reports no SN for the glove; its device id is the hand name", hand_name(hand)); + } + for (std::size_t i = 0; i < kGloveJointCount; ++i) + { + char name[RH_ID_MAX + 16]; + const std::int32_t length = rh_joint_name(to_rh(hand), static_cast(i), name, sizeof(name)); + if (length < 0) + { + throw std::runtime_error("LiveSource: rh_joint_name " + std::to_string(i) + " failed: " + rh_error_text()); + } + if (static_cast(length) >= sizeof(name)) + { + throw std::runtime_error("LiveSource: rh_joint_name " + std::to_string(i) + " is longer than " + + std::to_string(sizeof(name) - 1) + " characters"); + } + meta.joint_names[i] = name; + } + RhSubscribeOptions subscribe; + rh_subscribe_options_init(&subscribe); + subscribe.queue_size = config_.queue_size; + subscribe.ungated = 0; + // No callback: frames are pulled on poll(), so the SDK starts no thread for them. + check(rh_glove_subscribe(stream.glove, &subscribe, nullptr, nullptr, &stream.sub), "rh_glove_subscribe"); + + logger->info( + "{}: glove {} (product '{}', port {}, unit {}, USB serial {}, firmware {}, tactile {}), hand model {}", + hand_name(hand), meta.device_id, meta.product_code, field(info.port), field(info.unit_id), + field(info.usb_serial), field(info.fw_version), + meta.has_tactile ? std::to_string(info.tactile_point_count) + " taxels" : std::string("none on this product"), + stream.kinematics ? stream.kinematics->model_id() : std::string("none")); +} + +void LiveSource::adopt(Hand hand, HandSlot& slot, OpenOutcome outcome) +{ + auto logger = live_logger(); + const bool changed = outcome.status != slot.open_status || outcome.error != slot.error; + slot.open_status = outcome.status; + switch (outcome.result) + { + case OpenResult::Opened: + slot.state = HandSlot::State::Open; + slot.stream = std::move(outcome.stream); + slot.error.clear(); + slot.meta_due = true; + ++slot.stats.opens; + if (slot.attempts > 1) + { + logger->info("{}: glove opened after {} attempts", hand_name(hand), slot.attempts); + } + break; + case OpenResult::Retry: + slot.state = HandSlot::State::Closed; + slot.error = std::move(outcome.error); + if (glove_absent(outcome.status, outcome.code)) + { + // An absent glove is normal, for example when only one hand is used. + if (changed) + { + logger->info("{}: no glove ({}); looking for it every {} s", hand_name(hand), slot.error, + kOpenRetryPeriodNs / 1'000'000'000); + } + } + else if (changed || worth_logging(slot.attempts)) + { + logger->warn("{}: cannot open the glove yet ({} attempts): {}", hand_name(hand), slot.attempts, slot.error); + } + break; + case OpenResult::Fatal: + slot.state = HandSlot::State::Failed; + slot.error = std::move(outcome.error); + logger->error("{}: giving up on the glove: {}", hand_name(hand), slot.error); + break; + } +} + +void LiveSource::adopt_pair(PairOutcome outcome) +{ + auto logger = live_logger(); + OpenOutcome& left = outcome.hands[slot_index(Hand::Left)]; + OpenOutcome& right = outcome.hands[slot_index(Hand::Right)]; + if (outcome.error.empty()) + { + pair_kit_ = left.stream->kit; + pair_error_.clear(); + pair_failures_ = 0; + logger->info("both gloves open in one kit, sampled on the same USB frame"); + } + else + { + const bool changed = outcome.error != pair_error_; + pair_error_ = std::move(outcome.error); + ++pair_failures_; + if (left.result == OpenResult::Opened && right.result == OpenResult::Opened) + { + unpairable_ = true; + logger->warn( + "cannot open the gloves in one kit ({}); each streams on its own, not sampled on the same " + "USB frame", + pair_error_); + } + else if (changed || worth_logging(pair_failures_)) + { + logger->info("cannot open the gloves in one kit ({} attempts): {}", pair_failures_, pair_error_); + } + } + adopt(Hand::Left, slots_[slot_index(Hand::Left)], std::move(left)); + adopt(Hand::Right, slots_[slot_index(Hand::Right)], std::move(right)); +} + +void LiveSource::take_opened() +{ + std::array, 2> opened; + std::optional pair; + { + std::lock_guard lock(mutex_); + opened.swap(opened_); + pair.swap(pair_opened_); + } + if (pair) + { + adopt_pair(std::move(*pair)); + } + for (const Hand hand : { Hand::Left, Hand::Right }) + { + if (opened[slot_index(hand)]) + { + adopt(hand, slots_[slot_index(hand)], std::move(*opened[slot_index(hand)])); + } + } +} + +void LiveSource::look_for_gloves(std::int64_t now_ns) +{ + std::array due{ false, false }; + for (std::size_t i = 0; i < slots_.size(); ++i) + { + HandSlot& slot = slots_[i]; + if (slot.state != HandSlot::State::Closed) + { + continue; + } + // The constructor made the first attempt; the first poll only starts the retry clock. + due[i] = slot.next_open_ns && now_ns >= *slot.next_open_ns; + if (!slot.next_open_ns || due[i]) + { + slot.next_open_ns = now_ns + kOpenRetryPeriodNs; + } + } + if (!due[0] && !due[1]) + { + return; + } + if (slots_[0].state == HandSlot::State::Closed && slots_[1].state == HandSlot::State::Closed && both_plugged_in()) + { + for (HandSlot& slot : slots_) + { + slot.next_open_ns = now_ns + kOpenRetryPeriodNs; + } + request_pair(); + return; + } + for (const Hand hand : { Hand::Left, Hand::Right }) + { + if (due[slot_index(hand)]) + { + request_open(hand, slots_[slot_index(hand)]); + } + } +} + +void LiveSource::consider_pairing(std::int64_t now_ns) +{ + using State = HandSlot::State; + const HandSlot& left = slots_[slot_index(Hand::Left)]; + const HandSlot& right = slots_[slot_index(Hand::Right)]; + // A glove plugged in while the other streams opens on its own first, so that the other glove + // does not restart for two products that cannot share a kit. + if (left.state != State::Open || right.state != State::Open || left.stream->paired || right.stream->paired || + left.stream->meta.product_code != right.stream->meta.product_code || unpairable_) + { + return; + } + live_logger()->info( + "both gloves stream on their own; reopening them in one kit to sample them on the same USB " + "frame"); + for (HandSlot& slot : slots_) + { + close_stream(slot, "reopening it in one kit with the other glove", now_ns); + } + request_pair(); +} + +void LiveSource::request_pair() +{ + for (HandSlot& slot : slots_) + { + ++slot.attempts; + } + if (!config_.background_open) + { + adopt_pair(open_pair()); + return; + } + for (HandSlot& slot : slots_) + { + slot.state = HandSlot::State::Opening; + } + start_worker(); + { + std::lock_guard lock(mutex_); + pair_wanted_ = true; + } + wake_.notify_one(); +} + +void LiveSource::request_open(Hand hand, HandSlot& slot) +{ + ++slot.attempts; + if (!config_.background_open) + { + adopt(hand, slot, open_hand(hand)); + return; + } + slot.state = HandSlot::State::Opening; + start_worker(); + { + std::lock_guard lock(mutex_); + open_wanted_[slot_index(hand)] = true; + } + wake_.notify_one(); +} + +void LiveSource::release(HandStream& stream) +{ + // Subscriptions close before their kit. + if (stream.sub != nullptr) + { + rh_sub_close(stream.sub); + stream.sub = nullptr; + } + stream.glove = nullptr; + RhKit* kit = std::exchange(stream.kit, nullptr); + // The other glove may still stream from a two-hand kit. + if (kit != nullptr && !stream.paired) + { + close_kit(kit); + } +} + +void LiveSource::release_pair() +{ + for (const HandSlot& slot : slots_) + { + if (slot.stream && slot.stream->paired) + { + return; + } + } + if (RhKit* kit = std::exchange(pair_kit_, nullptr)) + { + close_kit(kit); + } +} + +void LiveSource::close_kit(RhKit* kit) +{ + if (!config_.background_open || stop_) + { + rh_kit_close(kit); + return; + } + // Closing stops the device stream, which can wait on a glove that is going away. + start_worker(); + { + std::lock_guard lock(mutex_); + to_close_.push_back(kit); + } + wake_.notify_one(); +} + +void LiveSource::start_worker() +{ + if (!worker_.joinable()) + { + worker_ = std::thread(&LiveSource::worker_loop, this); + } +} + +void LiveSource::worker_loop() +{ + std::unique_lock lock(mutex_); + for (;;) + { + wake_.wait( + lock, [this] { return stop_ || !to_close_.empty() || pair_wanted_ || open_wanted_[0] || open_wanted_[1]; }); + if (stop_) + { + return; + } + // Closes first: a glove looked for again needs its old kit to let go of the port. + if (!to_close_.empty()) + { + RhKit* kit = to_close_.back(); + to_close_.pop_back(); + lock.unlock(); + rh_kit_close(kit); + lock.lock(); + continue; + } + if (pair_wanted_) + { + pair_wanted_ = false; + lock.unlock(); + PairOutcome outcome = open_pair(); + lock.lock(); + pair_opened_ = std::move(outcome); + continue; + } + const Hand hand = open_wanted_[slot_index(Hand::Left)] ? Hand::Left : Hand::Right; + open_wanted_[slot_index(hand)] = false; + lock.unlock(); + OpenOutcome outcome = open_hand(hand); + lock.lock(); + opened_[slot_index(hand)] = std::move(outcome); + } +} + +LiveSource::~LiveSource() +{ + if (worker_.joinable()) + { + { + std::lock_guard lock(mutex_); + stop_ = true; + } + wake_.notify_all(); + // An open in progress finishes first; requests not started yet are dropped. + worker_.join(); + } + stop_ = true; + // Every subscription closes before its kit, so a two-hand kit closes after both of its gloves. + std::vector pair_kits; + const auto let_go = [&pair_kits, this](HandStream& stream) + { + if (stream.paired && stream.kit != nullptr && + std::find(pair_kits.begin(), pair_kits.end(), stream.kit) == pair_kits.end()) + { + pair_kits.push_back(stream.kit); + } + release(stream); + }; + for (HandSlot& slot : slots_) + { + if (slot.stream) + { + let_go(*slot.stream); + } + } + for (auto& outcome : opened_) + { + if (outcome && outcome->stream) + { + let_go(*outcome->stream); + } + } + if (pair_opened_) + { + for (OpenOutcome& outcome : pair_opened_->hands) + { + if (outcome.stream) + { + let_go(*outcome.stream); + } + } + } + pair_kit_ = nullptr; + for (RhKit* kit : pair_kits) + { + rh_kit_close(kit); + } + for (RhKit* kit : to_close_) + { + rh_kit_close(kit); + } +} + +SourceBatch LiveSource::poll(std::int64_t now_ns) +{ + SourceBatch batch; + if (!finished_) + { + take_opened(); + look_for_gloves(now_ns); + // Before service(), so a glove reopened in one kit right after it opened on its own + // delivers only the HandMeta of the kit it streams from. + consider_pairing(now_ns); + for (const Hand hand : { Hand::Left, Hand::Right }) + { + service(hand, slots_[slot_index(hand)], now_ns, batch); + } + if (slots_[0].state == HandSlot::State::Failed && slots_[1].state == HandSlot::State::Failed) + { + finished_ = true; + live_logger()->error("both gloves given up; no more samples"); + } + } + released_ += batch.records.size(); + batch.finished = finished_; + return batch; +} + +void LiveSource::service(Hand hand, HandSlot& slot, std::int64_t now_ns, SourceBatch& batch) +{ + if (slot.state != HandSlot::State::Open) + { + return; + } + if (slot.meta_due) + { + batch.records.push_back(slot.stream->meta); + batch.hand_restarted[slot_index(hand)] = slot.streamed; + slot.streamed = true; + slot.meta_due = false; + } + if (std::optional ended = pull(hand, slot, batch.records)) + { + end_stream(hand, slot, *ended, now_ns); + return; + } + check_gate(hand, *slot.stream, now_ns); + check_dropped(hand, slot, now_ns); +} + +std::optional LiveSource::pull(Hand hand, HandSlot& slot, std::vector& out) +{ + HandStream& stream = *slot.stream; + const std::string& product_code = stream.meta.product_code; + // At most one queue's worth per poll, so a fast producer cannot hold up the tick loop. + for (std::uint32_t n = 0; n < config_.queue_size; ++n) + { + RhGloveFrame frame = RH_STRUCT_INIT(RhGloveFrame); + const RhStatus status = rh_sub_recv_glove(stream.sub, 0, &frame); + if (status == RH_ERR_TIMEOUT) + { + return std::nullopt; + } + if (status != RH_OK) + { + return rh_error_text(); + } + ++slot.stats.frames; + + std::array joints_rad{}; + if (rh_joints_to_radians(product_code.c_str(), frame.joints_deg, joints_rad.data()) != RH_OK) + { + reject(hand, slot, "rh_joints_to_radians failed: " + rh_error_text()); + continue; + } + GloveSample sample; + try + { + sample = from_rh_frame(frame, joints_rad); + } + catch (const std::invalid_argument& e) + { + reject(hand, slot, e.what()); + continue; + } + if (sample.hand != hand) + { + reject(hand, slot, std::string("frame of the ") + hand_name(sample.hand) + " glove"); + continue; + } + if (!sample.has_sample_time) + { + // Gated subscriptions only carry frames with a sampling time; without one the sample + // cannot be aligned with anything. + reject(hand, slot, "gated frame without a sampling time"); + continue; + } + if (stream.kinematics) + { + sample.kinematics = stream.kinematics->solve(hand, sample.joints_rad, sample.joints_valid_mask); + } + if (!sample.kinematics) + { + ++slot.stats.without_kinematics; + } + out.push_back(std::move(sample)); + } + return std::nullopt; +} + +void LiveSource::reject(Hand hand, HandSlot& slot, const std::string& reason) +{ + if (worth_logging(++slot.stats.rejected)) + { + live_logger()->warn("{}: glove frame dropped ({} so far): {}", hand_name(hand), slot.stats.rejected, reason); + } +} + +void LiveSource::check_gate(Hand hand, HandStream& stream, std::int64_t now_ns) +{ + if (stream.last_gate_check_ns != 0 && now_ns - stream.last_gate_check_ns < kGateCheckPeriodNs) + { + return; + } + stream.last_gate_check_ns = now_ns; + const bool ok = rh_glove_capture_ok(stream.glove); + const RhSyncCause cause = ok ? RH_CAUSE_NONE : rh_glove_capture_cause(stream.glove); + if (ok == stream.gate_ok && cause == stream.gate_cause) + { + return; + } + if (ok) + { + live_logger()->info("{}: capture gate open", hand_name(hand)); + } + else + { + live_logger()->warn("{}: capture gate closed ({}); the glove delivers no samples until it opens", + hand_name(hand), cause_name(cause)); + } + stream.gate_ok = ok; + stream.gate_cause = cause; +} + +void LiveSource::check_dropped(Hand hand, HandSlot& slot, std::int64_t now_ns) +{ + HandStream& stream = *slot.stream; + const std::uint64_t dropped = rh_sub_dropped(stream.sub); + slot.stats.sdk_dropped = slot.dropped_before + dropped; + if (dropped == stream.reported_dropped || + (stream.last_drop_report_ns != 0 && now_ns - stream.last_drop_report_ns < kDropReportPeriodNs)) + { + return; + } + live_logger()->warn("{}: the SDK dropped {} glove frames (queue of {}); the plugin polls too slowly", + hand_name(hand), dropped - stream.reported_dropped, config_.queue_size); + stream.reported_dropped = dropped; + stream.last_drop_report_ns = now_ns; +} + +void LiveSource::end_stream(Hand hand, HandSlot& slot, const std::string& reason, std::int64_t now_ns) +{ + auto logger = live_logger(); + const bool paired = slot.stream->paired; + ++slot.stats.ends; + // A glove plugged in again may be one that can share a kit with the other. + unpairable_ = false; + close_stream(slot, "the glove stream ended: " + reason, now_ns); + if (!paired) + { + logger->warn("{}: glove stream ended ({}); looking for the glove again", hand_name(hand), reason); + return; + } + logger->warn("{}: glove stream ended ({}); looking for both gloves again", hand_name(hand), reason); + // The SDK cannot take one glove out of a two-hand kit, so the other glove closes with it. + const Hand mate = hand == Hand::Left ? Hand::Right : Hand::Left; + HandSlot& other = slots_[slot_index(mate)]; + if (other.state == HandSlot::State::Open && other.stream->paired) + { + ++other.stats.ends; + close_stream(other, std::string("the paired ") + hand_name(hand) + " glove's stream ended: " + reason, now_ns); + logger->warn("{}: closing the glove with the paired {} glove; looking for both gloves again", hand_name(mate), + hand_name(hand)); + } + release_pair(); +} + +void LiveSource::close_stream(HandSlot& slot, std::string error, std::int64_t now_ns) +{ + slot.stats.sdk_dropped = slot.dropped_before + rh_sub_dropped(slot.stream->sub); + slot.dropped_before = slot.stats.sdk_dropped; + release(*slot.stream); + slot.stream.reset(); + slot.state = HandSlot::State::Closed; + slot.open_status = RH_OK; + slot.error = std::move(error); + slot.meta_due = false; + slot.attempts = 0; + slot.next_open_ns = now_ns + kOpenRetryPeriodNs; +} + +std::optional LiveSource::glove_note(Hand hand) const +{ + const HandSlot& slot = slots_[slot_index(hand)]; + switch (slot.state) + { + case HandSlot::State::Failed: + return GloveNote{ .kind = GloveNote::Kind::Ended, .text = "cannot open the glove: " + slot.error }; + case HandSlot::State::Closed: + case HandSlot::State::Opening: + return GloveNote{ .kind = GloveNote::Kind::Waiting, .text = "waiting for the glove: " + slot.error }; + case HandSlot::State::Open: + break; + } + if (!slot.stream->gate_ok) + { + return GloveNote{ .kind = GloveNote::Kind::NotReady, + .text = std::string("capture gate closed (") + cause_name(slot.stream->gate_cause) + ")" }; + } + return std::nullopt; +} + +bool LiveSource::opened(Hand hand) const +{ + return slots_[slot_index(hand)].state == HandSlot::State::Open; +} + +bool LiveSource::paired(Hand hand) const +{ + const HandSlot& slot = slots_[slot_index(hand)]; + return slot.state == HandSlot::State::Open && slot.stream->paired; +} + +std::string LiveSource::product_code(Hand hand) const +{ + const HandSlot& slot = slots_[slot_index(hand)]; + return slot.stream ? slot.stream->meta.product_code : std::string(); +} + +std::string LiveSource::model_id(Hand hand) const +{ + const HandSlot& slot = slots_[slot_index(hand)]; + return slot.stream && slot.stream->kinematics ? slot.stream->kinematics->model_id() : std::string(); +} + +LiveSource::HandStats LiveSource::stats(Hand hand) const +{ + return slots_[slot_index(hand)].stats; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/live/rh_frame.cpp b/src/plugins/revohuman/live/rh_frame.cpp new file mode 100644 index 0000000000..15798782a2 --- /dev/null +++ b/src/plugins/revohuman/live/rh_frame.cpp @@ -0,0 +1,68 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/rh_frame.hpp" + +#include + +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +static_assert(RH_JOINT_COUNT == kGloveJointCount, "SDK joint count"); +static_assert(RH_TACTILE_MAX_POINTS == kMaxTaxels, "SDK taxel limit"); +static_assert(shape_size_bound(kHandJointCount, RH_TACTILE_POINTS_TYPE_B) <= kShapeMaxBytes, + "the largest glove taxel layout must fit one TactileMetadata"); + +GloveSample from_rh_frame(const RhGloveFrame& frame, const std::array& joints_rad) +{ + if (frame.struct_size < sizeof(RhGloveFrame)) + { + throw std::invalid_argument("RhGloveFrame.struct_size " + std::to_string(frame.struct_size) + + " is smaller than " + std::to_string(sizeof(RhGloveFrame))); + } + GloveSample out; + if (frame.hand == RH_HAND_LEFT) + { + out.hand = Hand::Left; + } + else if (frame.hand == RH_HAND_RIGHT) + { + out.hand = Hand::Right; + } + else + { + throw std::invalid_argument("RhGloveFrame.hand " + std::to_string(frame.hand) + " is not left or right"); + } + out.generation = frame.generation; + out.device_tick = frame.device_tick; + out.joints_valid_mask = frame.joints_valid_mask; + out.has_sample_time = (frame.flags & RH_FRAME_FLAG_HAS_SAMPLE_TIME) != 0; + out.sample_mono_ns = frame.sample_mono_ns; + out.joints_rad = joints_rad; + if (frame.flags & RH_FRAME_FLAG_HAS_TACTILE) + { + const RhTactile& src = frame.tactile; + if (src.point_count > RH_TACTILE_MAX_POINTS) + { + throw std::invalid_argument("RhTactile.point_count " + std::to_string(src.point_count) + + " exceeds the protocol limit"); + } + TactileSample t; + t.device_tick = src.device_tick; + t.sample_mono_ns = src.sample_mono_ns; + t.has_sample_time = src.has_sample_time != 0; + t.valid = src.valid != 0; + t.calibrated = src.calibrated != 0; + t.values.assign(src.values, src.values + src.point_count); + out.tactile = std::move(t); + } + return out; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/plugin.yaml b/src/plugins/revohuman/plugin.yaml new file mode 100644 index 0000000000..2041412c5b --- /dev/null +++ b/src/plugins/revohuman/plugin.yaml @@ -0,0 +1,26 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: revohuman +description: "RevoHuman exoskeleton gloves with tactile, pushed via OpenXR" +command: "./revohuman_plugin" +version: "1.0.0" +devices: + - path: "/hand/left" + type: "hand_tracker" + description: "Left hand: 26 OpenXR joints" + - path: "/hand/right" + type: "hand_tracker" + description: "Right hand: 26 OpenXR joints" + - path: "/glove/revohuman_left" + type: "joint_state" + description: "Left glove encoder angles (21 joints, rad, SDK order)" + - path: "/glove/revohuman_right" + type: "joint_state" + description: "Right glove encoder angles (21 joints, rad, SDK order)" + - path: "/tactile/revohuman_left" + type: "tactile" + description: "Left glove taxel forces with the TactileMetadata from the C SDK taxel layout" + - path: "/tactile/revohuman_right" + type: "tactile" + description: "Right glove taxel forces with the TactileMetadata from the C SDK taxel layout" diff --git a/src/python/isaaccapture/teleop_session_manager/teleop_session.py b/src/python/isaaccapture/teleop_session_manager/teleop_session.py index 985319e867..2bc81d9dfe 100644 --- a/src/python/isaaccapture/teleop_session_manager/teleop_session.py +++ b/src/python/isaaccapture/teleop_session_manager/teleop_session.py @@ -62,7 +62,14 @@ logger = logging.getLogger(__name__) # Plugins that publish device status (see PluginDeviceStatusPublisher); others launch unmonitored. -_MONITORED_PLUGIN_NAMES = frozenset({"manus_hand_plugin", "spacemouse", "gamepad"}) +_MONITORED_PLUGIN_NAMES = frozenset( + { + "manus_hand_plugin", + "spacemouse", + "gamepad", + "revohuman", + } +) def _resolve_sink(entry: GraphExecutable) -> tuple[GraphExecutable, IDeviceIOSink]: diff --git a/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index 1fd14aa203..5d928bf995 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -12,3 +12,8 @@ endif() if(BUILD_PLUGIN_OGLO) add_subdirectory(oglo_tactile) endif() + +# Skipped with the plugin: off Linux or without the RevoHuman C SDK. +if(BUILD_PLUGIN_REVOHUMAN AND TARGET revohuman_plugin::revohuman_core) + add_subdirectory(revohuman) +endif() diff --git a/tests/cpp/plugins/revohuman/CMakeLists.txt b/tests/cpp/plugins/revohuman/CMakeLists.txt new file mode 100644 index 0000000000..332100b955 --- /dev/null +++ b/tests/cpp/plugins/revohuman/CMakeLists.txt @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_subdirectory(core) +add_subdirectory(live) +add_subdirectory(integration) diff --git a/tests/cpp/plugins/revohuman/core/CMakeLists.txt b/tests/cpp/plugins/revohuman/core/CMakeLists.txt new file mode 100644 index 0000000000..bbab64734e --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/CMakeLists.txt @@ -0,0 +1,23 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(revohuman_core_tests + test_shapes.hpp + test_device_clock.cpp + test_device_status.cpp + test_event_log.cpp + test_pipeline.cpp + test_pose_math.cpp + test_serialize.cpp + test_sha256.cpp + test_tactile.cpp + test_wrist_tracking.cpp +) + +target_link_libraries(revohuman_core_tests + PRIVATE + Catch2::Catch2WithMain + revohuman_plugin::revohuman_core +) + +catch_discover_tests(revohuman_core_tests) diff --git a/tests/cpp/plugins/revohuman/core/test_device_clock.cpp b/tests/cpp/plugins/revohuman/core/test_device_clock.cpp new file mode 100644 index 0000000000..6d9bc7ffc0 --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_device_clock.cpp @@ -0,0 +1,41 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +using namespace plugins::revohuman; + +TEST_CASE("tick unwrapper anchors on the first value", "[revohuman][clock]") +{ + TickUnwrapper u; + REQUIRE_FALSE(u.anchored()); + REQUIRE(u.unwrap(1000) == 1000); + REQUIRE(u.anchored()); + REQUIRE(u.unwrap(1005) == 1005); +} + +TEST_CASE("tick unwrapper carries across the 32-bit wrap", "[revohuman][clock]") +{ + TickUnwrapper u; + REQUIRE(u.unwrap(0xFFFFFFF0u) == 0xFFFFFFF0ll); + REQUIRE(u.unwrap(0x00000010u) == 0x100000010ll); + REQUIRE(u.unwrap(0x00000020u) == 0x100000020ll); +} + +TEST_CASE("tick unwrapper follows a backward step instead of wrapping", "[revohuman][clock]") +{ + TickUnwrapper u; + u.unwrap(100); + REQUIRE(u.unwrap(90) == 90); + REQUIRE(u.unwrap(95) == 95); + u.reset(); + REQUIRE_FALSE(u.anchored()); + REQUIRE(u.unwrap(7) == 7); +} + +TEST_CASE("tick conversions", "[revohuman][clock]") +{ + REQUIRE(ms_ticks_to_ns(5) == 5'000'000); + REQUIRE(ms_ticks_to_ns(-2) == -2'000'000); +} diff --git a/tests/cpp/plugins/revohuman/core/test_device_status.cpp b/tests/cpp/plugins/revohuman/core/test_device_status.cpp new file mode 100644 index 0000000000..d85bf700cf --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_device_status.cpp @@ -0,0 +1,242 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include +#include + +using namespace plugins::revohuman; + +namespace +{ + +// Mirrors _VALID_PLUGIN_DEVICE_STATE_REASON_PAIRS in +// src/python/isaaccapture/teleop_session_manager/status_monitor.py; any other pair makes +// TeleopSession reject the whole status report. +const std::set> kAcceptedPairs = { + { core::PluginDeviceState_UNKNOWN, core::PluginDeviceReason_NO_HARDWARE_SIGNAL }, + { core::PluginDeviceState_UNKNOWN, core::PluginDeviceReason_NO_CURRENT_DATA }, + { core::PluginDeviceState_CONNECTED, core::PluginDeviceReason_NONE }, + { core::PluginDeviceState_DISCONNECTED, core::PluginDeviceReason_NO_HARDWARE_SIGNAL }, + { core::PluginDeviceState_DEGRADED, core::PluginDeviceReason_CALIBRATION_FAILED }, + { core::PluginDeviceState_DEGRADED, core::PluginDeviceReason_NO_CURRENT_DATA }, + { core::PluginDeviceState_DEGRADED, core::PluginDeviceReason_DEVICE_ERROR }, + { core::PluginDeviceState_FAILED, core::PluginDeviceReason_DEVICE_ERROR }, + { core::PluginDeviceState_DISABLED, core::PluginDeviceReason_DISABLED_BY_CONFIGURATION }, +}; + +enum Row +{ + kHandRow = 0, + kGloveRow = 1, + kTactileRow = 2, +}; + +HandStatusInput streaming() +{ + HandStatusInput input; + input.health.hand_active = true; + input.health.wrist_tracked = true; + input.health.glove_live = true; + input.health.glove_seen = true; + input.health.tactile_live = true; + input.health.tactile_seen = true; + return input; +} + +//! Glove and tactile have both been seen and are silent now. +HandStatusInput lost() +{ + HandStatusInput input; + input.health.glove_seen = true; + input.health.tactile_seen = true; + return input; +} + +std::vector> all_notes() +{ + return { std::nullopt, Source::GloveNote{ Source::GloveNote::Kind::Waiting, "waiting" }, + Source::GloveNote{ Source::GloveNote::Kind::NotReady, "gate closed" }, + Source::GloveNote{ Source::GloveNote::Kind::Ended, "ended" } }; +} + +} // namespace + +TEST_CASE("every health maps onto a state/reason pair TeleopSession accepts", "[revohuman][status]") +{ + const std::vector skips = { + TactileSkip::None, TactileSkip::NoShape, TactileSkip::NoTactile, + TactileSkip::Invalid, TactileSkip::Uncalibrated, TactileSkip::CountMismatch, + TactileSkip::NoSampleTime, TactileSkip::StaleSample, TactileSkip::NonMonotonicSample + }; + std::size_t checked = 0; + for (const Hand hand : { Hand::Left, Hand::Right }) + { + for (int bits = 0; bits < (1 << 7); ++bits) + { + for (const TactileSkip skip : skips) + { + for (const auto& note : all_notes()) + { + HandStatusInput input; + input.health.hand_active = (bits & 0x01) != 0; + input.health.wrist_tracked = (bits & 0x02) != 0; + input.health.kinematics_missing = (bits & 0x04) != 0; + input.health.glove_live = (bits & 0x08) != 0; + input.health.glove_seen = (bits & 0x10) != 0; + input.health.tactile_live = (bits & 0x20) != 0; + input.health.tactile_seen = (bits & 0x40) != 0; + input.health.tactile_skip = skip; + input.note = note; + for (const auto& row : make_hand_status(hand, input)) + { + INFO(row.path << " bits=" << bits << " skip=" << tactile_skip_name(skip)); + CHECK(kAcceptedPairs.count({ row.state, row.reason }) == 1); + ++checked; + } + } + } + } + } + CHECK(checked == 2 * 128 * 9 * 4 * 3); +} + +TEST_CASE("rows carry the device paths of plugin.yaml", "[revohuman][status]") +{ + const auto left = make_hand_status(Hand::Left, streaming()); + CHECK(left[kHandRow].path == "/hand/left"); + CHECK(left[kGloveRow].path == "/glove/revohuman_left"); + CHECK(left[kTactileRow].path == "/tactile/revohuman_left"); + + const auto right = make_hand_status(Hand::Right, streaming()); + CHECK(right[kHandRow].path == "/hand/right"); + CHECK(right[kGloveRow].path == "/glove/revohuman_right"); + CHECK(right[kTactileRow].path == "/tactile/revohuman_right"); + + for (const auto& row : left) + { + CHECK(row.state == core::PluginDeviceState_CONNECTED); + CHECK(row.reason == core::PluginDeviceReason_NONE); + } +} + +TEST_CASE("a device that never gave data is unknown, one that stopped is disconnected", "[revohuman][status]") +{ + const auto waiting = make_hand_status(Hand::Right, HandStatusInput{}); + for (const auto& row : waiting) + { + INFO(row.path); + CHECK(row.state == core::PluginDeviceState_UNKNOWN); + CHECK(row.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + } + + const auto stopped = make_hand_status(Hand::Right, lost()); + for (const auto& row : stopped) + { + INFO(row.path); + CHECK(row.state == core::PluginDeviceState_DISCONNECTED); + CHECK(row.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + } +} + +TEST_CASE("what the source says about a glove overrides its last sample", "[revohuman][status]") +{ + HandStatusInput input = streaming(); + + input.note = Source::GloveNote{ Source::GloveNote::Kind::Ended, "glove stream ended: unplugged" }; + auto rows = make_hand_status(Hand::Left, input); + CHECK(rows[kGloveRow].state == core::PluginDeviceState_FAILED); + CHECK(rows[kGloveRow].reason == core::PluginDeviceReason_DEVICE_ERROR); + CHECK(rows[kGloveRow].error == "glove stream ended: unplugged"); + + input.note = Source::GloveNote{ Source::GloveNote::Kind::NotReady, "gate closed" }; + rows = make_hand_status(Hand::Left, input); + CHECK(rows[kGloveRow].state == core::PluginDeviceState_DEGRADED); + CHECK(rows[kGloveRow].reason == core::PluginDeviceReason_NO_CURRENT_DATA); + + // Before the first sample a closed gate is still "waiting", not a degraded glove. + input = HandStatusInput{}; + input.note = Source::GloveNote{ Source::GloveNote::Kind::NotReady, "gate closed" }; + rows = make_hand_status(Hand::Left, input); + CHECK(rows[kGloveRow].state == core::PluginDeviceState_UNKNOWN); + CHECK(rows[kGloveRow].reason == core::PluginDeviceReason_NO_CURRENT_DATA); + + input.note = Source::GloveNote{ Source::GloveNote::Kind::Waiting, "waiting for the glove: BUSY" }; + rows = make_hand_status(Hand::Left, input); + CHECK(rows[kGloveRow].state == core::PluginDeviceState_UNKNOWN); + CHECK(rows[kGloveRow].reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + CHECK(rows[kGloveRow].error == "waiting for the glove: BUSY"); + + // Looked for again after it streamed: unplugged, so disconnected. + input = lost(); + input.note = Source::GloveNote{ Source::GloveNote::Kind::Waiting, "waiting for the glove: unplugged" }; + rows = make_hand_status(Hand::Left, input); + CHECK(rows[kGloveRow].state == core::PluginDeviceState_DISCONNECTED); + CHECK(rows[kGloveRow].reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); + CHECK(rows[kGloveRow].error == "waiting for the glove: unplugged"); +} + +TEST_CASE("the hand row says why no tracked hand is injected", "[revohuman][status]") +{ + HandStatusInput input = streaming(); + input.health.wrist_tracked = false; + auto rows = make_hand_status(Hand::Left, input); + CHECK(rows[kHandRow].state == core::PluginDeviceState_DEGRADED); + CHECK(rows[kHandRow].reason == core::PluginDeviceReason_NO_CURRENT_DATA); + CHECK(rows[kHandRow].error == "no tracked wrist pose from the host"); + + input.health.hand_active = false; + input.health.kinematics_missing = true; + rows = make_hand_status(Hand::Left, input); + CHECK(rows[kHandRow].state == core::PluginDeviceState_DEGRADED); + CHECK(rows[kHandRow].error == "glove samples carry no joint poses"); +} + +TEST_CASE("tactile reports why it is not pushed while the glove is live", "[revohuman][status]") +{ + HandStatusInput input = streaming(); + input.health.tactile_live = false; + + input.health.tactile_skip = TactileSkip::NoShape; + CHECK(make_hand_status(Hand::Left, input)[kTactileRow].state == core::PluginDeviceState_DISABLED); + CHECK(make_hand_status(Hand::Left, input)[kTactileRow].error.find("taxel layout") != std::string::npos); + + // A P1 glove has no tactile sensors: tactile is off, not broken, whether or not the glove is live. + input.health.tactile_skip = TactileSkip::NoTactile; + for (const bool live : { true, false }) + { + input.health.glove_live = live; + const auto off = make_hand_status(Hand::Left, input)[kTactileRow]; + CHECK(off.state == core::PluginDeviceState_DISABLED); + CHECK(off.reason == core::PluginDeviceReason_DISABLED_BY_CONFIGURATION); + CHECK(off.error == "this glove has no tactile sensors"); + } + input.health.glove_live = true; + + input.health.tactile_skip = TactileSkip::Uncalibrated; + auto row = make_hand_status(Hand::Left, input)[kTactileRow]; + CHECK(row.state == core::PluginDeviceState_DEGRADED); + CHECK(row.reason == core::PluginDeviceReason_CALIBRATION_FAILED); + + input.health.tactile_skip = TactileSkip::CountMismatch; + row = make_hand_status(Hand::Left, input)[kTactileRow]; + CHECK(row.state == core::PluginDeviceState_FAILED); + CHECK(row.reason == core::PluginDeviceReason_DEVICE_ERROR); + + input.health.tactile_skip = TactileSkip::StaleSample; + row = make_hand_status(Hand::Left, input)[kTactileRow]; + CHECK(row.state == core::PluginDeviceState_DEGRADED); + CHECK(row.reason == core::PluginDeviceReason_NO_CURRENT_DATA); + + // Without a live glove the last skip reason is stale: tactile is simply gone. + input.health.glove_live = false; + input.health.tactile_skip = TactileSkip::CountMismatch; + row = make_hand_status(Hand::Left, input)[kTactileRow]; + CHECK(row.state == core::PluginDeviceState_DISCONNECTED); + CHECK(row.reason == core::PluginDeviceReason_NO_HARDWARE_SIGNAL); +} diff --git a/tests/cpp/plugins/revohuman/core/test_event_log.cpp b/tests/cpp/plugins/revohuman/core/test_event_log.cpp new file mode 100644 index 0000000000..7130b9bb52 --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_event_log.cpp @@ -0,0 +1,72 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using plugins::revohuman::EventLog; + +namespace +{ + +std::vector lines_of(const std::filesystem::path& path) +{ + std::ifstream in(path); + std::vector lines; + std::string line; + while (std::getline(in, line)) + { + lines.push_back(line); + } + return lines; +} + +} // namespace + +TEST_CASE("event log is off for an empty path", "[revohuman][event_log]") +{ + EventLog log(""); + REQUIRE_FALSE(log.enabled()); + log.write("ignored", { { "a", std::int64_t{ 1 } } }); +} + +TEST_CASE("event log writes one JSON object per line and appends", "[revohuman][event_log]") +{ + const auto path = std::filesystem::temp_directory_path() / "revohuman_event_log_test.jsonl"; + std::filesystem::remove(path); + { + EventLog log(path.string()); + REQUIRE(log.enabled()); + log.write("injector_created", { { "hand", std::string("left") }, + { "epoch", std::uint64_t{ 7 } }, + { "dt", std::int64_t{ -3 } }, + { "tracked", true } }); + log.write("odd", { { "s", std::string("a\"b\\c\nd\x01") }, + { "nan", std::numeric_limits::quiet_NaN() }, + { "x", 0.5 } }); + } + { + EventLog log(path.string()); + log.write("again", {}); + } + const auto lines = lines_of(path); + std::filesystem::remove(path); + REQUIRE(lines.size() == 3); + REQUIRE(lines[0].find("\"event\":\"injector_created\"") != std::string::npos); + REQUIRE(lines[0].find("\"hand\":\"left\"") != std::string::npos); + REQUIRE(lines[0].find("\"epoch\":7") != std::string::npos); + REQUIRE(lines[0].find("\"dt\":-3") != std::string::npos); + REQUIRE(lines[0].find("\"tracked\":true") != std::string::npos); + REQUIRE(lines[1].find(R"("s":"a\"b\\c\nd\u0001")") != std::string::npos); + REQUIRE(lines[1].find("\"nan\":null") != std::string::npos); + REQUIRE(lines[1].find("\"x\":0.5") != std::string::npos); + REQUIRE(lines[2].find("\"event\":\"again\"") != std::string::npos); +} diff --git a/tests/cpp/plugins/revohuman/core/test_pipeline.cpp b/tests/cpp/plugins/revohuman/core/test_pipeline.cpp new file mode 100644 index 0000000000..0389e5a57e --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_pipeline.cpp @@ -0,0 +1,1180 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "test_shapes.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +using namespace plugins::revohuman; +using Catch::Approx; + +namespace +{ + +constexpr std::int64_t kMs = 1'000'000; + +struct Call +{ + std::string kind; + std::vector buffer; + std::int64_t t_ns = 0; + std::int64_t raw_ns = 0; + HandFrame frame; +}; + +class RecordingSink : public PushSink +{ +public: + void push_joint_state(const std::vector& buffer, std::int64_t sample_ns, std::int64_t raw_ns) override + { + calls.push_back({ "joint_state", buffer, sample_ns, raw_ns, {} }); + } + void push_tactile(const std::vector& buffer, std::int64_t sample_ns, std::int64_t raw_ns) override + { + calls.push_back({ "tactile", buffer, sample_ns, raw_ns, {} }); + } + void clear_tactile() override + { + calls.push_back({ "clear_tactile", {}, 0, 0, {} }); + } + void push_shape(const std::vector& buffer, std::int64_t now_ns) override + { + calls.push_back({ "shape", buffer, now_ns, now_ns, {} }); + } + void clear_shape() override + { + calls.push_back({ "clear_shape", {}, 0, 0, {} }); + } + void hand_create() override + { + calls.push_back({ "create", {}, 0, 0, {} }); + } + void hand_destroy() override + { + calls.push_back({ "destroy", {}, 0, 0, {} }); + } + void hand_push(const HandFrame& frame) override + { + calls.push_back({ "hand", {}, frame.sample_mono_ns, 0, frame }); + } + + std::vector of(const std::string& kind) const + { + std::vector out; + for (const auto& c : calls) + { + if (c.kind == kind) + { + out.push_back(c); + } + } + return out; + } + //! Kinds of the hand injector calls, in order. + std::vector hand_calls() const + { + std::vector out; + for (const auto& c : calls) + { + if (c.kind == "create" || c.kind == "destroy" || c.kind == "hand") + { + out.push_back(c.kind); + } + } + return out; + } + + std::vector calls; +}; + +template +const T* root_of(const std::vector& bytes) +{ + flatbuffers::Verifier verifier(bytes.data(), bytes.size()); + REQUIRE(verifier.VerifyBuffer(nullptr)); + return flatbuffers::GetRoot(bytes.data()); +} + +HandMeta meta() +{ + HandMeta m; + m.hand = Hand::Left; + m.product_code = "PTV1"; + m.device_id = "revohuman_left"; + for (std::size_t i = 0; i < kGloveJointCount; ++i) + { + m.joint_names[i] = "l" + std::to_string(i); + } + return m; +} + +//! A P1 glove: joints only, no tactile sensors. +HandMeta meta_without_tactile() +{ + HandMeta m = meta(); + m.product_code = "P1"; + m.has_tactile = false; + return m; +} + +//! Index of the first call of @p kind, or the number of calls when there is none. +std::size_t first_call(const RecordingSink& sink, const std::string& kind) +{ + std::size_t i = 0; + while (i < sink.calls.size() && sink.calls[i].kind != kind) + { + ++i; + } + return i; +} + +//! Joint poses equal to the bind poses of the synthetic shape, so the skinned normals are the +//! bind normals. +HandKinematics bind_kinematics() +{ + HandKinematics k; + for (std::uint32_t j = 0; j < kHandJointCount; ++j) + { + k.joints[j] = revohuman_test::bind_pose(j); + k.radius[j] = 0.01f; + } + k.joints[kWristJoint] = Pose{}; + k.position_valid_mask = k.orientation_valid_mask = (1u << kHandJointCount) - 1; + return k; +} + +//! The bind poses turned by angle_rad about Z. +HandKinematics turned_kinematics(double angle_rad) +{ + HandKinematics k = bind_kinematics(); + const auto turn = revohuman_test::axis_angle({ 0.0, 0.0, 1.0 }, angle_rad); + for (auto& joint : k.joints) + { + joint.q = quat_mul(turn, joint.q); + } + return k; +} + +//! Taxel-local force is invariant under hand motion. +void require_local_pressure(const Call& call) +{ + const auto* data = root_of(call.buffer); + const auto* f = data->taxel_forces()->Get(0); + REQUIRE(f->x() == 0); + REQUIRE(f->y() == 0); + REQUIRE(f->z() == 100.0f); +} + +GloveSample glove_at(std::int64_t t_ns, std::uint32_t tick = 1000, bool with_kinematics = true) +{ + GloveSample g; + g.hand = Hand::Left; + g.generation = 1; + g.device_tick = tick; + g.joints_valid_mask = (1u << kGloveJointCount) - 1; + g.has_sample_time = true; + g.sample_mono_ns = t_ns; + if (with_kinematics) + { + g.kinematics = bind_kinematics(); + } + return g; +} + +TactileSample tactile(std::size_t count, std::int64_t t_ns) +{ + TactileSample t; + t.device_tick = 500; + t.has_sample_time = true; + t.sample_mono_ns = t_ns; + t.valid = true; + t.calibrated = true; + for (std::size_t i = 0; i < count; ++i) + { + t.values.push_back(static_cast(100 + 7 * i)); + } + return t; +} + +//! Wrist source stub: reports the pose it is given and records the times it was asked for. +struct FakeWrist : WristProvider +{ + WristPose pose; + std::vector asked_ns; + + WristPose wrist(Hand hand, std::int64_t sample_mono_ns) override + { + REQUIRE(hand == Hand::Left); + asked_ns.push_back(sample_mono_ns); + return pose; + } +}; + +WristPose wrist_at(std::array p, bool tracked = true) +{ + WristPose w; + w.pose_stage.p = p; + w.valid = true; + w.tracked = tracked; + return w; +} + +std::shared_ptr shape(std::size_t taxels) +{ + return std::make_shared(build_shape_buffer(revohuman_test::rigid_shape(taxels))); +} + +//! A fresh event log file, removed again when the test ends. +struct TempLog +{ + explicit TempLog(const std::string& name) + : path(std::filesystem::temp_directory_path() / ("revohuman_" + name + ".jsonl")) + { + std::filesystem::remove(path); + } + ~TempLog() + { + std::filesystem::remove(path); + } + + //! The lines of one event, in order. + std::vector events(const std::string& event) const + { + std::ifstream in(path); + std::vector out; + std::string line; + while (std::getline(in, line)) + { + if (line.find("\"event\":\"" + event + "\"") != std::string::npos) + { + out.push_back(line); + } + } + return out; + } + + std::filesystem::path path; +}; + +//! The raw JSON value of a field in an event line. +std::string field(const std::string& line, const std::string& key) +{ + const std::string tag = "\"" + key + "\":"; + const auto at = line.find(tag); + REQUIRE(at != std::string::npos); + const auto begin = at + tag.size(); + return line.substr(begin, line.find_first_of(",}", begin) - begin); +} + +double number(const std::string& line, const std::string& key) +{ + return std::strtod(field(line, key).c_str(), nullptr); +} + +//! Every received tactile sample is accounted for exactly once in a tactile_rate window. +void require_rate_identity(const std::string& line) +{ + REQUIRE(number(line, "samples") == number(line, "repeats") + number(line, "skipped") + number(line, "pushed")); + REQUIRE(number(line, "skipped") == + number(line, "skip_stale_future") + number(line, "skip_stale_old") + number(line, "skip_other")); +} + +} // namespace + +TEST_CASE("glove samples before the metadata are dropped", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.on_glove(glove_at(100 * kMs), 100 * kMs); + REQUIRE(sink.calls.empty()); + REQUIRE(p.stats().glove_before_meta == 1); + + HandMeta other = meta(); + other.hand = Hand::Right; + p.set_meta(other); + p.on_glove(glove_at(101 * kMs), 101 * kMs); + REQUIRE(sink.calls.empty()); +} + +TEST_CASE("joint state goes out at the encoder sampling time, which also stands in for the device clock", + "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.on_glove(glove_at(1000 * kMs, 5000), 1003 * kMs); + p.on_glove(glove_at(1005 * kMs, 5005), 1008 * kMs); + GloveSample untimed = glove_at(0, 5010); + untimed.has_sample_time = false; + p.on_glove(untimed, 1013 * kMs); + + const auto js = sink.of("joint_state"); + REQUIRE(js.size() == 3); + // The device tick is when the frame was sent, not when the encoders were sampled. + REQUIRE(js[0].t_ns == 1000 * kMs); + REQUIRE(js[0].raw_ns == 1000 * kMs); + REQUIRE(js[1].t_ns == 1005 * kMs); + REQUIRE(js[1].raw_ns == 1005 * kMs); + REQUIRE(js[2].t_ns == 1013 * kMs); + REQUIRE(js[2].raw_ns == 1013 * kMs); + REQUIRE(js[0].buffer.size() <= kJointStateMaxBytes); +} + +TEST_CASE("a glove sample stamped after it arrived goes out at the time it arrived", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.on_glove(glove_at(1004 * kMs, 5000), 1003 * kMs); + p.on_glove(glove_at(1005 * kMs, 5005), 1008 * kMs); + + const auto js = sink.of("joint_state"); + REQUIRE(js.size() == 2); + REQUIRE(js[0].t_ns == 1003 * kMs); + REQUIRE(js[0].raw_ns == 1003 * kMs); + REQUIRE(js[1].t_ns == 1005 * kMs); + const auto hands = sink.of("hand"); + REQUIRE(hands.size() == 2); + REQUIRE(hands[0].t_ns == 1003 * kMs); + REQUIRE(hands[1].t_ns == 1005 * kMs); + REQUIRE(p.stats().glove_future_clamped == 1); +} + +TEST_CASE("a restarted device stream re-anchors its tactile tick clock", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + GloveSample g = glove_at(0, 0xFFFFFFF0u); + g.tactile = tactile(241, 0); + g.tactile->device_tick = 0xFFFFFFF0u; + p.on_glove(g, 0); + GloveSample restarted = glove_at(5 * kMs, 10); + restarted.generation = 2; + restarted.tactile = tactile(241, 5 * kMs); + restarted.tactile->device_tick = 10; + p.on_glove(restarted, 5 * kMs); + const auto pushed = sink.of("tactile"); + REQUIRE(pushed.size() == 2); + REQUIRE(pushed[1].raw_ns == 10 * kMs); + REQUIRE(p.stats().generation_changes == 1); +} + +TEST_CASE("the hand is injected at the glove sample time with the wrist from the provider", "[revohuman][pipeline]") +{ + RecordingSink sink; + FakeWrist wrist; + wrist.pose = wrist_at({ 0.5, 1.0, -1.0 }); + HandPipeline p(Hand::Left, {}, sink); + p.set_wrist_provider(&wrist); + p.set_meta(meta()); + + p.on_glove(glove_at(100 * kMs), 103 * kMs); + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand" }); + // The wrist is asked for at the encoder sampling time, not at the arrival time. + REQUIRE(wrist.asked_ns == std::vector{ 100 * kMs }); + auto frame = sink.of("hand")[0].frame; + REQUIRE(frame.sample_mono_ns == 100 * kMs); + REQUIRE(frame.tracked); + // Stage coordinates as the provider reports them, and the fingers follow the wrist. + const auto& w = frame.joints[kWristJoint].p; + REQUIRE(w[0] == Approx(0.5)); + REQUIRE(w[1] == Approx(1.0)); + REQUIRE(w[2] == Approx(-1.0)); + const auto bind = revohuman_test::bind_pose(3).p; + REQUIRE(frame.joints[3].p[0] == Approx(0.5 + bind[0])); + REQUIRE(frame.joints[3].p[1] == Approx(1.0 + bind[1])); + REQUIRE(frame.joints[3].p[2] == Approx(-1.0 + bind[2])); + REQUIRE(frame.radius[3] == 0.01f); + REQUIRE(p.health(103 * kMs).wrist_tracked); + + // Every frame takes the wrist anew; the injector stays. + wrist.pose = wrist_at({ 2.0, 0.0, 0.0 }); + p.on_glove(glove_at(105 * kMs, 1005), 108 * kMs); + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand", "hand" }); + frame = sink.of("hand")[1].frame; + REQUIRE(frame.joints[kWristJoint].p[0] == Approx(2.0)); + REQUIRE(p.stats().hands_pushed == 2); + REQUIRE(p.stats().hands_tracked == 2); + REQUIRE(p.stats().injector_creates == 1); +} + +TEST_CASE("a wrist pose the runtime only holds is pushed untracked", "[revohuman][pipeline]") +{ + RecordingSink sink; + FakeWrist wrist; + wrist.pose = wrist_at({ 1.0, 0.0, 0.0 }, false); + HandPipeline p(Hand::Left, {}, sink); + p.set_wrist_provider(&wrist); + p.set_meta(meta()); + p.on_glove(glove_at(100 * kMs), 101 * kMs); + + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand" }); + const auto frame = sink.of("hand")[0].frame; + REQUIRE_FALSE(frame.tracked); + REQUIRE(frame.joints[kWristJoint].p[0] == Approx(1.0)); + REQUIRE(p.stats().hands_tracked == 0); + const auto h = p.health(101 * kMs); + REQUIRE(h.hand_active); + REQUIRE_FALSE(h.wrist_tracked); +} + +TEST_CASE("without a wrist pose the hand is placed at the stage origin, untracked", "[revohuman][pipeline]") +{ + RecordingSink sink; + FakeWrist wrist; + HandPipeline p(Hand::Left, {}, sink); + SECTION("no provider") + { + } + SECTION("the provider has no pose") + { + // An invalid pose is never used, whatever it holds. + wrist.pose.pose_stage.p = { 5.0, 5.0, 5.0 }; + wrist.pose.tracked = true; + p.set_wrist_provider(&wrist); + } + p.set_meta(meta()); + p.on_glove(glove_at(100 * kMs), 101 * kMs); + + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand" }); + const auto frame = sink.of("hand")[0].frame; + REQUIRE_FALSE(frame.tracked); + for (const double v : frame.joints[kWristJoint].p) + { + REQUIRE(v == Approx(0.0).margin(1e-12)); + } + REQUIRE(p.stats().hands_tracked == 0); + REQUIRE_FALSE(p.health(101 * kMs).wrist_tracked); +} + +TEST_CASE("glove samples without joint poses never reach the injector", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.on_glove(glove_at(100 * kMs, 1000, false), 101 * kMs); + p.on_tick(200 * kMs); + REQUIRE(sink.hand_calls().empty()); + REQUIRE(p.stats().glove_without_kinematics == 1); + REQUIRE(sink.of("joint_state").size() == 1); +} + +TEST_CASE("a hand that is no longer pushed is removed and comes back with the next frame", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.on_glove(glove_at(100 * kMs), 101 * kMs); + p.on_tick(151 * kMs); + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand" }); + p.on_tick(152 * kMs); + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand", "destroy" }); + REQUIRE_FALSE(p.health(152 * kMs).hand_active); + + p.on_glove(glove_at(160 * kMs, 1060), 161 * kMs); + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand", "destroy", "create", "hand" }); + REQUIRE(p.health(161 * kMs).hand_active); + REQUIRE(p.stats().injector_creates == 2); + REQUIRE(p.stats().injector_destroys == 1); +} + +TEST_CASE("reset drops the hand and the clock anchors", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + GloveSample g = glove_at(100 * kMs, 0xFFFFFFF0u); + g.tactile = tactile(241, 100 * kMs); + g.tactile->device_tick = 0xFFFFFFF0u; + p.on_glove(g, 101 * kMs); + p.reset("loop"); + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand", "destroy" }); + REQUIRE_FALSE(p.health(101 * kMs).hand_active); + REQUIRE_FALSE(p.health(101 * kMs).glove_live); + + g = glove_at(105 * kMs, 10); + g.tactile = tactile(241, 105 * kMs); + g.tactile->device_tick = 10; + p.on_glove(g, 106 * kMs); + REQUIRE(sink.hand_calls() == std::vector{ "create", "hand", "destroy", "create", "hand" }); + REQUIRE(sink.of("tactile").back().raw_ns == 10 * kMs); +} + +TEST_CASE("a reset keeps the glove and its tactile seen", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + auto g = glove_at(100 * kMs); + g.tactile = tactile(241, 100 * kMs); + p.on_glove(g, 101 * kMs); + REQUIRE_FALSE(p.health(101 * kMs).glove_seen); + + p.set_meta(meta()); + p.set_shape(shape(241)); + p.on_glove(g, 102 * kMs); + p.reset("loop"); + const HandHealth h = p.health(102 * kMs); + REQUIRE_FALSE(h.glove_live); + REQUIRE(h.glove_seen); + REQUIRE(h.tactile_seen); +} + +TEST_CASE("tactile needs a matching shape and calibration", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + + GloveSample g = glove_at(100 * kMs); + g.tactile = tactile(241, 98 * kMs); + p.on_glove(g, 101 * kMs); + REQUIRE(sink.of("tactile").empty()); + REQUIRE(p.health(101 * kMs).tactile_skip == TactileSkip::NoShape); + + p.set_shape(shape(241)); + auto check_skip = [&](GloveSample sample, TactileSkip expected) + { + p.on_glove(sample, 102 * kMs); + REQUIRE(sink.of("tactile").empty()); + REQUIRE(p.health(102 * kMs).tactile_skip == expected); + }; + GloveSample bad = g; + bad.tactile->calibrated = false; + check_skip(bad, TactileSkip::Uncalibrated); + bad = g; + bad.tactile->valid = false; + check_skip(bad, TactileSkip::Invalid); + bad = g; + bad.tactile = tactile(240, 98 * kMs); + check_skip(bad, TactileSkip::CountMismatch); + + p.on_glove(g, 103 * kMs); + const auto pushed = sink.of("tactile"); + REQUIRE(pushed.size() == 1); + REQUIRE(pushed[0].t_ns == 98 * kMs); + REQUIRE(pushed[0].buffer.size() <= kTactileMaxBytes); + const auto* data = root_of(pushed[0].buffer); + REQUIRE(data->taxel_forces()->size() == 241); + for (std::size_t i = 0; i < 241; ++i) + { + const auto* f = data->taxel_forces()->Get(static_cast(i)); + REQUIRE(force_to_counts({ f->x(), f->y(), f->z() }) == static_cast(100 + 7 * i)); + // Force is expressed in each taxel frame, independent of hand orientation. + REQUIRE(f->x() == 0); + REQUIRE(f->y() == 0); + REQUIRE(f->z() == static_cast(100 + 7 * i)); + } + REQUIRE(p.health(103 * kMs).tactile_live); + REQUIRE(p.health(103 * kMs).tactile_skip == TactileSkip::None); +} + +TEST_CASE("each tactile tick is pushed once", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + + // The SDK may repeat the latest tactile frame on glove samples that come faster than it. + GloveSample g = glove_at(100 * kMs, 1000); + g.tactile = tactile(241, 98 * kMs); + p.on_glove(g, 101 * kMs); + g = glove_at(105 * kMs, 1005); + g.tactile = tactile(241, 98 * kMs); + p.on_glove(g, 106 * kMs); + REQUIRE(sink.of("tactile").size() == 1); + REQUIRE(p.stats().tactile_repeats == 1); + + g = glove_at(110 * kMs, 1010); + g.tactile = tactile(241, 108 * kMs); + g.tactile->device_tick = 510; + p.on_glove(g, 111 * kMs); + const auto pushed = sink.of("tactile"); + REQUIRE(pushed.size() == 2); + REQUIRE(pushed[1].t_ns == 108 * kMs); + REQUIRE(pushed[1].raw_ns - pushed[0].raw_ns == 10 * kMs); + + // A restarted stream may reuse the tick. + p.reset("test"); + p.set_meta(meta()); + g = glove_at(200 * kMs, 1000); + g.tactile = tactile(241, 198 * kMs); + p.on_glove(g, 201 * kMs); + REQUIRE(sink.of("tactile").size() == 3); +} + +TEST_CASE("the shape is pushed at start and refreshed every period", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + const auto s = shape(241); + p.set_shape(s); + p.on_tick(0); + p.on_tick(500 * kMs); + p.on_tick(999 * kMs); + p.on_tick(1000 * kMs); + const auto shapes = sink.of("shape"); + REQUIRE(shapes.size() == 2); + REQUIRE(shapes[0].buffer == s->bytes()); + REQUIRE(shapes[0].buffer.size() <= kShapeMaxBytes); + REQUIRE(shapes[1].t_ns == 1000 * kMs); +} + +TEST_CASE("the shape waits for a glove with tactile sensors", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_shape(shape(241)); + p.on_tick(0); + REQUIRE(sink.of("shape").empty()); + + p.set_meta(meta()); + // The first force sample brings the shape with it, ahead of the next tick. + GloveSample g = glove_at(100 * kMs); + g.tactile = tactile(241, 98 * kMs); + p.on_glove(g, 101 * kMs); + REQUIRE(sink.of("shape").size() == 1); + REQUIRE(sink.of("tactile").size() == 1); + REQUIRE(first_call(sink, "shape") < first_call(sink, "tactile")); + p.on_tick(102 * kMs); + REQUIRE(sink.of("shape").size() == 1); +} + +TEST_CASE("a glove without tactile sensors streams joints and the hand without tactile", "[revohuman][pipeline]") +{ + RecordingSink sink; + FakeWrist wrist; + wrist.pose = wrist_at({ 0.0, 0.0, 0.0 }); + HandPipeline p(Hand::Left, {}, sink); + p.set_wrist_provider(&wrist); + p.set_shape(shape(241)); + p.set_meta(meta_without_tactile()); + p.on_tick(0); + + GloveSample g = glove_at(100 * kMs); + // Whatever the source reports, a glove without sensors pushes no forces. + g.tactile = tactile(241, 98 * kMs); + p.on_glove(g, 101 * kMs); + + REQUIRE(sink.of("joint_state").size() == 1); + REQUIRE(sink.of("hand").size() == 1); + REQUIRE(sink.of("tactile").empty()); + const auto h = p.health(101 * kMs); + REQUIRE(h.glove_live); + REQUIRE(h.hand_active); + REQUIRE_FALSE(h.tactile_live); + REQUIRE(h.tactile_skip == TactileSkip::NoTactile); + + // Nor does the shape come with a later tick. + p.on_tick(2000 * kMs); + REQUIRE(sink.of("shape").empty()); + REQUIRE(p.stats().shapes_pushed == 0); +} + +TEST_CASE("a glove without tactile sensors removes the forces and shape of the one before", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_shape(shape(241)); + p.set_meta(meta()); + GloveSample g = glove_at(100 * kMs); + g.tactile = tactile(241, 98 * kMs); + p.on_glove(g, 101 * kMs); + REQUIRE(sink.of("shape").size() == 1); + REQUIRE(sink.of("tactile").size() == 1); + + p.set_meta(meta_without_tactile()); + REQUIRE(sink.of("clear_tactile").size() == 1); + REQUIRE(sink.of("clear_shape").size() == 1); + p.on_tick(2000 * kMs); + REQUIRE(sink.of("shape").size() == 1); + + // A glove with sensors again gets the shape at once. + p.reset("test"); + p.set_meta(meta()); + p.on_tick(2001 * kMs); + const auto shapes = sink.of("shape"); + REQUIRE(shapes.size() == 2); + REQUIRE(shapes[1].t_ns == 2001 * kMs); + // Once removed, there is nothing more to remove. + p.set_meta(meta_without_tactile()); + p.set_meta(meta_without_tactile()); + REQUIRE(sink.of("clear_shape").size() == 2); +} + +TEST_CASE("health tells which part of the glove is live", "[revohuman][pipeline]") +{ + RecordingSink sink; + FakeWrist wrist; + wrist.pose = wrist_at({ 0.0, 0.0, 0.0 }); + HandPipeline p(Hand::Left, {}, sink); + p.set_wrist_provider(&wrist); + p.set_meta(meta()); + auto h = p.health(0); + REQUIRE_FALSE(h.glove_live); + REQUIRE_FALSE(h.wrist_tracked); + REQUIRE_FALSE(h.hand_active); + REQUIRE(h.tactile_skip == TactileSkip::NoShape); + + p.on_glove(glove_at(10 * kMs, 1000, false), 10 * kMs); + h = p.health(10 * kMs); + REQUIRE(h.glove_live); + REQUIRE(h.kinematics_missing); + REQUIRE_FALSE(p.health(511 * kMs).glove_live); + + p.on_glove(glove_at(20 * kMs, 1010), 20 * kMs); + h = p.health(20 * kMs); + REQUIRE(h.wrist_tracked); + REQUIRE(h.hand_active); + REQUIRE_FALSE(h.kinematics_missing); + + p.on_tick(71 * kMs); + h = p.health(71 * kMs); + REQUIRE_FALSE(h.hand_active); + REQUIRE_FALSE(h.wrist_tracked); +} + +TEST_CASE("local pressure is independent of missing or invalid encoder poses", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + auto g = glove_at(100 * kMs); + g.tactile = tactile(241, 98 * kMs); + SECTION("no kinematics") + { + g.kinematics.reset(); + } + SECTION("missing encoder sampling time") + { + g.has_sample_time = false; + } + SECTION("thumb encoders unavailable") + { + g.joints_valid_mask = 0xffff; + g.kinematics->orientation_valid_mask = 0; + g.kinematics->position_valid_mask = 0; + } + SECTION("nonunit encoder orientation") + { + g.kinematics->joints[9].q = { 0, 0, 0, 2 }; + } + p.on_glove(g, 101 * kMs); + const auto pushed = sink.of("tactile"); + REQUIRE(pushed.size() == 1); + REQUIRE(pushed[0].t_ns == 98 * kMs); + require_local_pressure(pushed[0]); + REQUIRE(p.health(101 * kMs).tactile_live); + if (!g.kinematics) + { + REQUIRE(sink.of("hand").empty()); + REQUIRE(p.health(101 * kMs).kinematics_missing); + } + else + { + const auto hands = sink.of("hand"); + REQUIRE(hands.size() == 1); + REQUIRE(hands[0].frame.orientation_valid_mask == (g.kinematics->orientation_valid_mask & kMappedHandMask)); + REQUIRE(hands[0].frame.position_valid_mask == (g.kinematics->position_valid_mask & kMappedHandMask)); + REQUIRE_FALSE(hands[0].frame.tracked); + } +} + +TEST_CASE("tactile requires its own fresh ordered sampling time", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + auto g = glove_at(100 * kMs); + g.tactile = tactile(241, 98 * kMs); + TactileSkip expected = TactileSkip::None; + SECTION("missing sampling time") + { + g.tactile->has_sample_time = false; + expected = TactileSkip::NoSampleTime; + } + SECTION("old sample") + { + g.tactile->sample_mono_ns = 0; + expected = TactileSkip::StaleSample; + } + SECTION("future sample") + { + g.tactile->sample_mono_ns = 102 * kMs; + expected = TactileSkip::StaleSample; + } + SECTION("encoder time does not gate local pressure") + { + g.tactile->sample_mono_ns = 94 * kMs; + } + SECTION("unused position flags do not block force directions") + { + g.kinematics->position_valid_mask = 0; + } + SECTION("nonfinite or nonunit orientation despite valid mask") + { + g.kinematics->joints[9].q = { 0, 0, 0, 2 }; + } + SECTION("sampling time regresses despite increasing device tick") + { + p.on_glove(g, 100 * kMs); + sink.calls.clear(); + g.tactile->device_tick += 1; + g.tactile->sample_mono_ns -= kMs; + expected = TactileSkip::NonMonotonicSample; + } + SECTION("device tick regresses despite increasing sampling time") + { + p.on_glove(g, 100 * kMs); + sink.calls.clear(); + g.tactile->device_tick -= 1; + g.tactile->sample_mono_ns += kMs; + expected = TactileSkip::NonMonotonicSample; + } + p.on_glove(g, 101 * kMs); + REQUIRE(sink.of("tactile").size() == (expected == TactileSkip::None ? 1u : 0u)); + REQUIRE(p.health(101 * kMs).tactile_skip == expected); +} + +TEST_CASE("local pressure is published immediately without waiting for an encoder pose", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + for (std::uint32_t i = 0; i < 4; ++i) + { + auto g = glove_at((100 + 5 * i) * kMs, 1000 + 5 * i); + g.kinematics = turned_kinematics(0.2 * i); + g.tactile = tactile(241, 111 * kMs); + p.on_glove(g, (112 + 5 * i) * kMs); + REQUIRE(sink.of("tactile").size() == 1); + } + REQUIRE(p.stats().tactile_repeats == 3); + const auto pushed = sink.of("tactile"); + REQUIRE(pushed[0].t_ns == 111 * kMs); + require_local_pressure(pushed[0]); +} + +TEST_CASE("tactile generation changes and reset clear freshness", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + auto g = glove_at(100 * kMs); + g.tactile = tactile(241, 98 * kMs); + p.on_glove(g, 101 * kMs); + REQUIRE(p.health(101 * kMs).tactile_live); + REQUIRE_FALSE(p.health(199 * kMs).tactile_live); + SECTION("reset") + { + p.reset("reconnect"); + REQUIRE_FALSE(p.health(102 * kMs).tactile_live); + } + SECTION("generation") + { + g.generation += 1; + g.tactile->valid = false; + p.on_glove(g, 102 * kMs); + REQUIRE_FALSE(p.health(102 * kMs).tactile_live); + g.tactile->valid = true; + } + p.on_glove(g, 103 * kMs); + REQUIRE(sink.of("tactile").size() == 2); +} + +TEST_CASE("an invalid re-send of the pushed tactile sample holds it until the maximum age", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + auto g = glove_at(100 * kMs); + g.tactile = tactile(241, 98 * kMs); + p.on_glove(g, 101 * kMs); + REQUIRE(p.health(101 * kMs).tactile_live); + + // One failed read of the tactile board: the firmware re-sends the last snapshot as invalid. + g.tactile->valid = false; + p.on_glove(g, 106 * kMs); + REQUIRE(p.health(106 * kMs).tactile_live); + REQUIRE(p.stats().tactile_repeats == 1); + REQUIRE(p.stats().tactile_invalid_repeats == 1); + REQUIRE(p.stats().tactile_skipped == 0); + + SECTION("the next read recovers") + { + auto next = glove_at(110 * kMs, 1010); + next.tactile = tactile(241, 107 * kMs); + next.tactile->device_tick = 512; + p.on_glove(next, 111 * kMs); + REQUIRE(p.health(111 * kMs).tactile_live); + REQUIRE(sink.of("tactile").size() == 2); + REQUIRE(p.stats().tactile_skipped == 0); + } + SECTION("a re-send after the maximum age is invalid") + { + p.on_glove(g, 198 * kMs); + REQUIRE(p.health(198 * kMs).tactile_live); + p.on_glove(g, 199 * kMs); + REQUIRE_FALSE(p.health(199 * kMs).tactile_live); + REQUIRE(p.health(199 * kMs).tactile_skip == TactileSkip::Invalid); + REQUIRE(p.stats().tactile_invalid_repeats == 2); + REQUIRE(p.stats().tactile_skipped == 1); + } + SECTION("an expiry while held reports invalid") + { + p.on_tick(199 * kMs); + REQUIRE(sink.of("clear_tactile").size() == 1); + REQUIRE(p.health(199 * kMs).tactile_skip == TactileSkip::Invalid); + } + SECTION("an expiry after a valid repeat reports a stale sample") + { + g.tactile->valid = true; + p.on_glove(g, 110 * kMs); + p.on_tick(199 * kMs); + REQUIRE(p.health(199 * kMs).tactile_skip == TactileSkip::StaleSample); + } + SECTION("a new tick marked invalid is invalid at once") + { + g.tactile->device_tick = 512; + g.tactile->sample_mono_ns = 107 * kMs; + p.on_glove(g, 111 * kMs); + REQUIRE_FALSE(p.health(111 * kMs).tactile_live); + REQUIRE(p.health(111 * kMs).tactile_skip == TactileSkip::Invalid); + REQUIRE(p.stats().tactile_skipped == 1); + } + SECTION("a re-send after a reset is invalid") + { + p.reset("reconnect"); + p.on_glove(g, 107 * kMs); + REQUIRE(p.health(107 * kMs).tactile_skip == TactileSkip::Invalid); + } +} + +TEST_CASE("tactile_rate counts immediate local force publication", "[revohuman][pipeline]") +{ + TempLog file("tactile_rate_steady"); + { + EventLog log(file.path.string()); + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink, &log); + p.set_meta(meta()); + p.set_shape(shape(241)); + // Encoder/tactile sampling skew must not delay taxel-local forces. + const std::int64_t offsets_ms[] = { 3, 6, 5, 4 }; + for (std::uint32_t i = 0; i <= 600; ++i) + { + const std::int64_t glove_ns = 100 * kMs + 5 * kMs * i; + auto g = glove_at(glove_ns, 1000 + 5 * i); + g.tactile = tactile(241, glove_ns + offsets_ms[i % 4] * kMs); + g.tactile->device_tick = 500 + 5 * i; + const std::int64_t now_ns = glove_ns + 10 * kMs; + p.on_glove(g, now_ns); + p.on_tick(now_ns); + } + REQUIRE(p.stats().tactile_pushed == 601); + REQUIRE(p.stats().tactile_skipped == 0); + } + const auto rates = file.events("tactile_rate"); + REQUIRE(rates.size() == 3); + for (std::size_t w = 0; w < rates.size(); ++w) + { + const auto& line = rates[w]; + REQUIRE(field(line, "hand") == "\"left\""); + REQUIRE(number(line, "t_ns") == Approx(static_cast(110 * kMs + 1000 * kMs * (w + 1)))); + REQUIRE(number(line, "window_s") == Approx(1.0)); + REQUIRE(number(line, "samples") == 200); + REQUIRE(number(line, "pushed") == 200); + REQUIRE(number(line, "skipped") == 0); + REQUIRE(number(line, "lag_max_ms") == Approx(7.0)); + REQUIRE(number(line, "samples_total") == 1 + 200 * (w + 1)); + REQUIRE(number(line, "pushed_total") == 1 + 200 * (w + 1)); + require_rate_identity(line); + } +} + +TEST_CASE("tactile_rate accounts for rejection and recovery without pose queues", "[revohuman][pipeline]") +{ + TempLog file("tactile_rate_skips"); + { + EventLog log(file.path.string()); + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink, &log); + p.set_meta(meta()); + p.set_shape(shape(241)); + p.on_tick(0); + auto send = [&](std::uint32_t tick, std::int64_t sample_ms, std::int64_t now_ms, bool valid = true) + { + auto g = glove_at(now_ms * kMs, tick, false); + g.tactile = tactile(241, sample_ms * kMs); + g.tactile->device_tick = tick; + g.tactile->valid = valid; + p.on_glove(g, now_ms * kMs); + }; + send(500, 1002, 1001); // future + send(501, 800, 1006); // stale + send(502, 1009, 1011); // publish + send(502, 1009, 1016); // repeat + send(503, 1023, 1024); // publish without an encoder pose + send(503, 1023, 1026, false); // invalid re-send of the pushed sample, held + p.on_tick(1300 * kMs); + send(504, 1359, 1361, false); // invalid, never a valid zero + p.reset("loop"); + send(500, 1403, 1404); // reused device tick after reset + p.on_tick(2400 * kMs); + REQUIRE(p.stats().tactile_samples == 8); + REQUIRE(p.stats().tactile_pushed == 3); + REQUIRE(p.stats().tactile_skipped == 3); + REQUIRE(p.stats().tactile_repeats == 2); + REQUIRE(p.stats().tactile_invalid_repeats == 1); + } + const auto rates = file.events("tactile_rate"); + REQUIRE(rates.size() == 2); + REQUIRE(number(rates[0], "window_s") == Approx(1.3)); + REQUIRE(number(rates[0], "samples") == 6); + REQUIRE(number(rates[0], "pushed") == 2); + REQUIRE(number(rates[0], "repeats") == 2); + REQUIRE(number(rates[0], "invalid_repeats") == 1); + REQUIRE(number(rates[0], "skip_stale_future") == 1); + REQUIRE(number(rates[0], "skip_stale_old") == 1); + require_rate_identity(rates[0]); + REQUIRE(number(rates[1], "window_s") == Approx(1.1)); + REQUIRE(number(rates[1], "samples") == 2); + REQUIRE(number(rates[1], "pushed") == 1); + REQUIRE(number(rates[1], "skip_other") == 1); + REQUIRE(number(rates[1], "invalid_repeats") == 0); + REQUIRE(number(rates[1], "lag_max_ms") == Approx(1.0)); + require_rate_identity(rates[1]); + REQUIRE(number(rates[1], "samples_total") == 8); + REQUIRE(number(rates[1], "skipped_total") == 3); + REQUIRE(number(rates[1], "invalid_repeats_total") == 1); +} + +TEST_CASE("tactile_rate is off with a zero period", "[revohuman][pipeline]") +{ + TempLog file("tactile_rate_off"); + { + EventLog log(file.path.string()); + RecordingSink sink; + PipelineConfig config; + config.tactile_rate_period_ns = 0; + HandPipeline p(Hand::Left, config, sink, &log); + p.set_meta(meta()); + for (std::int64_t t = 0; t <= 3000; t += 100) + { + p.on_tick(t * kMs); + } + } + REQUIRE(file.events("tactile_rate").empty()); +} + +TEST_CASE("skipped tactile samples keep the force output until it expires", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + auto g = glove_at(100 * kMs); + std::int64_t t = 100; + std::uint32_t tick = 500; + auto next = [&] + { + t += 5; + tick += 5; + g.tactile = tactile(241, t * kMs); + g.tactile->device_tick = tick; + }; + g.tactile = tactile(241, t * kMs); + p.on_glove(g, (t + 1) * kMs); + REQUIRE(p.health((t + 1) * kMs).tactile_live); + + // Valid samples alternating with each kind of skip leave the collection in place. + for (int kind = 0; kind < 4; ++kind) + { + next(); + switch (kind) + { + case 0: + g.tactile->valid = false; + break; + case 1: + g.tactile->calibrated = false; + break; + case 2: + g.tactile->values.pop_back(); + break; + default: + g.tactile->sample_mono_ns = (t + 2) * kMs; + break; + } + p.on_glove(g, (t + 1) * kMs); + REQUIRE_FALSE(p.health((t + 1) * kMs).tactile_live); + next(); + p.on_glove(g, (t + 1) * kMs); + REQUIRE(p.health((t + 1) * kMs).tactile_live); + } + REQUIRE(sink.of("tactile").size() == 5); + REQUIRE(sink.of("clear_tactile").empty()); + + // Skips that outlast the maximum age end in one removal, timed from the last pushed sample. + const std::int64_t pushed = t; + next(); + g.tactile->valid = false; + p.on_glove(g, (t + 1) * kMs); + p.on_tick((pushed + 100) * kMs); + REQUIRE(sink.of("clear_tactile").empty()); + p.on_tick((pushed + 101) * kMs); + REQUIRE(sink.of("clear_tactile").size() == 1); + REQUIRE_FALSE(p.health((pushed + 101) * kMs).tactile_live); + p.on_tick((pushed + 102) * kMs); + REQUIRE(sink.of("clear_tactile").size() == 1); + + t = pushed + 105; + next(); + p.on_glove(g, (t + 1) * kMs); + REQUIRE(p.health((t + 1) * kMs).tactile_live); + REQUIRE(sink.of("tactile").size() == 6); +} + +TEST_CASE("reset and stream generations clear old force output", "[revohuman][pipeline]") +{ + RecordingSink sink; + HandPipeline p(Hand::Left, {}, sink); + p.set_meta(meta()); + p.set_shape(shape(241)); + auto g = glove_at(100 * kMs); + g.tactile = tactile(241, 100 * kMs); + p.on_glove(g, 101 * kMs); + p.reset("loop"); + REQUIRE(sink.of("clear_tactile").size() == 1); + REQUIRE_FALSE(p.health(101 * kMs).glove_live); + REQUIRE_FALSE(p.health(101 * kMs).tactile_live); + p.on_glove(g, 102 * kMs); + REQUIRE(p.health(102 * kMs).tactile_live); + g.generation = 2; + g.tactile.reset(); + p.on_glove(g, 103 * kMs); + REQUIRE(sink.of("clear_tactile").size() == 2); + REQUIRE_FALSE(p.health(103 * kMs).tactile_live); +} diff --git a/tests/cpp/plugins/revohuman/core/test_pose_math.cpp b/tests/cpp/plugins/revohuman/core/test_pose_math.cpp new file mode 100644 index 0000000000..696446206f --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_pose_math.cpp @@ -0,0 +1,131 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "test_shapes.hpp" + +#include +#include +#include + +#include +#include + +using namespace plugins::revohuman; +using Catch::Approx; + +namespace +{ + +double det(const Mat3& m) +{ + return m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) + + m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); +} + +Vec3 mat_mul(const Mat3& m, const Vec3& v) +{ + return { m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2], m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2], + m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2] }; +} + +void require_vec(const Vec3& a, const Vec3& b, double eps = 1e-12) +{ + for (int i = 0; i < 3; ++i) + { + REQUIRE(a[i] == Approx(b[i]).margin(eps)); + } +} + +//! Same rotation: q and -q are equal. +void require_same_rotation(const Quat& a, const Quat& b, double eps = 1e-12) +{ + const double d = a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; + REQUIRE(std::abs(d) == Approx(1.0).margin(eps)); +} + +Pose sample_pose() +{ + Pose p; + p.p = { 0.3, -1.2, 0.7 }; + p.q = revohuman_test::axis_angle({ 0.2, -0.5, 1.0 }, 1.1); + return p; +} + +} // namespace + +TEST_CASE("quat_to_mat is a proper rotation that agrees with quat_rotate", "[revohuman][pose]") +{ + const Pose p = sample_pose(); + const Mat3 m = quat_to_mat(p.q); + REQUIRE(det(m) == Approx(1.0)); + const Vec3 v{ 0.4, 0.1, -0.9 }; + require_vec(mat_mul(m, v), quat_rotate(p.q, v)); +} + +TEST_CASE("compose and inverse are consistent", "[revohuman][pose]") +{ + const Pose a = sample_pose(); + Pose b; + b.p = { -0.05, 0.02, 0.11 }; + b.q = revohuman_test::axis_angle({ 1.0, 0.0, 0.3 }, -0.7); + + const Pose id = compose(a, inverse(a)); + require_vec(id.p, { 0, 0, 0 }); + require_same_rotation(id.q, { 0, 0, 0, 1 }); + + const Vec3 v{ 0.01, -0.2, 0.3 }; + require_vec(transform_point(compose(a, b), v), transform_point(a, transform_point(b, v))); +} + +TEST_CASE("slerp hits its endpoints and takes the short arc", "[revohuman][pose]") +{ + const Quat a = revohuman_test::axis_angle({ 0, 0, 1 }, 0.2); + const Quat b = revohuman_test::axis_angle({ 0, 0, 1 }, 1.0); + require_same_rotation(quat_slerp(a, b, 0.0), a); + require_same_rotation(quat_slerp(a, b, 1.0), b); + require_same_rotation(quat_slerp(a, b, 0.5), revohuman_test::axis_angle({ 0, 0, 1 }, 0.6)); + const Quat neg_b{ -b[0], -b[1], -b[2], -b[3] }; + require_same_rotation(quat_slerp(a, neg_b, 0.5), revohuman_test::axis_angle({ 0, 0, 1 }, 0.6)); +} + +TEST_CASE("joints in the stage chain the wrist pose and the kinematics", "[revohuman][pose]") +{ + const Pose wrist = sample_pose(); + + HandKinematics kin; + for (std::uint32_t j = 0; j < kHandJointCount; ++j) + { + kin.joints[j] = revohuman_test::bind_pose(j); + } + const auto joints = joints_in_stage(wrist, kin); + for (std::uint32_t j = 0; j < kHandJointCount; ++j) + { + const Vec3 local{ 0.01, 0.0, 0.0 }; + require_vec(transform_point(joints[j], local), transform_point(wrist, transform_point(kin.joints[j], local))); + } +} + +TEST_CASE("a pose flag is seven numbers with a non-zero quaternion", "[revohuman][pose]") +{ + const Pose pose = parse_pose("0.01,-0.1,0.08,0,0,0,2", "--aim-to-wrist-left"); + REQUIRE(pose.p[0] == Approx(0.01)); + REQUIRE(pose.p[1] == Approx(-0.1)); + REQUIRE(pose.p[2] == Approx(0.08)); + // Normalized. + REQUIRE(pose.q[3] == Approx(1.0)); + + for (const char* bad : { "", "0,0,0,0,0,0", "0,0,0,0,0,0,1,0", "0,0,0,0,0,0,x", "0,0,0,0,0,,1", " 0,0,0,0,0,0,1", + "0,0,0,0,0,0,1 ", "nan,0,0,0,0,0,1", "0,0,inf,0,0,0,1", "0,0,0,0,0,0,0" }) + { + INFO("'" << bad << "'"); + REQUIRE_THROWS_AS(parse_pose(bad, "--aim-to-wrist-left"), std::invalid_argument); + } +} + +TEST_CASE("wrist offset flags remain finite in OpenXR precision", "[revohuman][pose]") +{ + const Pose pose = parse_pose("0,0,0,1e308,1e308,1e308,1e308", "--aim-to-wrist-left"); + for (double v : pose.q) + REQUIRE(v == Approx(0.5)); + REQUIRE_THROWS_AS(parse_pose("1e100,0,0,0,0,0,1", "--aim-to-wrist-left"), std::invalid_argument); +} diff --git a/tests/cpp/plugins/revohuman/core/test_serialize.cpp b/tests/cpp/plugins/revohuman/core/test_serialize.cpp new file mode 100644 index 0000000000..746ca618e1 --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_serialize.cpp @@ -0,0 +1,98 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace plugins::revohuman; + +namespace +{ + +template +const T* verified_root(const std::vector& bytes) +{ + flatbuffers::Verifier verifier(bytes.data(), bytes.size()); + REQUIRE(verifier.VerifyBuffer(nullptr)); + return flatbuffers::GetRoot(bytes.data()); +} + +std::array joint_names() +{ + std::array names; + for (std::size_t i = 0; i < kGloveJointCount; ++i) + { + names[i] = "right_joint_" + std::to_string(20 - i); + } + return names; +} + +} // namespace + +TEST_CASE("joint state keeps SDK order, float radians and the valid mask", "[revohuman][serialize]") +{ + const auto names = joint_names(); + std::array rad{}; + for (std::size_t i = 0; i < kGloveJointCount; ++i) + { + rad[i] = 0.123456789012345 * static_cast(i) - 1.0; + } + const std::uint32_t mask = 0x1FFFFFu & ~((1u << 3) | (1u << 17)); + + const auto bytes = serialize_joint_state("revohuman_right", names, rad, mask); + REQUIRE(bytes.size() <= kJointStateMaxBytes); + const auto* out = verified_root(bytes); + REQUIRE(out->device_id()->str() == "revohuman_right"); + REQUIRE_FALSE(out->has_velocity()); + REQUIRE_FALSE(out->has_effort()); + REQUIRE_FALSE(out->ee_pose_valid()); + REQUIRE(out->joints()->size() == kGloveJointCount); + for (std::size_t i = 0; i < kGloveJointCount; ++i) + { + const auto* j = out->joints()->Get(static_cast(i)); + REQUIRE(j->name()->str() == names[i]); + REQUIRE(j->position() == static_cast(rad[i])); + REQUIRE(j->valid() == ((mask >> i) & 1u)); + } +} + +TEST_CASE("joint state over its cap is refused", "[revohuman][serialize]") +{ + auto names = joint_names(); + for (auto& n : names) + { + n = std::string(300, 'j'); + } + REQUIRE_THROWS_AS(serialize_joint_state("x", names, {}, 0), std::length_error); +} + +TEST_CASE("tactile data carries the count and every force", "[revohuman][serialize]") +{ + std::vector> forces(kMaxTaxels); + for (std::size_t i = 0; i < forces.size(); ++i) + { + forces[i] = { 0.001f * i, -0.5f, static_cast(i) }; + } + const auto bytes = serialize_tactile(forces); + REQUIRE(bytes.size() <= tactile_size_bound(kMaxTaxels)); + REQUIRE(bytes.size() <= kTactileMaxBytes); + const auto* out = verified_root(bytes); + REQUIRE(out->taxel_forces()->size() == kMaxTaxels); + for (std::size_t i = 0; i < forces.size(); ++i) + { + const auto* f = out->taxel_forces()->Get(static_cast(i)); + REQUIRE(f->x() == forces[i][0]); + REQUIRE(f->y() == forces[i][1]); + REQUIRE(f->z() == forces[i][2]); + } + + REQUIRE(verified_root(serialize_tactile({}))->taxel_forces()->size() == 0); + REQUIRE_THROWS_AS(serialize_tactile(std::vector>(2000)), std::length_error); +} diff --git a/tests/cpp/plugins/revohuman/core/test_sha256.cpp b/tests/cpp/plugins/revohuman/core/test_sha256.cpp new file mode 100644 index 0000000000..a382b73d79 --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_sha256.cpp @@ -0,0 +1,43 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include + +using plugins::revohuman::sha256_hex; + +namespace +{ + +std::string hash(const std::string& s) +{ + return sha256_hex(reinterpret_cast(s.data()), s.size()); +} + +} // namespace + +TEST_CASE("sha256 FIPS 180-4 vectors", "[revohuman][sha256]") +{ + REQUIRE(hash("") == "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855"); + REQUIRE(hash("abc") == "ba7816bf8f01cfea414140de5dae2223b00361a396177a9cb410ff61f20015ad"); + REQUIRE(hash("abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq") == + "248d6a61d20638b8e5c026930c3e6039a33ce45964ff2167f6ecedd419db06c1"); +} + +TEST_CASE("sha256 of one million 'a'", "[revohuman][sha256]") +{ + const std::vector data(1'000'000, 'a'); + REQUIRE(sha256_hex(data.data(), data.size()) == "cdc76e5c9914fb9281a1c7e284d73e67f1809a48a497200e046d39ccc7112cd0"); +} + +TEST_CASE("sha256 padding edges", "[revohuman][sha256]") +{ + // 55 and 56 bytes straddle the one-block / two-block padding boundary. + REQUIRE(hash(std::string(55, 'a')) == "9f4390f8d30c2dd92ec9f095b65e2b9ae9b0a925a5258e241c9f1e910f734318"); + REQUIRE(hash(std::string(56, 'a')) == "b35439a4ac6f0948b6d6f9e3c6af0f5f590ce20f1bde7090ef7970686ec6738a"); + REQUIRE(hash(std::string(64, 'a')) == "ffe054fe7ae0cb6dc65c3af9b61d5209f439851db43d0ba5997337df154668eb"); +} diff --git a/tests/cpp/plugins/revohuman/core/test_shapes.hpp b/tests/cpp/plugins/revohuman/core/test_shapes.hpp new file mode 100644 index 0000000000..760a2c9b6f --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_shapes.hpp @@ -0,0 +1,58 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include + +namespace revohuman_test +{ + +using plugins::revohuman::kHandJointCount; +using plugins::revohuman::Pose; +using plugins::revohuman::ShapeSpec; + +//! A rotation of angle_rad about axis. +inline std::array axis_angle(std::array axis, double angle_rad) +{ + const auto n = plugins::revohuman::normalize(axis); + const double s = std::sin(angle_rad / 2.0); + return { n[0] * s, n[1] * s, n[2] * s, std::cos(angle_rad / 2.0) }; +} + +inline Pose bind_pose(std::uint32_t joint) +{ + Pose p; + p.p = { 0.004 * joint, 0.011 * (joint % 5), -0.002 * joint }; + p.q = axis_angle({ 1.0, 2.0, 3.0 }, 0.05 * joint); + return p; +} + +//! A valid rigid shape: taxel i sits on joint 2 + i % 24, weight 1 on that joint. +inline ShapeSpec rigid_shape(std::size_t taxels) +{ + ShapeSpec spec; + for (std::uint32_t j = 0; j < kHandJointCount; ++j) + { + spec.bind_poses.push_back(bind_pose(j)); + spec.joint_ids.push_back(static_cast(j)); + } + for (std::size_t i = 0; i < taxels; ++i) + { + const auto joint = static_cast(2 + i % 24); + const auto& b = spec.bind_poses[joint]; + const double off = 0.001 * static_cast(i % 7); + Pose taxel = b; + taxel.p = { b.p[0] + off, b.p[1] - off, b.p[2] + 0.005 }; + spec.taxel_bind_poses.push_back(taxel); + spec.joint_indices.push_back({ joint, joint, joint, joint }); + spec.joint_weights.push_back({ 1.0f, 0.0f, 0.0f, 0.0f }); + } + return spec; +} + +} // namespace revohuman_test diff --git a/tests/cpp/plugins/revohuman/core/test_tactile.cpp b/tests/cpp/plugins/revohuman/core/test_tactile.cpp new file mode 100644 index 0000000000..38bd1a8b75 --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_tactile.cpp @@ -0,0 +1,244 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "test_shapes.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace plugins::revohuman; +using Catch::Approx; +using revohuman_test::rigid_shape; + +namespace +{ +std::string sha_of(const std::vector& bytes) +{ + return sha256_hex(bytes.data(), bytes.size()); +} + +std::vector problems(const ShapeSpec& spec) +{ + const auto bytes = build_shape_buffer(spec); + return validate_shape(*flatbuffers::GetRoot(bytes.data())); +} + +LoadedShape load(const ShapeSpec& spec) +{ + return LoadedShape(build_shape_buffer(spec)); +} + +HandKinematics bind_kinematics(const ShapeSpec& spec) +{ + HandKinematics kin; + for (std::size_t j = 0; j < spec.joint_ids.size(); ++j) + kin.joints[spec.joint_ids[j]] = spec.bind_poses[j]; + return kin; +} +} // namespace + +TEST_CASE("rigid metadata fits the tracker cap for every glove layout", "[revohuman][tactile]") +{ + for (std::size_t n : { 1u, 24u, 211u, 241u, 300u, 350u }) + { + const auto spec = rigid_shape(n); + const auto loaded = load(spec); + INFO("taxels " << n << " bytes " << loaded.bytes().size()); + REQUIRE(problems(spec).empty()); + REQUIRE(loaded.taxel_count() == n); + REQUIRE(loaded.bytes().size() <= shape_size_bound(kHandJointCount, n)); + REQUIRE(loaded.bytes().size() <= kShapeMaxBytes); + } + REQUIRE_THROWS(load(rigid_shape(kMaxTaxels + 1))); +} + +TEST_CASE("metadata loading names the buffer by its sha256 and verifies it", "[revohuman][tactile]") +{ + const auto bytes = build_shape_buffer(rigid_shape(24)); + REQUIRE(LoadedShape(bytes).sha256() == sha_of(bytes)); + auto corrupt = bytes; + corrupt[3] = 0x7F; + REQUIRE_THROWS(LoadedShape(corrupt)); + auto oversized = bytes; + oversized.resize(kShapeMaxBytes + 1); + REQUIRE_THROWS(LoadedShape(oversized)); +} + +TEST_CASE("metadata rejects invalid poses, references and weights", "[revohuman][tactile]") +{ + auto s = rigid_shape(24); + SECTION("non-unit taxel orientation") + { + s.taxel_bind_poses[3].q = { 0, 0, 0, 2 }; + } + SECTION("NaN taxel orientation") + { + s.taxel_bind_poses[3].q[0] = std::numeric_limits::quiet_NaN(); + } + SECTION("non-finite taxel position") + { + s.taxel_bind_poses[0].p[1] = std::numeric_limits::infinity(); + } + SECTION("non-unit segment orientation") + { + s.bind_poses[4].q = { 0, 0, 0, 2 }; + } + SECTION("weights do not sum to one") + { + s.joint_weights[5] = { .7f, .2f, 0, 0 }; + } + SECTION("negative weight") + { + s.joint_weights[5] = { 1.5f, -.5f, 0, 0 }; + } + SECTION("non-finite weight") + { + s.joint_weights[5][0] = std::numeric_limits::quiet_NaN(); + } + SECTION("zero weights") + { + s.joint_weights[5] = { 0, 0, 0, 0 }; + } + SECTION("invalid segment index in weighted slot") + { + s.joint_indices[5][0] = 26; + } + SECTION("invalid segment index in zero-weight slot") + { + s.joint_indices[5][1] = 26; + } + SECTION("duplicate joint identifier") + { + s.joint_ids[3] = s.joint_ids[0]; + } + SECTION("joint count sentinel is not a joint") + { + s.joint_ids[3] = core::HandJoint_NUM_JOINTS; + } + SECTION("different segment and joint-id lengths") + { + s.joint_ids.pop_back(); + } + SECTION("different taxel vector lengths") + { + s.taxel_bind_poses.pop_back(); + } + SECTION("empty layout") + { + s = {}; + } + REQUIRE_FALSE(problems(s).empty()); + REQUIRE_THROWS(load(s)); +} + +TEST_CASE("skinning at bind returns taxel origins and outward Z axes", "[revohuman][tactile]") +{ + const auto spec = rigid_shape(241); + const auto shape = load(spec); + const auto kin = bind_kinematics(spec); + const auto pos = shape.skin_positions(kin); + const auto nrm = shape.current_normals(kin); + for (std::size_t i = 0; i < pos.size(); ++i) + { + const auto normal = quat_rotate(spec.taxel_bind_poses[i].q, { 0, 0, 1 }); + for (int c = 0; c < 3; ++c) + { + REQUIRE(pos[i][c] == Approx(spec.taxel_bind_poses[i].p[c]).margin(1e-6)); + REQUIRE(nrm[i][c] == Approx(normal[c]).margin(1e-6)); + } + } +} + +TEST_CASE("skinning follows a rigid motion of the hand", "[revohuman][tactile]") +{ + const auto spec = rigid_shape(211); + const auto shape = load(spec); + const Pose motion{ { .1, -.2, .05 }, revohuman_test::axis_angle({ .3, 1, -.4 }, .9) }; + auto kin = bind_kinematics(spec); + for (auto& joint : kin.joints) + joint = compose(motion, joint); + const auto pos = shape.skin_positions(kin); + const auto nrm = shape.current_normals(kin); + for (std::size_t i = 0; i < pos.size(); ++i) + { + const auto p = transform_point(motion, spec.taxel_bind_poses[i].p); + const auto n = quat_rotate(quat_mul(motion.q, spec.taxel_bind_poses[i].q), { 0, 0, 1 }); + for (int c = 0; c < 3; ++c) + { + REQUIRE(pos[i][c] == Approx(p[c]).margin(1e-6)); + REQUIRE(nrm[i][c] == Approx(n[c]).margin(1e-6)); + } + } +} + +TEST_CASE("subset segment slots resolve through reordered joint_ids", "[revohuman][tactile]") +{ + ShapeSpec spec; + spec.joint_ids = { core::HandJoint_INDEX_TIP, core::HandJoint_WRIST }; + spec.bind_poses.resize(2); + spec.taxel_bind_poses = { Pose{ { .01, .02, .03 }, { 0, 0, 0, 1 } } }; + spec.joint_indices = { { 0, 1, 1, 1 } }; + spec.joint_weights = { { 1, 0, 0, 0 } }; + const auto loaded = load(spec); + auto kin = bind_kinematics(spec); + kin.joints[0].p = { 9, 9, 9 }; // Segment 0 does not mean OpenXR PALM. + kin.joints[core::HandJoint_INDEX_TIP].p = { .1, .2, .3 }; + kin.joints[core::HandJoint_INDEX_TIP].q = revohuman_test::axis_angle({ 1, 0, 0 }, 1.0); + kin.orientation_valid_mask = 1u << core::HandJoint_INDEX_TIP; + REQUIRE(loaded.has_valid_orientations(kin)); + const auto p = transform_point(kin.joints[core::HandJoint_INDEX_TIP], spec.taxel_bind_poses[0].p); + const auto n = quat_rotate(kin.joints[core::HandJoint_INDEX_TIP].q, { 0, 0, 1 }); + for (int c = 0; c < 3; ++c) + { + REQUIRE(loaded.skin_positions(kin)[0][c] == Approx(p[c]).margin(1e-6)); + REQUIRE(loaded.current_normals(kin)[0][c] == Approx(n[c]).margin(1e-6)); + } + kin.orientation_valid_mask = 1; + REQUIRE_FALSE(loaded.has_valid_orientations(kin)); +} + +TEST_CASE("normal force is the SDK reading on positive local Z", "[revohuman][tactile]") +{ + for (std::uint32_t value = 0; value <= 0xFFFF; ++value) + { + const auto forces = taxel_forces({ static_cast(value) }); + REQUIRE(forces[0][0] == 0); + REQUIRE(forces[0][1] == 0); + REQUIRE(forces[0][2] == static_cast(value)); + REQUIRE(force_to_counts(forces[0]) == static_cast(value)); + } + REQUIRE(taxel_forces({}).empty()); +} + +TEST_CASE("only weighted joint orientations gate the layout", "[revohuman][tactile]") +{ + const auto spec = rigid_shape(1); + const auto shape = load(spec); + auto kin = bind_kinematics(spec); + REQUIRE_FALSE(shape.has_valid_orientations(kin)); + const auto joint = spec.joint_ids[spec.joint_indices[0][0]]; + kin.orientation_valid_mask = 1u << joint; + REQUIRE(shape.has_valid_orientations(kin)); + kin.joints[0].q = { 0, 0, 0, 0 }; + REQUIRE(shape.has_valid_orientations(kin)); + kin.joints[joint].q[0] = std::numeric_limits::quiet_NaN(); + REQUIRE_FALSE(shape.has_valid_orientations(kin)); +} + +TEST_CASE("loader refuses a blended taxel the schema permits", "[revohuman][tactile]") +{ + auto spec = rigid_shape(24); + spec.joint_indices[5] = { 7, 8, 0, 0 }; + spec.joint_weights[5] = { .75f, .25f, 0, 0 }; + REQUIRE(problems(spec).empty()); + REQUIRE_THROWS_WITH(load(spec), Catch::Matchers::ContainsSubstring("only rigid binding")); +} diff --git a/tests/cpp/plugins/revohuman/core/test_wrist_tracking.cpp b/tests/cpp/plugins/revohuman/core/test_wrist_tracking.cpp new file mode 100644 index 0000000000..74caeff1b5 --- /dev/null +++ b/tests/cpp/plugins/revohuman/core/test_wrist_tracking.cpp @@ -0,0 +1,51 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +using namespace plugins::revohuman; + +namespace +{ + +constexpr std::int64_t kMs = 1'000'000; + +} // namespace + +TEST_CASE("injector is created on the first frame and kept while frames arrive", "[revohuman][wrist]") +{ + InjectorLifecycle life(50 * kMs); + REQUIRE_FALSE(life.active()); + + auto s = life.on_frame(0); + REQUIRE(s.create); + REQUIRE_FALSE(s.destroy); + REQUIRE(s.reason == "first_frame"); + REQUIRE(life.active()); + + s = life.on_frame(5 * kMs); + REQUIRE_FALSE(s.create); + REQUIRE_FALSE(s.destroy); + REQUIRE(s.reason.empty()); + REQUIRE(life.active()); +} + +TEST_CASE("injector is destroyed when pushes stall and recreated by the next frame", "[revohuman][wrist]") +{ + InjectorLifecycle life(50 * kMs); + REQUIRE_FALSE(life.on_tick(1000 * kMs).destroy); + + life.on_frame(100 * kMs); + REQUIRE_FALSE(life.on_tick(150 * kMs).destroy); + const auto s = life.on_tick(151 * kMs); + REQUIRE(s.destroy); + REQUIRE(s.reason == "stall"); + REQUIRE_FALSE(life.active()); + REQUIRE_FALSE(life.on_tick(300 * kMs).destroy); + + const auto again = life.on_frame(310 * kMs); + REQUIRE(again.create); + REQUIRE(again.reason == "first_frame"); + REQUIRE(life.active()); +} diff --git a/tests/cpp/plugins/revohuman/integration/CMakeLists.txt b/tests/cpp/plugins/revohuman/integration/CMakeLists.txt new file mode 100644 index 0000000000..61fc387b68 --- /dev/null +++ b/tests/cpp/plugins/revohuman/integration/CMakeLists.txt @@ -0,0 +1,41 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The reviewed hand geometry against the SDK and the manifest. The hand kinematics library is only +# built when the live source links against the C SDK, so a build without the SDK library skips this suite. +if(TARGET revohuman_plugin::revohuman_hand_kinematics) + add_executable(revohuman_integration_tests test_integration.cpp) + target_link_libraries(revohuman_integration_tests + PRIVATE + Catch2::Catch2WithMain + revohuman_plugin::revohuman_live + revohuman_plugin::revohuman_hand_kinematics + ) + target_compile_definitions(revohuman_integration_tests PRIVATE + REVOHUMAN_MANIFEST="${CMAKE_SOURCE_DIR}/src/plugins/revohuman/plugin.yaml" + REVOHUMAN_TEST_SDK_POINTS=$ + REVOHUMAN_TEST_SDK_VARIANT_MODEL=$ + REVOHUMAN_TEST_SDK_TAXEL_LAYOUT=$) + # Reads plugin.yaml from the source tree at run time, so it is not installed. + set_target_properties(revohuman_integration_tests PROPERTIES ISAACTELEOP_NEEDS_SOURCE_TREE TRUE) + # The point set and taxel checks call the SDK directly; its imported target is scoped to the plugin directory. + if(REVOHUMAN_HAS_SDK_POINTS OR REVOHUMAN_HAS_SDK_TAXEL_LAYOUT) + target_link_libraries(revohuman_integration_tests PRIVATE "${REVOHUMAN_SDK_LIBRARY}") + endif() + catch_discover_tests(revohuman_integration_tests) +endif() + +# The hand capture labels a take with REVOHUMAN_WRIST_SOURCE, so the plugin refuses a value it does not know and a +# --wrist-source that disagrees with it. Both are refused before the plugin opens anything. Without the +# SDK library revohuman_plugin is only a placeholder target, so there is nothing to run. +if(REVOHUMAN_LIVE_AVAILABLE) + add_test(NAME revohuman_wrist_source_variable_unknown COMMAND revohuman_plugin) + set_tests_properties(revohuman_wrist_source_variable_unknown PROPERTIES + ENVIRONMENT "REVOHUMAN_WRIST_SOURCE=hand-tracking" + PASS_REGULAR_EXPRESSION "REVOHUMAN_WRIST_SOURCE must be auto, hand_tracking or controller, got 'hand-tracking'") + add_test(NAME revohuman_wrist_source_flag_disagrees + COMMAND revohuman_plugin --wrist-source=controller) + set_tests_properties(revohuman_wrist_source_flag_disagrees PROPERTIES + ENVIRONMENT "REVOHUMAN_WRIST_SOURCE=hand_tracking" + PASS_REGULAR_EXPRESSION "--wrist-source=controller disagrees with REVOHUMAN_WRIST_SOURCE=hand_tracking") +endif() diff --git a/tests/cpp/plugins/revohuman/integration/test_integration.cpp b/tests/cpp/plugins/revohuman/integration/test_integration.cpp new file mode 100644 index 0000000000..95a1ce21fe --- /dev/null +++ b/tests/cpp/plugins/revohuman/integration/test_integration.cpp @@ -0,0 +1,467 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include + +#if REVOHUMAN_TEST_SDK_POINTS && REVOHUMAN_TEST_SDK_TAXEL_LAYOUT +# include +#endif + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::revohuman; +using Catch::Approx; + +#if REVOHUMAN_TEST_SDK_POINTS +namespace +{ +constexpr std::uint32_t kAllAngles = (1u << kGloveJointCount) - 1; +constexpr std::array kDistal{ 9, 14, 19, 24, 4 }; +constexpr std::array kLengths{ .025370264322333556, .025370258547203105, .025370262040019596, + .025198549138189495, .03058276137814423 }; +// Non-thumb fingers: glove angle slot of the MCP flexion, and the OpenXR proximal and intermediate joints. +constexpr std::array kMcpSlots{ 2, 6, 10, 14 }; +constexpr std::array kProximal{ 7, 12, 17, 22 }; +constexpr std::array kIntermediate{ 8, 13, 18, 23 }; + +Vec3 delta(const Vec3& a, const Vec3& b) +{ + return { a[0] - b[0], a[1] - b[1], a[2] - b[2] }; +} + +} // namespace +#endif + +#if REVOHUMAN_TEST_SDK_POINTS +TEST_CASE("reviewed hand tips follow each DIP for both hands", "[revohuman][geometry]") +{ + const auto provider = make_hand_kinematics(HandModel::Dv1); + CHECK(provider->model_id().rfind("sdk-points:dv1:reviewed-openxr-symmetric-v6-", 0) == 0); + for (const auto hand : { Hand::Left, Hand::Right }) + { + std::array angles{}; + const auto initial = provider->solve(hand, angles, kAllAngles); + REQUIRE(initial); + REQUIRE(initial->position_valid_mask == kMappedHandMask); + REQUIRE(initial->orientation_valid_mask == kMappedHandMask); + for (int finger = 0; finger < 5; ++finger) + { + angles.fill(0); + angles[4 * finger] = .6; + const auto moved = provider->solve(hand, angles, kAllAngles); + REQUIRE(moved); + const auto distal = kDistal[finger]; + const auto tip = distal + 1; + CHECK(norm(delta(moved->joints[distal].p, initial->joints[distal].p)) < 1e-12); + CHECK(norm(delta(moved->joints[tip].p, initial->joints[tip].p)) > .001); + const auto segment = delta(moved->joints[tip].p, moved->joints[distal].p); + CHECK(norm(segment) == Approx(kLengths[finger]).margin(1e-12)); + const auto forward = quat_rotate(moved->joints[tip].q, { 0, 0, -1 }); + CHECK(dot(normalize(segment), forward) == Approx(1).margin(1e-12)); + double quaternion_dot = 0; + for (int i = 0; i < 4; ++i) + quaternion_dot += initial->joints[tip].q[i] * moved->joints[tip].q[i]; + CHECK(2 * std::acos(std::min(1.0, std::abs(quaternion_dot))) == Approx(.6).margin(1e-12)); + for (int other = 0; other < 5; ++other) + if (other != finger) + CHECK(norm(delta(moved->joints[kDistal[other] + 1].p, initial->joints[kDistal[other] + 1].p)) < 1e-12); + const auto invalid = provider->solve(hand, angles, kAllAngles & ~(1u << (4 * finger))); + REQUIRE(invalid); + CHECK((invalid->position_valid_mask & (1u << tip)) == 0); + CHECK((invalid->orientation_valid_mask & (1u << tip)) == 0); + } + for (std::size_t slot = 0; slot < angles.size(); ++slot) + { + angles.fill(0); + angles[slot] = .4; + const auto moved = provider->solve(hand, angles, kAllAngles); + REQUIRE(moved); + CHECK(moved->position_valid_mask == kMappedHandMask); + double change = 0; + for (std::size_t j = 0; j < kHandJointCount; ++j) + if (kMappedHandMask & (1u << j)) + { + change += norm(delta(moved->joints[j].p, initial->joints[j].p)); + for (int k = 0; k < 4; ++k) + change += std::abs(moved->joints[j].q[k] - initial->joints[j].q[k]); + } + CHECK(change > 1e-6); + for (int fixed : { 0, 1, 6, 11, 16, 21 }) + { + CHECK(norm(delta(moved->joints[fixed].p, initial->joints[fixed].p)) < 1e-12); + CHECK(moved->joints[fixed].q == initial->joints[fixed].q); + } + } + } +} + +TEST_CASE("the corrected WRIST lies on the neutral palm plane for both products", "[revohuman][geometry]") +{ + // Palm-link +X toward the skin, expressed in the reviewed OpenXR WRIST basis. + const Vec3 inward{ 0, -.9996343707598901, .027039319434451838 }; + for (const auto model : { HandModel::Dv1, HandModel::P }) + { + const auto provider = make_hand_kinematics(model); + for (const auto hand : { Hand::Left, Hand::Right }) + { + const auto solved = provider->solve(hand, {}, kAllAngles); + REQUIRE(solved); + CHECK(norm(solved->joints[1].p) < 1e-12); + CHECK(std::abs(solved->joints[1].q[0]) < 1e-12); + CHECK(std::abs(solved->joints[1].q[1]) < 1e-12); + CHECK(std::abs(solved->joints[1].q[2]) < 1e-12); + CHECK(solved->joints[1].q[3] == Approx(1).margin(1e-12)); + CHECK(std::abs(dot(solved->joints[0].p, inward)) < 1e-12); + for (const int proximal : { 12, 17 }) + CHECK(std::abs(dot(solved->joints[proximal].p, inward)) < .000501); + } + } +} + +TEST_CASE("a non-thumb proximal joint is rigid on the MCP link", "[revohuman][geometry]") +{ + // The bone to the intermediate joint keeps its length and turns by the encoder angle. P takes the + // corrections fitted on DV1, whose MCP axes it misses by 2.5e-4 rad, so it is rigid to a few microns. + for (const auto& [model, tolerance] : { std::pair{ HandModel::Dv1, 1e-9 }, std::pair{ HandModel::P, 1e-5 } }) + { + const auto provider = make_hand_kinematics(model); + for (const auto hand : { Hand::Left, Hand::Right }) + { + CAPTURE(provider->model_id(), hand_name(hand)); + std::array angles{}; + const auto initial = provider->solve(hand, angles, kAllAngles); + REQUIRE(initial); + for (std::size_t finger = 0; finger < kMcpSlots.size(); ++finger) + { + const int proximal = kProximal[finger], intermediate = kIntermediate[finger]; + const double length = norm(delta(initial->joints[proximal].p, initial->joints[intermediate].p)); + for (const double bend : { .35, .7, 1.2 }) + { + CAPTURE(finger, bend); + angles.fill(0); + angles[kMcpSlots[finger]] = bend; + const auto moved = provider->solve(hand, angles, kAllAngles); + REQUIRE(moved); + CHECK(std::abs(norm(delta(moved->joints[proximal].p, moved->joints[intermediate].p)) - length) < + tolerance); + double quaternion_dot = 0; + for (int i = 0; i < 4; ++i) + quaternion_dot += initial->joints[proximal].q[i] * moved->joints[proximal].q[i]; + CHECK(std::abs(2 * std::acos(std::min(1.0, std::abs(quaternion_dot))) - bend) < 10 * tolerance); + const auto invalid = provider->solve(hand, angles, kAllAngles & ~(1u << kMcpSlots[finger])); + REQUIRE(invalid); + CHECK((invalid->position_valid_mask & (1u << proximal)) == 0); + } + } + } + } +} + +TEST_CASE("hand fitting remains mirrored between left and right", "[revohuman][geometry]") +{ + for (const auto model : { HandModel::Dv1, HandModel::P }) + { + const auto provider = make_hand_kinematics(model); + for (const double bend : { 0., .35, .7 }) + { + std::array angles{}; + angles.fill(bend); + const auto left = provider->solve(Hand::Left, angles, kAllAngles); + const auto right = provider->solve(Hand::Right, angles, kAllAngles); + REQUIRE(left); + REQUIRE(right); + CHECK(left->position_valid_mask == right->position_valid_mask); + CHECK(left->orientation_valid_mask == right->orientation_valid_mask); + for (std::size_t j = 0; j < kHandJointCount; ++j) + { + CAPTURE(provider->model_id(), bend, j); + const auto& l = left->joints[j]; + const auto& r = right->joints[j]; + CHECK(norm(delta({ -l.p[0], l.p[1], l.p[2] }, r.p)) < 1e-12); + const Quat mirrored{ l.q[0], -l.q[1], -l.q[2], l.q[3] }; + double qdot = 0; + for (std::size_t k = 0; k < 4; ++k) + qdot += mirrored[k] * r.q[k]; + CHECK(std::abs(qdot) == Approx(1).margin(1e-12)); + } + } + } +} + +TEST_CASE("hand model P has poses but no taxels", "[revohuman][geometry]") +{ + // P1 gloves have no tactile sensors, so the SDK has no taxel layout for their hand model. + const auto provider = make_hand_kinematics(HandModel::P); + CHECK(provider->model_id() == + "sdk-points:p:reviewed-openxr-symmetric-v6-b400ebf2df5abcbf1d8c88fd66ded8ec73cd5c35bb62cf28c92eb153e007edc2"); + for (const auto hand : { Hand::Left, Hand::Right }) + { + CAPTURE(hand_name(hand)); + for (const double bend : { 0.0, .5 }) + { + std::array angles{}; + angles.fill(bend); + const auto solved = provider->solve(hand, angles, kAllAngles); + REQUIRE(solved); + CHECK(solved->position_valid_mask == kMappedHandMask); + CHECK(solved->orientation_valid_mask == kMappedHandMask); + } + } + CHECK_THROWS_AS(make_sdk_tactile_shape(HandModel::P, Hand::Left), std::runtime_error); + CHECK_THROWS_AS(sdk_taxels(HandModel::P, Hand::Left, {}), std::runtime_error); +} + +#else +TEST_CASE("live hand poses fail at startup without the SDK point API", "[revohuman][geometry]") +{ + CHECK_THROWS_AS(make_hand_kinematics(HandModel::Dv1), std::runtime_error); + CHECK_THROWS_AS(make_hand_kinematics(HandModel::P), std::runtime_error); + CHECK_THROWS_AS(make_sdk_tactile_shape(HandModel::P, Hand::Left), std::runtime_error); +} +#endif + +#if REVOHUMAN_TEST_SDK_POINTS && REVOHUMAN_TEST_SDK_TAXEL_LAYOUT +namespace +{ +constexpr std::size_t kLayoutTaxels = RH_TACTILE_POINTS_TYPE_B; +// TactileMetadata stores poses as float32, so what is read back from it carries float rounding. +constexpr double kFloatPosition = 1e-7; // m +constexpr double kFloatDirection = 1e-6; + +double angle_between(const Vec3& a, const Vec3& b) +{ + return std::acos(std::clamp(dot(normalize(a), normalize(b)), -1.0, 1.0)); +} + +double det(const Vec3& a, const Vec3& b, const Vec3& c) +{ + return a[0] * (b[1] * c[2] - b[2] * c[1]) - a[1] * (b[0] * c[2] - b[2] * c[0]) + a[2] * (b[0] * c[1] - b[1] * c[0]); +} + +//! Largest position and normal-angle error of the skinned taxels against the SDK's at @p angles. +std::pair skin_error(const LoadedShape& shape, + const KinematicsProvider& provider, + Hand hand, + const std::array& angles) +{ + const auto kinematics = provider.solve(hand, angles, kAllAngles); + REQUIRE(kinematics); + REQUIRE(shape.has_valid_orientations(*kinematics)); + const auto expected = sdk_taxels(HandModel::Dv1, hand, angles); + const auto positions = shape.skin_positions(*kinematics); + const auto normals = shape.current_normals(*kinematics); + double position = 0, normal = 0; + for (std::size_t i = 0; i < positions.size(); ++i) + { + position = std::max(position, norm(delta(positions[i], expected.positions[i]))); + normal = std::max(normal, angle_between(normals[i], expected.normals[i])); + } + return { position, normal }; +} +} // namespace + +TEST_CASE("the TactileMetadata comes from the SDK taxel layout and matches it at the bind pose", + "[revohuman][tactile][geometry]") +{ + const auto provider = make_hand_kinematics(HandModel::Dv1); + // The OpenXR joint each SDK link's taxels ride on: DIP, PIP and (non-thumb) MCP links. + const std::array joint_of_link{ 9, 8, 7, -1, 14, 13, 12, -1, 19, 18, 17, + -1, 24, 23, 22, -1, 4, 3, -1, -1, -1 }; + for (const auto hand : { Hand::Left, Hand::Right }) + { + CAPTURE(hand_name(hand)); + const auto built = make_sdk_tactile_shape(HandModel::Dv1, hand); + CHECK(built.layout_source.rfind("cuff-surface-v", 0) == 0); + const LoadedShape shape(built.metadata); + REQUIRE(shape.taxel_count() == kLayoutTaxels); + CHECK(shape.bytes().size() <= shape_size_bound(kHandJointCount, kLayoutTaxels)); + const auto& lbs = *shape.shape().geometry_mapping_as_LBS(); + const auto* ids = shape.shape().segment_source_as_HandSkeletonRef()->joint_ids(); + REQUIRE(ids->size() == kHandJointCount); + const std::array zero{}; + const auto bind = provider->solve(hand, zero, kAllAngles); + REQUIRE(bind); + for (std::size_t j = 0; j < kHandJointCount; ++j) + { + CAPTURE(j); + CHECK(ids->Get(j) == j); + if (!(kMappedHandMask & (1u << j))) + continue; + const auto& pose = *lbs.segment_bind_poses()->Get(j); + CHECK(norm(delta({ pose.position().x(), pose.position().y(), pose.position().z() }, bind->joints[j].p)) < + kFloatPosition); + const Quat q{ pose.orientation().x(), pose.orientation().y(), pose.orientation().z(), pose.orientation().w() }; + double qdot = 0; + for (int k = 0; k < 4; ++k) + qdot += q[k] * bind->joints[j].q[k]; + CHECK(1 - std::abs(qdot) < kFloatDirection); + } + const auto taxels = sdk_taxels(HandModel::Dv1, hand, zero); + REQUIRE(taxels.positions.size() == kLayoutTaxels); + CHECK(taxels.source == built.layout_source); + for (std::size_t i = 0; i < kLayoutTaxels; ++i) + { + CAPTURE(i, taxels.links[i]); + const auto* segments = lbs.taxel_segment_indices()->Get(i)->values(); + const auto* weights = lbs.taxel_segment_weights()->Get(i)->values(); + CHECK(static_cast(ids->Get(segments->Get(0))) == joint_of_link.at(taxels.links[i])); + CHECK(weights->Get(0) == 1.0f); + for (unsigned k = 1; k < weights->size(); ++k) + CHECK(weights->Get(k) == 0.0f); + } + const auto positions = shape.skin_positions(*bind); + const auto normals = shape.current_normals(*bind); + for (std::size_t i = 0; i < kLayoutTaxels; ++i) + { + CAPTURE(i); + CHECK(norm(delta(positions[i], taxels.positions[i])) < kFloatPosition); + CHECK(norm(delta(normals[i], taxels.normals[i])) < kFloatDirection); + } + } +} + +TEST_CASE("the SDK taxels are the layout on rh_hand_model_fk, in the WRIST frame", "[revohuman][tactile][geometry]") +{ + // Compare in the corrected WRIST origin before checking rotation-invariant lengths and inner products. + std::array angles{}; + for (std::size_t i = 0; i < angles.size(); ++i) + angles[i] = .3 + .02 * static_cast(i); + for (const auto hand : { Hand::Left, Hand::Right }) + { + CAPTURE(hand_name(hand)); + const RhHand sdk_hand = hand == Hand::Left ? RH_HAND_LEFT : RH_HAND_RIGHT; + std::array links{}; + REQUIRE(rh_hand_model_fk(RH_HAND_MODEL_DV1, sdk_hand, angles.data(), nullptr, links.data()) == RH_OK); + auto layout = std::make_unique(); + layout->struct_size = sizeof(RhTaxelLayout); + REQUIRE(rh_hand_model_taxel_layout(RH_HAND_MODEL_DV1, sdk_hand, kLayoutTaxels, layout.get()) == RH_OK); + std::vector palm_p, palm_n; + for (std::size_t i = 0; i < kLayoutTaxels; ++i) + { + const auto& t = layout->taxels[i]; + const auto& l = links[t.link]; + Vec3 p{}, n{}; + for (int r = 0; r < 3; ++r) + { + p[r] = l.p[r]; + for (int c = 0; c < 3; ++c) + { + p[r] += l.r[3 * r + c] * t.p[c]; + n[r] += l.r[3 * r + c] * t.n[c]; + } + } + p[0] -= .01259782883756545; // Reviewed wrist depth in palm_link coordinates. + palm_p.push_back(p); + palm_n.push_back(n); + } + const auto taxels = sdk_taxels(HandModel::Dv1, hand, angles); + REQUIRE(taxels.positions.size() == kLayoutTaxels); + const auto& p = taxels.positions; + for (std::size_t i = 0; i < kLayoutTaxels; ++i) + { + CAPTURE(i); + const auto j = (i + 1) % kLayoutTaxels, k = (i + 97) % kLayoutTaxels; + CHECK(taxels.links[i] == layout->taxels[i].link); + CHECK(std::abs(norm(taxels.normals[i]) - 1) < 1e-12); + CHECK(std::abs(norm(p[i]) - norm(palm_p[i])) < 1e-12); + CHECK(std::abs(dot(p[i], p[j]) - dot(palm_p[i], palm_p[j])) < 1e-12); + CHECK(std::abs(dot(taxels.normals[i], p[i]) - dot(palm_n[i], palm_p[i])) < 1e-12); + CHECK(std::abs(det(p[i], p[j], p[k]) - det(palm_p[i], palm_p[j], palm_p[k])) < 1e-15); + } + } +} + +TEST_CASE("skinned taxels follow the SDK's when the fingers bend", "[revohuman][tactile][geometry]") +{ + // Taxels ride rigidly on the OpenXR joint of their link, and each of those joints is rigid on that + // link: DIP, PIP and thumb joints, and the non-thumb proximal joints carried with the MCP flexion. + const auto provider = make_hand_kinematics(HandModel::Dv1); + for (const auto hand : { Hand::Left, Hand::Right }) + { + CAPTURE(hand_name(hand)); + const LoadedShape shape(make_sdk_tactile_shape(HandModel::Dv1, hand).metadata); + for (const double bend : { .35, .7, 1.2 }) + { + CAPTURE(bend); + std::array angles{}; + for (std::size_t slot = 0; slot < kGloveJointCount; ++slot) + { + CAPTURE(slot); + angles.fill(0); + angles[slot] = bend; + const auto [position, normal] = skin_error(shape, *provider, hand, angles); + CHECK(position < 1e-6); + CHECK(normal < 1e-6); + } + angles.fill(bend); + const auto [position, normal] = skin_error(shape, *provider, hand, angles); + CHECK(position < 1e-6); + CHECK(normal < 1e-6); + } + } +} +#else +TEST_CASE("tactile has no metadata without the SDK taxel layout", "[revohuman][tactile][geometry]") +{ + CHECK_THROWS_AS(make_sdk_tactile_shape(HandModel::Dv1, Hand::Left), std::runtime_error); + CHECK_THROWS_AS(sdk_taxels(HandModel::Dv1, Hand::Left, {}), std::runtime_error); +} +#endif + +#if REVOHUMAN_TEST_SDK_VARIANT_MODEL +TEST_CASE("the SDK gives each product's hand model, with no default", "[revohuman][geometry]") +{ + CHECK(sdk_has_product_hand_models()); + CHECK(hand_model_of_product("P1") == HandModel::P); + CHECK(hand_model_of_product("PTV1") == HandModel::Dv1); + for (const char* code : { "", "auto", "P9" }) + { + CAPTURE(code); + CHECK_THROWS_AS(hand_model_of_product(code), std::runtime_error); + } +} +#else +TEST_CASE("no product has a hand model without the SDK's table", "[revohuman][geometry]") +{ + CHECK_FALSE(sdk_has_product_hand_models()); + CHECK_THROWS_AS(hand_model_of_product("PTV1"), std::runtime_error); +} +#endif + +TEST_CASE("manifest declares every per-hand status output", "[revohuman][status]") +{ + std::ifstream file(REVOHUMAN_MANIFEST); + REQUIRE(file.good()); + const std::string yaml{ std::istreambuf_iterator(file), std::istreambuf_iterator() }; + const std::regex paths("path: \\\"([^\\\"]+)\\\""); + std::set actual; + std::size_t count = 0; + for (auto it = std::sregex_iterator(yaml.begin(), yaml.end(), paths); it != std::sregex_iterator(); ++it) + { + actual.insert((*it)[1]); + ++count; + } + std::set expected; + for (const std::string side : { "left", "right" }) + for (const std::string prefix : { "/hand/", "/glove/revohuman_", "/tactile/revohuman_" }) + expected.insert(prefix + side); + CHECK(actual == expected); + CHECK(count == expected.size()); +} diff --git a/tests/cpp/plugins/revohuman/live/CMakeLists.txt b/tests/cpp/plugins/revohuman/live/CMakeLists.txt new file mode 100644 index 0000000000..08ef1bbe3a --- /dev/null +++ b/tests/cpp/plugins/revohuman/live/CMakeLists.txt @@ -0,0 +1,18 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The live source against a fake of the C SDK library, so it is tested without gloves. +add_executable(revohuman_live_tests + fake_sdk/fake_revohuman.cpp + fake_sdk/fake_revohuman.hpp + test_live_source.cpp + test_rh_frame.cpp +) + +target_link_libraries(revohuman_live_tests + PRIVATE + Catch2::Catch2WithMain + revohuman_plugin::revohuman_live +) + +catch_discover_tests(revohuman_live_tests) diff --git a/tests/cpp/plugins/revohuman/live/fake_sdk/fake_revohuman.cpp b/tests/cpp/plugins/revohuman/live/fake_sdk/fake_revohuman.cpp new file mode 100644 index 0000000000..bd63f4cef3 --- /dev/null +++ b/tests/cpp/plugins/revohuman/live/fake_sdk/fake_revohuman.cpp @@ -0,0 +1,780 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "fake_revohuman.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct RhKit +{ + std::array open{ false, false }; +}; + +struct RhGlove +{ + RhHand hand = RH_HAND_UNKNOWN; +}; + +struct RhScanList +{ + std::vector gloves; +}; + +struct RhSubscription +{ + RhHand hand = RH_HAND_UNKNOWN; + std::uint32_t capacity = 0; + std::deque queue; + std::uint64_t dropped = 0; + bool kit_closed = false; +}; + +namespace +{ + +struct Failure +{ + RhStatus status = RH_OK; + RhErrorCode code = RH_E_OK; + std::string message; +}; + +struct GloveState +{ + RhGlove handle; + std::string sn; + std::string unit_id; + std::map> joint_names; + bool present = true; + bool own_variant = false; + std::optional variant; + std::optional open_failure; + int open_failures_left = 0; + std::optional subscribe_failure; + int subscribe_failures_left = 0; + bool gate_ok = true; + RhSyncCause cause = RH_CAUSE_NONE; + std::optional end; + RhKit* kit = nullptr; + RhSubscription* sub = nullptr; +}; + +struct State +{ + fake_rh::Calls calls; + std::optional variant = "P1"; + std::optional open_failure; + int open_failures_left = 0; + std::optional pair_open_failure; + int pair_open_failures_left = 0; + std::optional scan_failure; + int scan_failures_left = 0; + std::array gloves; + std::vector subs; + std::vector kits; + + State() + { + gloves[0].handle.hand = RH_HAND_LEFT; + gloves[0].unit_id = "fake-unit-left"; + gloves[1].handle.hand = RH_HAND_RIGHT; + gloves[1].unit_id = "fake-unit-right"; + } +}; + +State& state() +{ + static State s; + return s; +} + +// Every entry point holds it; recursive because the controls call the helpers below. +std::recursive_mutex& state_mutex() +{ + static std::recursive_mutex m; + return m; +} + +using Lock = std::lock_guard; + +// Like the real SDK, the last error belongs to the calling thread. +thread_local RhErrorCode last_code = RH_E_OK; +thread_local std::string last_error; + +std::array, 2> open_delay_ms{}; +std::atomic opens_started_count{ 0 }; + +std::size_t slot(RhHand hand) +{ + if (hand != RH_HAND_LEFT && hand != RH_HAND_RIGHT) + { + throw std::logic_error("fake_rh: hand must be left or right"); + } + return hand == RH_HAND_LEFT ? 0 : 1; +} + +RhStatus fail(const Failure& f) +{ + last_code = f.code; + last_error = "E" + std::to_string(f.code) + " " + f.message; + return f.status; +} + +bool take_failure(std::optional& failure, int& left, Failure& out) +{ + if (!failure || left <= 0) + { + return false; + } + --left; + out = *failure; + return true; +} + +const char* side_of(std::size_t i) +{ + return i == 0 ? "left" : "right"; +} + +const char* port_of(std::size_t i) +{ + return i == 0 ? "/dev/ttyACM1" : "/dev/ttyACM0"; +} + +const std::optional& variant_of(const State& s, std::size_t i) +{ + return s.gloves[i].own_variant ? s.gloves[i].variant : s.variant; +} + +//! Sensors of a product as the SDK's product table lists them: only PTV1 has tactile and a wrist camera. +RhSensorMask sensors_of(const std::string& variant) +{ + const RhSensorMask base = RH_SENSOR_ENCODER | RH_SENSOR_TRACKER; + return variant == "PTV1" ? base | RH_SENSOR_TACTILE | RH_SENSOR_WRIST_CAMERA : base; +} + +std::string copied(const char* s) +{ + return s != nullptr ? std::string(s) : std::string(""); +} + +template +void put(char (&dst)[N], const std::string& src) +{ + std::strncpy(dst, src.c_str(), N - 1); + dst[N - 1] = '\0'; +} + +int32_t put_string(const std::string& s, char* buf, size_t buf_len) +{ + if (buf_len > 0) + { + const std::size_t n = std::min(s.size(), buf_len - 1); + std::memcpy(buf, s.data(), n); + buf[n] = '\0'; + } + return static_cast(s.size()); +} + +} // namespace + +namespace fake_rh +{ + +void reset() +{ + Lock lock(state_mutex()); + for (RhSubscription* sub : state().subs) + { + delete sub; + } + for (RhKit* kit : state().kits) + { + delete kit; + } + state() = State{}; + for (auto& delay : open_delay_ms) + { + delay = 0; + } + opens_started_count = 0; + last_code = RH_E_OK; + last_error.clear(); +} + +Calls& calls() +{ + return state().calls; +} + +Calls snapshot() +{ + Lock lock(state_mutex()); + return state().calls; +} + +int open_kits() +{ + Lock lock(state_mutex()); + return static_cast(state().kits.size()); +} + +int open_pair_kits() +{ + Lock lock(state_mutex()); + return static_cast(std::count_if( + state().kits.begin(), state().kits.end(), [](const RhKit* kit) { return kit->open[0] && kit->open[1]; })); +} + +void set_variant(std::optional code) +{ + Lock lock(state_mutex()); + state().variant = std::move(code); +} + +void set_variant(RhHand hand, std::optional code) +{ + Lock lock(state_mutex()); + GloveState& g = state().gloves[slot(hand)]; + g.own_variant = true; + g.variant = std::move(code); +} + +void set_glove_ids(RhHand hand, const std::string& sn, const std::string& unit_id) +{ + Lock lock(state_mutex()); + state().gloves[slot(hand)].sn = sn; + state().gloves[slot(hand)].unit_id = unit_id; +} + +void set_joint_name(RhHand hand, std::uint32_t index, std::optional name) +{ + Lock lock(state_mutex()); + state().gloves[slot(hand)].joint_names[index] = std::move(name); +} + +void set_present(RhHand hand, bool present) +{ + Lock lock(state_mutex()); + state().gloves[slot(hand)].present = present; +} + +void fail_next_scan(RhStatus status, RhErrorCode code, const std::string& message, int times) +{ + Lock lock(state_mutex()); + state().scan_failure = Failure{ status, code, message }; + state().scan_failures_left = times; +} + +void fail_next_open(RhStatus status, RhErrorCode code, const std::string& message, int times) +{ + Lock lock(state_mutex()); + state().open_failure = Failure{ status, code, message }; + state().open_failures_left = times; +} + +void fail_next_pair_open(RhStatus status, RhErrorCode code, const std::string& message, int times) +{ + Lock lock(state_mutex()); + state().pair_open_failure = Failure{ status, code, message }; + state().pair_open_failures_left = times; +} + +void fail_next_open(RhHand hand, RhStatus status, RhErrorCode code, const std::string& message, int times) +{ + Lock lock(state_mutex()); + GloveState& g = state().gloves[slot(hand)]; + g.open_failure = Failure{ status, code, message }; + g.open_failures_left = times; +} + +void set_open_delay_ms(RhHand hand, int ms) +{ + open_delay_ms[slot(hand)] = ms; +} + +int opens_started() +{ + return opens_started_count; +} + +void fail_next_subscribe(RhHand hand, RhStatus status, RhErrorCode code, const std::string& message, int times) +{ + Lock lock(state_mutex()); + GloveState& g = state().gloves[slot(hand)]; + g.subscribe_failure = Failure{ status, code, message }; + g.subscribe_failures_left = times; +} + +void push(RhHand hand, const RhGloveFrame& frame) +{ + Lock lock(state_mutex()); + RhSubscription* sub = state().gloves[slot(hand)].sub; + if (sub == nullptr) + { + throw std::logic_error("fake_rh::push: the glove has no subscription"); + } + if (sub->queue.size() >= sub->capacity) + { + sub->queue.pop_front(); + ++sub->dropped; + } + sub->queue.push_back(frame); +} + +void end(RhHand hand, RhStatus status, RhErrorCode code, const std::string& message) +{ + Lock lock(state_mutex()); + state().gloves[slot(hand)].end = Failure{ status, code, message }; +} + +void set_gate(RhHand hand, bool ok, RhSyncCause cause) +{ + Lock lock(state_mutex()); + state().gloves[slot(hand)].gate_ok = ok; + state().gloves[slot(hand)].cause = cause; +} + +RhGloveFrame frame(RhHand hand, std::int64_t sample_mono_ns) +{ + RhGloveFrame f; + std::memset(&f, 0, sizeof(f)); + f.struct_size = sizeof(RhGloveFrame); + f.hand = hand; + f.generation = 1; + f.flags = RH_FRAME_FLAG_GATE_OPEN | RH_FRAME_FLAG_HAS_SAMPLE_TIME; + f.device_tick = static_cast(sample_mono_ns / 1'000'000); + f.joints_valid_mask = (1u << RH_JOINT_COUNT) - 1u; + f.sample_wall_ns = sample_mono_ns; + f.sample_mono_ns = sample_mono_ns; + for (std::size_t i = 0; i < RH_JOINT_COUNT; ++i) + { + f.joints_deg[i] = 10.0 * static_cast(i); + } + return f; +} + +} // namespace fake_rh + +extern "C" +{ + + void rh_init_options_init(RhInitOptions* opts) + { + std::memset(opts, 0, sizeof(*opts)); + opts->struct_size = sizeof(*opts); + opts->log_level = RH_LOG_WARNING; + } + + RhStatus rh_init(const RhInitOptions*) + { + Lock lock(state_mutex()); + ++state().calls.init; + return RH_OK; + } + + const char* rh_version(void) + { + return "0.0.0-fake"; + } + + void rh_set_log_callback(RhLogCallback cb, void*) + { + Lock lock(state_mutex()); + state().calls.log_callback_set = cb != nullptr; + } + + void rh_shutdown(void) + { + Lock lock(state_mutex()); + State& s = state(); + ++s.calls.shutdown; + s.calls.kits_open_at_shutdown = static_cast(s.kits.size()); + s.calls.log_callback_set_at_shutdown = s.calls.log_callback_set; + } + + const char* rh_error_class_name(RhErrorCode code) + { + switch (code) + { + case RH_E_REVOHUMAN_ERROR: + return "RevoHumanError"; + case RH_E_DISCONNECTED: + return "TransportDisconnected"; + case RH_E_PORT_BUSY: + return "PortBusyError"; + case RH_E_FIRMWARE_INCOMPATIBLE: + return "FirmwareIncompatible"; + case RH_E_UNREGISTERED_GLOVE: + return "UnregisteredGloveError"; + case RH_E_PRODUCT_UNIDENTIFIED: + return "ProductUnidentified"; + default: + return nullptr; + } + } + + RhErrorCode rh_last_error_code(void) + { + return last_code; + } + + int32_t rh_last_error(char* buf, size_t buf_len) + { + return put_string(last_error, buf, buf_len); + } + + void rh_kit_options_init(RhKitOptions* opts) + { + std::memset(opts, 0, sizeof(*opts)); + opts->struct_size = sizeof(*opts); + opts->hands = RH_HANDS_BOTH; + opts->product = "auto"; + opts->strict = 1; + opts->sync = RH_SYNC_ON; + opts->rate_hz = 200.0; + opts->glove.struct_size = sizeof(opts->glove); + } + + RhStatus rh_kit_open(const RhKitOptions* opts, RhKit** out) + { + const std::array wanted{ opts->hands != RH_HANDS_RIGHT, opts->hands != RH_HANDS_LEFT }; + ++opens_started_count; + const int delay = std::max(wanted[0] ? open_delay_ms[0].load() : 0, wanted[1] ? open_delay_ms[1].load() : 0); + if (delay > 0) + { + // Outside the lock, so other threads go on while this open is "connecting". + std::this_thread::sleep_for(std::chrono::milliseconds(delay)); + } + Lock lock(state_mutex()); + State& s = state(); + ++s.calls.kit_open; + s.calls.hands = opts->hands; + s.calls.open_hands.push_back(opts->hands); + s.calls.product = copied(opts->product); + s.calls.tracker_config_null = opts->tracker_config_json == nullptr; + s.calls.rate_hz = opts->rate_hz; + s.calls.sync = opts->sync; + Failure failure; + if (take_failure(s.open_failure, s.open_failures_left, failure)) + { + return fail(failure); + } + for (std::size_t i = 0; i < 2; ++i) + { + GloveState& g = s.gloves[i]; + if (!wanted[i]) + { + continue; + } + if (!g.present) + { + return fail({ RH_ERR_NOT_FOUND, RH_E_REVOHUMAN_ERROR, + std::string("RevoHumanError: no ") + side_of(i) + " glove plugged in" }); + } + if (take_failure(g.open_failure, g.open_failures_left, failure)) + { + return fail(failure); + } + if (g.kit != nullptr) + { + return fail({ RH_ERR_BUSY, RH_E_PORT_BUSY, + std::string("PortBusyError: the ") + side_of(i) + " glove is in another kit" }); + } + } + // Like the SDK, which reads both gloves' device info before it pairs them. + if (wanted[0] && wanted[1] && variant_of(s, 0) && variant_of(s, 1) && *variant_of(s, 0) != *variant_of(s, 1)) + { + return fail({ RH_ERR_CONFLICT, RH_E_VARIANT_MISMATCH, + "VariantMismatch: the left glove is " + *variant_of(s, 0) + ", the right glove is " + + *variant_of(s, 1) }); + } + if (wanted[0] && wanted[1] && take_failure(s.pair_open_failure, s.pair_open_failures_left, failure)) + { + return fail(failure); + } + auto* kit = new RhKit; + kit->open = wanted; + for (std::size_t i = 0; i < 2; ++i) + { + if (wanted[i]) + { + s.gloves[i].kit = kit; + } + } + s.kits.push_back(kit); + *out = kit; + return RH_OK; + } + + void rh_kit_close(RhKit* kit) + { + Lock lock(state_mutex()); + State& s = state(); + if (kit == nullptr || std::find(s.kits.begin(), s.kits.end(), kit) == s.kits.end()) + { + return; + } + ++s.calls.kit_close; + for (RhSubscription* sub : s.subs) + { + if (s.gloves[slot(sub->hand)].kit == kit) + { + sub->kit_closed = true; + } + } + for (GloveState& g : s.gloves) + { + if (g.kit == kit) + { + g.kit = nullptr; + } + } + std::erase(s.kits, kit); + delete kit; + } + + RhStatus rh_kit_identity(const RhKit* kit, RhProductIdentity* out) + { + Lock lock(state_mutex()); + const State& s = state(); + const std::optional* variant = &s.variant; + for (std::size_t i = 0; i < 2; ++i) + { + if (kit->open[i] && s.gloves[i].own_variant) + { + variant = &s.gloves[i].variant; + } + } + if (!*variant) + { + return fail({ RH_ERR_STATE, RH_E_PRODUCT_UNIDENTIFIED, "ProductUnidentified: no evidence" }); + } + put(out->variant_code, **variant); + out->variant_sensors = sensors_of(**variant); + return RH_OK; + } + + RhGlove* rh_kit_glove(RhKit* kit, RhHand hand) + { + Lock lock(state_mutex()); + if (kit == nullptr || (hand != RH_HAND_LEFT && hand != RH_HAND_RIGHT) || !kit->open[slot(hand)]) + { + return nullptr; + } + return &state().gloves[slot(hand)].handle; + } + + RhStatus rh_glove_info(const RhGlove* glove, RhGloveInfo* out) + { + Lock lock(state_mutex()); + if (out->struct_size < sizeof(RhGloveInfo)) + { + return fail({ RH_ERR_INVALID_ARG, RH_E_REVOHUMAN_ERROR, "RevoHumanError: struct_size" }); + } + const GloveState& g = state().gloves[slot(glove->hand)]; + out->hand = glove->hand; + put(out->port, port_of(slot(glove->hand))); + put(out->unit_id, g.unit_id); + put(out->sn, g.sn); + put(out->fw_version, "fake-1.0"); + out->encoder_count = RH_JOINT_COUNT; + const auto& variant = variant_of(state(), slot(glove->hand)); + out->tactile_point_count = variant && (sensors_of(*variant) & RH_SENSOR_TACTILE) ? 211 : 0; + return RH_OK; + } + + int32_t rh_joint_name(RhHand hand, uint32_t index, char* buf, size_t buf_len) + { + if (index >= RH_JOINT_COUNT) + { + return fail({ RH_ERR_INVALID_ARG, RH_E_REVOHUMAN_ERROR, "RevoHumanError: joint index" }); + } + if (hand == RH_HAND_LEFT || hand == RH_HAND_RIGHT) + { + Lock lock(state_mutex()); + const auto& names = state().gloves[slot(hand)].joint_names; + if (const auto it = names.find(index); it != names.end()) + { + if (!it->second) + { + return fail({ RH_ERR_INVALID_ARG, RH_E_REVOHUMAN_ERROR, "RevoHumanError: no joint name" }); + } + return put_string(*it->second, buf, buf_len); + } + } + const char* side = hand == RH_HAND_LEFT ? "left_" : (hand == RH_HAND_RIGHT ? "right_" : ""); + return put_string(std::string(side) + "fake_joint_" + std::to_string(index), buf, buf_len); + } + + RhStatus rh_joints_to_radians(const char* product_code, const double deg[RH_JOINT_COUNT], double rad[RH_JOINT_COUNT]) + { + Lock lock(state_mutex()); + state().calls.joints_product = copied(product_code); + const double offset = product_code != nullptr && std::strcmp(product_code, "PTV1") == 0 ? 1.0 : 0.0; + for (std::size_t i = 0; i < RH_JOINT_COUNT; ++i) + { + rad[i] = deg[i] / 100.0 + offset; + } + return RH_OK; + } + + void rh_scan_options_init(RhScanOptions* opts) + { + std::memset(opts, 0, sizeof(*opts)); + opts->struct_size = sizeof(*opts); + } + + RhStatus rh_scan(const RhScanOptions*, RhScanList** out) + { + Lock lock(state_mutex()); + State& s = state(); + ++s.calls.scan; + *out = nullptr; + Failure failure; + if (take_failure(s.scan_failure, s.scan_failures_left, failure)) + { + return fail(failure); + } + auto* list = new RhScanList; + for (std::size_t i = 0; i < 2; ++i) + { + const GloveState& g = s.gloves[i]; + if (!g.present) + { + continue; + } + RhDiscoveredGlove found; + std::memset(&found, 0, sizeof(found)); + found.struct_size = sizeof(found); + found.hand = g.handle.hand; + put(found.port, port_of(i)); + put(found.unit_id, g.unit_id); + found.registered = 1; + list->gloves.push_back(found); + } + *out = list; + return RH_OK; + } + + size_t rh_scan_count(const RhScanList* list) + { + return list != nullptr ? list->gloves.size() : 0; + } + + RhStatus rh_scan_get(const RhScanList* list, size_t index, RhDiscoveredGlove* out) + { + if (list == nullptr || index >= list->gloves.size()) + { + return fail({ RH_ERR_INVALID_ARG, RH_E_REVOHUMAN_ERROR, "RevoHumanError: scan index" }); + } + *out = list->gloves[index]; + return RH_OK; + } + + void rh_scan_free(RhScanList* list) + { + delete list; + } + + void rh_subscribe_options_init(RhSubscribeOptions* opts) + { + std::memset(opts, 0, sizeof(*opts)); + opts->struct_size = sizeof(*opts); + opts->queue_size = 64; + } + + RhStatus rh_glove_subscribe( + RhGlove* glove, const RhSubscribeOptions* opts, RhGloveFrameCallback cb, void*, RhSubscription** out) + { + Lock lock(state_mutex()); + State& s = state(); + ++s.calls.subscribe; + s.calls.queue_size = opts->queue_size; + s.calls.ungated = opts->ungated; + s.calls.callback_given = cb != nullptr; + GloveState& g = s.gloves[slot(glove->hand)]; + Failure failure; + if (take_failure(g.subscribe_failure, g.subscribe_failures_left, failure)) + { + return fail(failure); + } + auto* sub = new RhSubscription; + sub->hand = glove->hand; + sub->capacity = opts->queue_size; + s.subs.push_back(sub); + g.sub = sub; + g.end.reset(); + *out = sub; + return RH_OK; + } + + RhStatus rh_sub_recv_glove(RhSubscription* sub, int32_t, RhGloveFrame* out) + { + Lock lock(state_mutex()); + if (!sub->queue.empty()) + { + *out = sub->queue.front(); + sub->queue.pop_front(); + return RH_OK; + } + if (sub->kit_closed) + { + return fail({ RH_ERR_DISCONNECTED, RH_E_DISCONNECTED, "TransportDisconnected: the kit is closed" }); + } + const GloveState& g = state().gloves[slot(sub->hand)]; + if (g.end) + { + return fail(*g.end); + } + return fail({ RH_ERR_TIMEOUT, RH_E_REVOHUMAN_ERROR, "RevoHumanError: timeout" }); + } + + uint64_t rh_sub_dropped(const RhSubscription* sub) + { + Lock lock(state_mutex()); + return sub->dropped; + } + + void rh_sub_close(RhSubscription* sub) + { + Lock lock(state_mutex()); + State& s = state(); + ++s.calls.sub_close; + if (sub->kit_closed) + { + ++s.calls.sub_close_after_kit_close; + } + GloveState& g = s.gloves[slot(sub->hand)]; + if (g.sub == sub) + { + g.sub = nullptr; + } + std::erase(s.subs, sub); + delete sub; + } + + bool rh_glove_capture_ok(const RhGlove* glove) + { + Lock lock(state_mutex()); + return state().gloves[slot(glove->hand)].gate_ok; + } + + RhSyncCause rh_glove_capture_cause(const RhGlove* glove) + { + Lock lock(state_mutex()); + const GloveState& g = state().gloves[slot(glove->hand)]; + return g.gate_ok ? RH_CAUSE_NONE : g.cause; + } + +} // extern "C" diff --git a/tests/cpp/plugins/revohuman/live/fake_sdk/fake_revohuman.hpp b/tests/cpp/plugins/revohuman/live/fake_sdk/fake_revohuman.hpp new file mode 100644 index 0000000000..b2a365754a --- /dev/null +++ b/tests/cpp/plugins/revohuman/live/fake_sdk/fake_revohuman.hpp @@ -0,0 +1,101 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +// A fake of the RevoHuman C SDK functions the live source calls, with controls to script it. It +// has two gloves, each of which can be plugged in or not and belongs to at most one open kit (one +// kit may hold both, unless the SDK identifies different products on them); it +// needs no device and starts no thread, and every function may be called from any thread. Its +// joint mapping is deliberately not the real one, so tests can tell the SDK's result from anything +// else: rad = deg / 100, plus 1 for product "PTV1". Joint names are "_fake_joint_". + +#include +#include +#include +#include +#include + +namespace fake_rh +{ + +//! What the code under test asked of the SDK. Option strings are copied; NULL reads as "". +struct Calls +{ + int init = 0; + int kit_open = 0; + int kit_close = 0; + int scan = 0; + int subscribe = 0; + int sub_close = 0; + int sub_close_after_kit_close = 0; + bool log_callback_set = false; //!< A callback is installed now (passing NULL removes it). + int shutdown = 0; + int kits_open_at_shutdown = -1; //!< Kits still open at the last rh_shutdown. + bool log_callback_set_at_shutdown = false; + + RhHands hands = -1; //!< Of the last rh_kit_open. + std::vector open_hands; //!< Of every rh_kit_open, in order. + std::string product; + bool tracker_config_null = false; + double rate_hz = 0.0; + RhSyncPolicy sync = -1; + + std::uint32_t queue_size = 0; + int ungated = -1; + bool callback_given = false; + + std::string joints_product; //!< product_code of the last rh_joints_to_radians call. +}; + +//! Forgets everything, including kits and subscriptions the code under test leaked. +void reset(); +//! Only while no other thread calls the SDK; snapshot() otherwise. +Calls& calls(); +Calls snapshot(); +//! Kits open now. +int open_kits(); +//! Kits open now that hold both gloves. +int open_pair_kits(); + +//! The variant code rh_kit_identity reports (default "P1"); nullopt makes it fail. Its sensors follow +//! the SDK's product table: tactile only for "PTV1". +void set_variant(std::optional code); +//! The same for this glove. rh_kit_open of both gloves fails with RH_E_VARIANT_MISMATCH when they +//! have different ones. +void set_variant(RhHand hand, std::optional code); +//! Firmware SN and USB unit id of a glove (defaults: no SN, "fake-unit-"). +void set_glove_ids(RhHand hand, const std::string& sn, const std::string& unit_id); +//! The name rh_joint_name gives this joint of this hand; nullopt makes it fail with +//! RH_ERR_INVALID_ARG. +void set_joint_name(RhHand hand, std::uint32_t index, std::optional name); +//! Whether the glove is plugged in (default: both are). rh_kit_open of an absent glove fails with +//! RH_ERR_NOT_FOUND and E1000, as the real SDK does (E1400 is a glove it cannot assign to a hand), +//! and rh_scan leaves it out; a kit already holding it is not affected, see end(). +void set_present(RhHand hand, bool present); +//! Makes the next `times` rh_scan calls fail with this status and error. +void fail_next_scan(RhStatus status, RhErrorCode code, const std::string& message, int times = 1); +//! Makes the next `times` rh_kit_open calls fail with this status and error. +void fail_next_open(RhStatus status, RhErrorCode code, const std::string& message, int times = 1); +//! The same for opens of this glove only. +void fail_next_open(RhHand hand, RhStatus status, RhErrorCode code, const std::string& message, int times = 1); +//! The same for opens of both gloves in one kit only, as when they cannot be synced. +void fail_next_pair_open(RhStatus status, RhErrorCode code, const std::string& message, int times = 1); +//! rh_kit_open of this glove sleeps this long before it does anything. +void set_open_delay_ms(RhHand hand, int ms); +//! rh_kit_open calls started so far, counted before their delay. +int opens_started(); +//! Makes the next `times` rh_glove_subscribe calls of this glove fail with this status and error. +void fail_next_subscribe(RhHand hand, RhStatus status, RhErrorCode code, const std::string& message, int times = 1); + +//! Queues a frame on the glove's subscription, dropping the oldest when it is full. +void push(RhHand hand, const RhGloveFrame& frame); +//! Once its queue is empty, the glove's subscription fails with this status and error. A new +//! subscription of the glove starts without it. +void end(RhHand hand, RhStatus status, RhErrorCode code, const std::string& message); +void set_gate(RhHand hand, bool ok, RhSyncCause cause); + +//! A gated frame sampled at sample_mono_ns, every joint valid, joint i at 10 * i degrees. +RhGloveFrame frame(RhHand hand, std::int64_t sample_mono_ns); + +} // namespace fake_rh diff --git a/tests/cpp/plugins/revohuman/live/test_live_source.cpp b/tests/cpp/plugins/revohuman/live/test_live_source.cpp new file mode 100644 index 0000000000..923c810abe --- /dev/null +++ b/tests/cpp/plugins/revohuman/live/test_live_source.cpp @@ -0,0 +1,1146 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "fake_sdk/fake_revohuman.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::revohuman; +using Catch::Matchers::ContainsSubstring; + +namespace +{ + +constexpr std::int64_t kMs = 1'000'000; +constexpr std::int64_t kStart = 5'000 * kMs; + +struct FakeSdk +{ + FakeSdk() + { + fake_rh::reset(); + } + ~FakeSdk() + { + fake_rh::reset(); + } +}; + +//! Opens and closes inside poll(), so every step of a test is deterministic. +LiveSourceConfig inline_config() +{ + LiveSourceConfig config; + config.background_open = false; + return config; +} + +//! Only the left glove is plugged in. +LiveSourceConfig left_only() +{ + fake_rh::set_present(RH_HAND_RIGHT, false); + return inline_config(); +} + +using WallClock = std::chrono::steady_clock; + +//! Polls, with now_ns advancing like the wall clock from start_ns, until done() or 5 s passed. +template +bool poll_until(LiveSource& source, std::int64_t start_ns, std::vector& batches, Done done) +{ + const auto t0 = WallClock::now(); + while (WallClock::now() - t0 < std::chrono::seconds(5)) + { + const auto elapsed = std::chrono::duration_cast(WallClock::now() - t0).count(); + batches.push_back(source.poll(start_ns + elapsed)); + if (done()) + { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + return false; +} + +template +std::vector records_of(const SourceBatch& batch) +{ + std::vector out; + for (const auto& record : batch.records) + { + if (const auto* r = std::get_if(&record)) + { + out.push_back(*r); + } + } + return out; +} + +//! Joint poses that encode the input: joint 2 sits at x = joints_rad[0]. Gives none when joint 0 is +//! invalid. +class EchoKinematics : public KinematicsProvider +{ +public: + explicit EchoKinematics(std::string id) : id_(std::move(id)) + { + } + std::string model_id() const override + { + return id_; + } + std::optional solve(Hand, + const std::array& joints_rad, + std::uint32_t joints_valid_mask) const override + { + if ((joints_valid_mask & 1u) == 0) + { + return std::nullopt; + } + HandKinematics k; + k.joints[2].p = { joints_rad[0], 0.0, 0.0 }; + return k; + } + +private: + std::string id_; +}; + +//! Makes an EchoKinematics named "echo:" and records the products it was asked for, from +//! whichever thread opens the glove. +struct EchoFactory +{ + std::shared_ptr mutex = std::make_shared(); + std::shared_ptr> products = std::make_shared>(); + + KinematicsFactory make() const + { + return [mutex = mutex, products = products](const std::string& product_code) + { + std::lock_guard lock(*mutex); + products->push_back(product_code); + return std::make_shared("echo:" + product_code); + }; + } + std::vector asked() const + { + std::lock_guard lock(*mutex); + return *products; + } +}; + +} // namespace + +TEST_CASE("live source opens both plugged-in gloves together in one kit", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSourceConfig config = inline_config(); + config.queue_size = 32; + LiveSource source(config); + + const fake_rh::Calls& calls = fake_rh::calls(); + CHECK(calls.init == 1); + CHECK(calls.log_callback_set); + CHECK(calls.scan == 1); + CHECK(calls.open_hands == std::vector{ RH_HANDS_BOTH }); + // Sampled on the same USB frame. + CHECK(calls.sync == RH_SYNC_ON); + // Nothing declares the product: the SDK identifies it on the gloves. + CHECK(calls.product == "auto"); + CHECK(calls.tracker_config_null); + CHECK(calls.rate_hz == 200.0); + CHECK(calls.subscribe == 2); + CHECK(calls.queue_size == 32); + CHECK(calls.ungated == 0); + CHECK_FALSE(calls.callback_given); + CHECK(fake_rh::open_kits() == 1); + CHECK(fake_rh::open_pair_kits() == 1); + CHECK(source.opened(Hand::Left)); + CHECK(source.opened(Hand::Right)); + CHECK(source.paired(Hand::Left)); + CHECK(source.paired(Hand::Right)); + + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart)); + fake_rh::push(RH_HAND_RIGHT, fake_rh::frame(RH_HAND_RIGHT, kStart)); + const SourceBatch batch = source.poll(kStart); + CHECK(records_of(batch).size() == 2); + const auto samples = records_of(batch); + REQUIRE(samples.size() == 2); + CHECK(samples[0].hand == Hand::Left); + CHECK(samples[1].hand == Hand::Right); + CHECK_FALSE(batch.hand_restarted[0]); + CHECK_FALSE(batch.hand_restarted[1]); + // Already together: nothing is reopened. + source.poll(kStart + 10'000 * kMs); + CHECK(calls.kit_open == 1); + CHECK(calls.kit_close == 0); +} + +TEST_CASE("live source streams the one glove that is plugged in", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSource source(left_only()); + const fake_rh::Calls& calls = fake_rh::calls(); + CHECK(calls.product == "auto"); + CHECK(calls.open_hands == std::vector{ RH_HANDS_LEFT, RH_HANDS_RIGHT }); + CHECK(calls.subscribe == 1); + CHECK(source.opened(Hand::Left)); + CHECK_FALSE(source.opened(Hand::Right)); + CHECK_FALSE(source.paired(Hand::Left)); + + const SourceBatch batch = source.poll(kStart); + const auto metas = records_of(batch); + REQUIRE(metas.size() == 1); + CHECK(metas[0].hand == Hand::Left); + CHECK_FALSE(batch.finished); + + CHECK_FALSE(source.glove_note(Hand::Left)); + const auto note = source.glove_note(Hand::Right); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK_THAT(note->text, ContainsSubstring("RH_ERR_NOT_FOUND: E1000 RevoHumanError")); +} + +TEST_CASE("live source rejects a config without a queue", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSourceConfig no_queue = inline_config(); + no_queue.queue_size = 0; + CHECK_THROWS_AS(LiveSource(no_queue), std::invalid_argument); + CHECK(fake_rh::calls().scan == 0); + CHECK(fake_rh::calls().kit_open == 0); +} + +TEST_CASE("live source delivers each hand's metadata once, before its samples", "[revohuman][live]") +{ + FakeSdk sdk; + fake_rh::set_glove_ids(RH_HAND_LEFT, "FAKE-SN-LEFT", "1-1.2"); + fake_rh::set_glove_ids(RH_HAND_RIGHT, "", ""); + LiveSource source(inline_config()); + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart)); + + const SourceBatch first = source.poll(kStart); + REQUIRE(first.records.size() == 3); + const auto* left = std::get_if(&first.records[0]); + REQUIRE(left != nullptr); + CHECK(std::holds_alternative(first.records[1])); + const auto* right = std::get_if(&first.records[2]); + REQUIRE(right != nullptr); + CHECK_FALSE(first.hand_restarted[0]); + CHECK_FALSE(first.hand_restarted[1]); + + CHECK(left->hand == Hand::Left); + CHECK(left->product_code == "P1"); + // P1 has no tactile sensors; the SDK's product table says so. + CHECK_FALSE(left->has_tactile); + CHECK(left->device_id == "FAKE-SN-LEFT"); + CHECK(left->joint_names[0] == "left_fake_joint_0"); + CHECK(left->joint_names[20] == "left_fake_joint_20"); + CHECK(right->hand == Hand::Right); + CHECK(right->device_id == "revohuman_right"); + CHECK(right->joint_names[3] == "right_fake_joint_3"); + + fake_rh::push(RH_HAND_RIGHT, fake_rh::frame(RH_HAND_RIGHT, kStart + 5 * kMs)); + const SourceBatch second = source.poll(kStart + 5 * kMs); + CHECK(records_of(second).empty()); + CHECK(records_of(second).size() == 1); + CHECK(source.records_released() == 4); + CHECK_FALSE(second.finished); +} + +TEST_CASE("live source names a glove without an SN by its hand, not by its USB port", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSource source(left_only()); + const auto metas = records_of(source.poll(kStart)); + REQUIRE(metas.size() == 1); + CHECK(metas[0].device_id == "revohuman_left"); +} + +TEST_CASE("live source takes joint angles from rh_joints_to_radians for the identified product", "[revohuman][live]") +{ + FakeSdk sdk; + SECTION("identified product") + { + fake_rh::set_variant("PTV1"); + LiveSource source(left_only()); + CHECK(source.product_code(Hand::Left) == "PTV1"); + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart)); + const auto samples = records_of(source.poll(kStart)); + REQUIRE(samples.size() == 1); + CHECK(fake_rh::calls().joints_product == "PTV1"); + CHECK(samples[0].joints_rad[0] == Catch::Approx(1.0)); + CHECK(samples[0].joints_rad[7] == Catch::Approx(1.7)); + CHECK(samples[0].sample_mono_ns == kStart); + CHECK(samples[0].has_sample_time); + CHECK(samples[0].joints_valid_mask == (1u << 21) - 1u); + CHECK_FALSE(samples[0].kinematics); + } + SECTION("a glove whose product the SDK cannot tell is not opened yet, with no default table") + { + fake_rh::set_variant(std::nullopt); + LiveSource source(left_only()); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK(fake_rh::open_kits() == 0); + CHECK(source.product_code(Hand::Left).empty()); + auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK_THAT(note->text, ContainsSubstring("rh_kit_identity failed (RH_ERR_STATE): E1803 ProductUnidentified (the " + "SDK cannot identify the glove product)")); + + fake_rh::set_variant("P1"); + source.poll(kStart); + source.poll(kStart + 2'000 * kMs); + REQUIRE(source.opened(Hand::Left)); + CHECK(source.product_code(Hand::Left) == "P1"); + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 2'000 * kMs)); + REQUIRE(records_of(source.poll(kStart + 2'010 * kMs)).size() == 1); + CHECK(fake_rh::calls().joints_product == "P1"); + } + SECTION("a glove the SDK opened without a product code is given up") + { + fake_rh::set_variant(""); + LiveSource source(left_only()); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK(fake_rh::open_kits() == 0); + const auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Ended); + CHECK_THAT(note->text, ContainsSubstring("without identifying its product")); + CHECK(fake_rh::calls().joints_product.empty()); + } + SECTION("each glove with the product identified on it") + { + // The SDK cannot open two products in one kit, so each glove streams on its own. + fake_rh::set_variant(RH_HAND_RIGHT, "PTV1"); + LiveSource source(inline_config()); + CHECK(fake_rh::calls().open_hands == std::vector{ RH_HANDS_BOTH, RH_HANDS_LEFT, RH_HANDS_RIGHT }); + CHECK(fake_rh::open_kits() == 2); + CHECK_FALSE(source.paired(Hand::Left)); + CHECK_FALSE(source.paired(Hand::Right)); + CHECK(source.product_code(Hand::Left) == "P1"); + CHECK(source.product_code(Hand::Right) == "PTV1"); + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart)); + fake_rh::push(RH_HAND_RIGHT, fake_rh::frame(RH_HAND_RIGHT, kStart)); + const SourceBatch batch = source.poll(kStart); + const auto metas = records_of(batch); + REQUIRE(metas.size() == 2); + CHECK(metas[0].product_code == "P1"); + CHECK_FALSE(metas[0].has_tactile); + CHECK(metas[1].product_code == "PTV1"); + CHECK(metas[1].has_tactile); + const auto samples = records_of(batch); + REQUIRE(samples.size() == 2); + CHECK(samples[0].hand == Hand::Left); + CHECK(samples[0].joints_rad[7] == Catch::Approx(0.7)); + CHECK(samples[1].hand == Hand::Right); + CHECK(samples[1].joints_rad[7] == Catch::Approx(1.7)); + // They are not tried together again. + source.poll(kStart + 10'000 * kMs); + CHECK(fake_rh::calls().kit_open == 3); + CHECK(fake_rh::calls().kit_close == 0); + } +} + +TEST_CASE("live source drops frames it cannot use and counts them", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSource source(left_only()); + + RhGloveFrame wrong_hand = fake_rh::frame(RH_HAND_RIGHT, kStart); + RhGloveFrame no_time = fake_rh::frame(RH_HAND_LEFT, kStart + 5 * kMs); + no_time.flags &= ~RH_FRAME_FLAG_HAS_SAMPLE_TIME; + RhGloveFrame bad_tactile = fake_rh::frame(RH_HAND_LEFT, kStart + 10 * kMs); + bad_tactile.flags |= RH_FRAME_FLAG_HAS_TACTILE; + bad_tactile.tactile.point_count = RH_TACTILE_MAX_POINTS + 1; + const RhGloveFrame good = fake_rh::frame(RH_HAND_LEFT, kStart + 15 * kMs); + for (const RhGloveFrame& f : { wrong_hand, no_time, bad_tactile, good }) + { + fake_rh::push(RH_HAND_LEFT, f); + } + + const auto samples = records_of(source.poll(kStart + 15 * kMs)); + REQUIRE(samples.size() == 1); + CHECK(samples[0].sample_mono_ns == kStart + 15 * kMs); + const LiveSource::HandStats stats = source.stats(Hand::Left); + CHECK(stats.frames == 4); + CHECK(stats.rejected == 3); + CHECK(stats.opens == 1); + CHECK(stats.ends == 0); +} + +TEST_CASE("live source attaches joint poses from the kinematics provider", "[revohuman][live]") +{ + FakeSdk sdk; + fake_rh::set_variant("PTV1"); + const EchoFactory factory; + LiveSource source(left_only(), factory.make()); + CHECK(factory.asked() == std::vector{ "PTV1" }); + CHECK(source.model_id(Hand::Left) == "echo:PTV1"); + + RhGloveFrame missing_joint = fake_rh::frame(RH_HAND_LEFT, kStart + 5 * kMs); + missing_joint.joints_valid_mask &= ~1u; + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart)); + fake_rh::push(RH_HAND_LEFT, missing_joint); + + const SourceBatch batch = source.poll(kStart + 5 * kMs); + const auto metas = records_of(batch); + REQUIRE(metas.size() == 1); + const auto samples = records_of(batch); + REQUIRE(samples.size() == 2); + REQUIRE(samples[0].kinematics); + // The provider sees the SDK's radians, not the device degrees. + CHECK(samples[0].kinematics->joints[2].p[0] == Catch::Approx(1.0)); + CHECK_FALSE(samples[1].kinematics); + CHECK(source.stats(Hand::Left).without_kinematics == 1); +} + +TEST_CASE("live source makes each glove's kinematics for the product identified on it", "[revohuman][live]") +{ + FakeSdk sdk; + fake_rh::set_variant(RH_HAND_RIGHT, "PTV1"); + const EchoFactory factory; + LiveSource source(inline_config(), factory.make()); + CHECK(factory.asked() == std::vector{ "P1", "PTV1" }); + CHECK(source.model_id(Hand::Left) == "echo:P1"); + CHECK(source.model_id(Hand::Right) == "echo:PTV1"); + + // A glove opened again is asked for again, with the product identified on it then. + fake_rh::end(RH_HAND_RIGHT, RH_ERR_DISCONNECTED, RH_E_DISCONNECTED, "TransportDisconnected: unplugged"); + source.poll(kStart); + CHECK_FALSE(source.opened(Hand::Right)); + CHECK(source.model_id(Hand::Right).empty()); + // Now the same product as the left glove: once opened on its own, both are reopened in one kit. + fake_rh::set_variant(RH_HAND_RIGHT, "P1"); + const SourceBatch batch = source.poll(kStart + 2'000 * kMs); + REQUIRE(source.opened(Hand::Right)); + CHECK(source.paired(Hand::Right)); + CHECK(source.paired(Hand::Left)); + CHECK(factory.asked() == std::vector{ "P1", "PTV1", "P1", "P1", "P1" }); + CHECK(source.model_id(Hand::Left) == "echo:P1"); + CHECK(source.model_id(Hand::Right) == "echo:P1"); + const auto metas = records_of(batch); + REQUIRE(metas.size() == 2); + CHECK(batch.hand_restarted[0]); + CHECK(batch.hand_restarted[1]); +} + +TEST_CASE("live source gives up on a glove whose product has no kinematics", "[revohuman][live]") +{ + FakeSdk sdk; + fake_rh::set_variant(RH_HAND_LEFT, "P1"); + fake_rh::set_variant(RH_HAND_RIGHT, "PTV1"); + std::string message; + bool logic_error = false; + SECTION("the plugin does not support its model") + { + logic_error = true; + message = "SDK hand model 'p' differs from the reviewed models; use a matching C SDK"; + } + SECTION("the SDK has no model for it") + { + message = "the C SDK has no hand model for product 'P1'"; + } + const LiveSourceConfig config = inline_config(); + const KinematicsFactory factory = [&](const std::string& product_code) -> std::shared_ptr + { + if (product_code == "P1") + { + if (logic_error) + { + throw std::invalid_argument(message); + } + throw std::runtime_error(message); + } + return std::make_shared("echo:" + product_code); + }; + SECTION("the other glove streams") + { + LiveSource source(config, factory); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK(source.opened(Hand::Right)); + CHECK(fake_rh::open_kits() == 1); + const auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Ended); + CHECK_THAT(note->text, ContainsSubstring("no hand kinematics for product 'P1': " + message)); + + fake_rh::push(RH_HAND_RIGHT, fake_rh::frame(RH_HAND_RIGHT, kStart)); + const auto samples = records_of(source.poll(kStart)); + REQUIRE(samples.size() == 1); + CHECK(samples[0].kinematics); + // A given-up glove is not tried again. + CHECK_FALSE(source.poll(kStart + 10'000 * kMs).finished); + // The kit of both (refused: different products), then each glove's own. + CHECK(fake_rh::calls().kit_open == 3); + CHECK(fake_rh::calls().kit_close == 1); + } + SECTION("both gloves of such a product: nothing can ever stream") + { + fake_rh::set_variant(RH_HAND_RIGHT, "P1"); + CHECK_THROWS_WITH(LiveSource(config, factory), ContainsSubstring("cannot open either glove")); + CHECK(fake_rh::open_kits() == 0); + } +} + +TEST_CASE("live source notes a closed capture gate, checked every 100 ms", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSource source(left_only()); + CHECK_FALSE(source.glove_note(Hand::Left)); + + fake_rh::set_gate(RH_HAND_LEFT, false, RH_CAUSE_CLOCK_DEGRADED); + source.poll(kStart); + auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::NotReady); + CHECK_THAT(note->text, ContainsSubstring("clock_degraded")); + + fake_rh::set_gate(RH_HAND_LEFT, true, RH_CAUSE_NONE); + source.poll(kStart + 50 * kMs); + CHECK(source.glove_note(Hand::Left)); + source.poll(kStart + 100 * kMs); + CHECK_FALSE(source.glove_note(Hand::Left)); +} + +TEST_CASE("live source counts frames the SDK dropped", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSourceConfig config = left_only(); + config.queue_size = 4; + LiveSource source(config); + for (int i = 0; i < 6; ++i) + { + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + i * 5 * kMs)); + } + const auto samples = records_of(source.poll(kStart + 30 * kMs)); + REQUIRE(samples.size() == 4); + CHECK(samples[0].sample_mono_ns == kStart + 10 * kMs); + CHECK(source.stats(Hand::Left).sdk_dropped == 2); +} + +TEST_CASE("live source looks again for a glove streaming on its own when its subscription ends", "[revohuman][live]") +{ + FakeSdk sdk; + // Different products, so each glove has a kit of its own. + fake_rh::set_variant(RH_HAND_RIGHT, "PTV1"); + { + LiveSource source(inline_config()); + REQUIRE_FALSE(source.paired(Hand::Left)); + source.poll(kStart); + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 5 * kMs)); + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 10 * kMs)); + fake_rh::end(RH_HAND_LEFT, RH_ERR_DISCONNECTED, RH_E_DISCONNECTED, "TransportDisconnected: unplugged"); + fake_rh::set_present(RH_HAND_LEFT, false); + + // Frames queued before the end are still delivered; then that glove's kit closes. + SourceBatch batch = source.poll(kStart + 10 * kMs); + CHECK(records_of(batch).size() == 2); + CHECK_FALSE(batch.finished); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK(source.stats(Hand::Left).ends == 1); + CHECK(fake_rh::calls().sub_close == 1); + CHECK(fake_rh::calls().kit_close == 1); + CHECK(fake_rh::open_kits() == 1); + auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK_THAT(note->text, ContainsSubstring("E1101 TransportDisconnected (the glove's USB link dropped)")); + + // The other glove goes on streaming. + CHECK_FALSE(source.glove_note(Hand::Right)); + fake_rh::push(RH_HAND_RIGHT, fake_rh::frame(RH_HAND_RIGHT, kStart + 15 * kMs)); + batch = source.poll(kStart + 15 * kMs); + const auto right = records_of(batch); + REQUIRE(right.size() == 1); + CHECK(right[0].hand == Hand::Right); + + // Looked for every 2 s after the end. + source.poll(kStart + 2'009 * kMs); + CHECK(fake_rh::calls().kit_open == 3); + source.poll(kStart + 2'010 * kMs); + CHECK(fake_rh::calls().kit_open == 4); + note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK_THAT(note->text, ContainsSubstring("RH_ERR_NOT_FOUND: E1000 RevoHumanError")); + + // Plugged in again: it starts over with new metadata. + fake_rh::set_glove_ids(RH_HAND_LEFT, "FAKE-SN-LEFT-AGAIN", "fake-unit-left-again"); + fake_rh::set_present(RH_HAND_LEFT, true); + batch = source.poll(kStart + 4'010 * kMs); + CHECK(fake_rh::calls().kit_open == 5); + CHECK(source.opened(Hand::Left)); + CHECK_FALSE(source.glove_note(Hand::Left)); + const auto metas = records_of(batch); + REQUIRE(metas.size() == 1); + CHECK(metas[0].hand == Hand::Left); + CHECK(metas[0].device_id == "FAKE-SN-LEFT-AGAIN"); + CHECK(batch.hand_restarted[0]); + CHECK_FALSE(batch.hand_restarted[1]); + CHECK(source.stats(Hand::Left).opens == 2); + + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 4'015 * kMs)); + batch = source.poll(kStart + 4'015 * kMs); + CHECK(records_of(batch).size() == 1); + CHECK_FALSE(batch.hand_restarted[0]); + CHECK(source.stats(Hand::Left).frames == 3); + } + // Ended subscriptions are not closed twice. + CHECK(fake_rh::calls().sub_close == 3); + CHECK(fake_rh::calls().sub_close_after_kit_close == 0); + CHECK(fake_rh::calls().kit_close == 3); + CHECK(fake_rh::open_kits() == 0); +} + +TEST_CASE("live source closes both paired gloves when one's subscription ends", "[revohuman][live]") +{ + FakeSdk sdk; + { + LiveSource source(inline_config()); + REQUIRE(source.paired(Hand::Left)); + source.poll(kStart); + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 5 * kMs)); + fake_rh::push(RH_HAND_RIGHT, fake_rh::frame(RH_HAND_RIGHT, kStart + 5 * kMs)); + fake_rh::end(RH_HAND_LEFT, RH_ERR_DISCONNECTED, RH_E_DISCONNECTED, "TransportDisconnected: link reset"); + + // The left glove's queued frames are delivered; the SDK cannot take it out of the kit alone, + // so the right glove closes with it, and then the kit. + SourceBatch batch = source.poll(kStart + 10 * kMs); + const auto samples = records_of(batch); + REQUIRE(samples.size() == 1); + CHECK(samples[0].hand == Hand::Left); + CHECK_FALSE(batch.finished); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK_FALSE(source.opened(Hand::Right)); + CHECK(source.stats(Hand::Left).ends == 1); + CHECK(source.stats(Hand::Right).ends == 1); + CHECK(fake_rh::calls().sub_close == 2); + CHECK(fake_rh::calls().sub_close_after_kit_close == 0); + CHECK(fake_rh::calls().kit_close == 1); + CHECK(fake_rh::open_kits() == 0); + auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK_THAT(note->text, ContainsSubstring("E1101 TransportDisconnected (the glove's USB link dropped)")); + note = source.glove_note(Hand::Right); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK_THAT(note->text, ContainsSubstring("the paired left glove's stream ended: E1101")); + + // Both are looked for again together 2 s later, and both start over. + source.poll(kStart + 2'009 * kMs); + CHECK(fake_rh::calls().kit_open == 1); + batch = source.poll(kStart + 2'010 * kMs); + CHECK(fake_rh::calls().open_hands == std::vector{ RH_HANDS_BOTH, RH_HANDS_BOTH }); + CHECK(source.paired(Hand::Left)); + CHECK(source.paired(Hand::Right)); + CHECK(records_of(batch).size() == 2); + CHECK(batch.hand_restarted[0]); + CHECK(batch.hand_restarted[1]); + CHECK(source.stats(Hand::Left).opens == 2); + CHECK(source.stats(Hand::Right).opens == 2); + } + CHECK(fake_rh::calls().sub_close == 4); + CHECK(fake_rh::calls().sub_close_after_kit_close == 0); + CHECK(fake_rh::calls().kit_close == 2); + CHECK(fake_rh::open_kits() == 0); +} + +TEST_CASE("live source reopens a glove plugged in while the other streams in one kit with it", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSource source(left_only()); + source.poll(kStart); + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 5 * kMs)); + REQUIRE(records_of(source.poll(kStart + 5 * kMs)).size() == 1); + + // The right glove opens on its own first, so its product is known before the left glove + // restarts; then both open in one kit, and only the kit's HandMeta of the right glove is + // delivered. + fake_rh::set_present(RH_HAND_RIGHT, true); + SourceBatch batch = source.poll(kStart + 2'000 * kMs); + CHECK(fake_rh::calls().open_hands == + std::vector{ RH_HANDS_LEFT, RH_HANDS_RIGHT, RH_HANDS_RIGHT, RH_HANDS_BOTH }); + CHECK(fake_rh::calls().kit_close == 2); + CHECK(fake_rh::open_kits() == 1); + CHECK(fake_rh::open_pair_kits() == 1); + CHECK(source.paired(Hand::Left)); + CHECK(source.paired(Hand::Right)); + const auto metas = records_of(batch); + REQUIRE(metas.size() == 2); + CHECK(batch.hand_restarted[0]); + CHECK_FALSE(batch.hand_restarted[1]); + + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 2'005 * kMs)); + fake_rh::push(RH_HAND_RIGHT, fake_rh::frame(RH_HAND_RIGHT, kStart + 2'005 * kMs)); + batch = source.poll(kStart + 2'005 * kMs); + CHECK(records_of(batch).size() == 2); + CHECK(records_of(batch).empty()); + CHECK(source.poll(kStart + 10'000 * kMs).records.empty()); + CHECK(fake_rh::calls().kit_open == 4); +} + +TEST_CASE("live source does not reopen two gloves in one kit again until either stream ends", "[revohuman][live]") +{ + FakeSdk sdk; + LiveSource source(left_only()); + source.poll(kStart); + + fake_rh::set_present(RH_HAND_RIGHT, true); + fake_rh::fail_next_pair_open( + RH_ERR_PROTOCOL, RH_E_TIME_SYNC_UNSUPPORTED, "TimeSyncUnsupported: the left glove cannot join a sync session"); + SourceBatch batch = source.poll(kStart + 2'000 * kMs); + // Each glove streams on its own again, without the right glove restarting. + CHECK(fake_rh::calls().open_hands == std::vector{ RH_HANDS_LEFT, RH_HANDS_RIGHT, RH_HANDS_RIGHT, + RH_HANDS_BOTH, RH_HANDS_LEFT, RH_HANDS_RIGHT }); + CHECK(source.opened(Hand::Left)); + CHECK(source.opened(Hand::Right)); + CHECK_FALSE(source.paired(Hand::Left)); + CHECK_FALSE(source.paired(Hand::Right)); + CHECK_FALSE(source.glove_note(Hand::Left)); + CHECK_FALSE(source.glove_note(Hand::Right)); + CHECK(records_of(batch).size() == 2); + CHECK(batch.hand_restarted[0]); + CHECK_FALSE(batch.hand_restarted[1]); + + // No loop of reopening them together. + source.poll(kStart + 4'000 * kMs); + source.poll(kStart + 10'000 * kMs); + CHECK(fake_rh::calls().kit_open == 6); + CHECK(fake_rh::open_kits() == 2); + + // Once a glove is plugged in again, they are tried together once more. + fake_rh::end(RH_HAND_RIGHT, RH_ERR_DISCONNECTED, RH_E_DISCONNECTED, "TransportDisconnected: unplugged"); + source.poll(kStart + 10'005 * kMs); + CHECK_FALSE(source.opened(Hand::Right)); + CHECK(source.opened(Hand::Left)); + source.poll(kStart + 12'005 * kMs); + CHECK(fake_rh::calls().kit_open == 8); + CHECK(source.paired(Hand::Left)); + CHECK(source.paired(Hand::Right)); +} + +TEST_CASE("live source opens the gloves on their own when it cannot scan for them", "[revohuman][live]") +{ + FakeSdk sdk; + fake_rh::fail_next_scan(RH_ERR_IO, RH_E_REVOHUMAN_ERROR, "RevoHumanError: cannot read /sys/bus/usb"); + LiveSource source(inline_config()); + CHECK(fake_rh::calls().open_hands == std::vector{ RH_HANDS_LEFT, RH_HANDS_RIGHT }); + CHECK(fake_rh::open_kits() == 2); + + // Both stream on their own, so the first poll reopens them in one kit before either delivers + // anything. + const SourceBatch batch = source.poll(kStart); + CHECK(fake_rh::open_pair_kits() == 1); + CHECK(fake_rh::open_kits() == 1); + CHECK(source.paired(Hand::Left)); + CHECK(records_of(batch).size() == 2); + CHECK_FALSE(batch.hand_restarted[0]); + CHECK_FALSE(batch.hand_restarted[1]); +} + +TEST_CASE("live source closes its subscriptions before the kits", "[revohuman][live]") +{ + FakeSdk sdk; + int kits = 0; + SECTION("one kit for both gloves") + { + kits = 1; + } + SECTION("a kit for each glove") + { + fake_rh::set_variant(RH_HAND_RIGHT, "PTV1"); + kits = 2; + } + { + LiveSource source(inline_config()); + source.poll(kStart); + REQUIRE(fake_rh::open_kits() == kits); + } + CHECK(fake_rh::calls().sub_close == 2); + CHECK(fake_rh::calls().sub_close_after_kit_close == 0); + CHECK(fake_rh::calls().kit_close == kits); +} + +TEST_CASE("live source starts without gloves and opens them once they can be", "[revohuman][open]") +{ + FakeSdk sdk; + fake_rh::fail_next_open( + RH_HAND_LEFT, RH_ERR_BUSY, RH_E_PORT_BUSY, "PortBusyError: /dev/ttyACM0 is held by pid 42", 2); + LiveSource source(left_only()); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK(fake_rh::calls().kit_open == 2); + auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK_THAT(note->text, ContainsSubstring("RH_ERR_BUSY: E1102 PortBusyError (another process holds the glove's serial " + "port)")); + + // The first poll starts the retry clock; attempts follow every 2 s, for each hand. + SourceBatch batch = source.poll(kStart); + CHECK(batch.records.empty()); + CHECK_FALSE(batch.finished); + CHECK(fake_rh::calls().kit_open == 2); + CHECK(source.poll(kStart + 1'999 * kMs).records.empty()); + CHECK(fake_rh::calls().kit_open == 2); + CHECK(source.poll(kStart + 2'000 * kMs).records.empty()); + CHECK(fake_rh::calls().kit_open == 4); + CHECK_FALSE(source.opened(Hand::Left)); + + batch = source.poll(kStart + 4'000 * kMs); + CHECK(fake_rh::calls().kit_open == 6); + CHECK(source.opened(Hand::Left)); + CHECK_FALSE(source.opened(Hand::Right)); + CHECK_FALSE(source.glove_note(Hand::Left)); + CHECK(records_of(batch).size() == 1); + CHECK(records_of(batch).empty()); + CHECK_FALSE(batch.hand_restarted[0]); + + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 4'005 * kMs)); + batch = source.poll(kStart + 4'010 * kMs); + CHECK(records_of(batch).empty()); + CHECK(records_of(batch).size() == 1); + CHECK(fake_rh::calls().kit_open == 6); +} + +TEST_CASE("live source fails fast when retrying cannot help either glove", "[revohuman][open]") +{ + FakeSdk sdk; + SECTION("bad options") + { + fake_rh::fail_next_open(RH_ERR_INVALID_ARG, RH_E_REVOHUMAN_ERROR, "RevoHumanError: rate_hz must be positive", 3); + CHECK_THROWS_WITH( + LiveSource(inline_config()), ContainsSubstring("cannot open either glove: left: RH_ERR_INVALID_ARG: E1000 " + "RevoHumanError; right: RH_ERR_INVALID_ARG: E1000")); + } + SECTION("a product conflict") + { + fake_rh::fail_next_open(RH_ERR_CONFLICT, RH_E_PRODUCT_MISMATCH, "ProductMismatch: declared P1, found PTV1", 3); + CHECK_THROWS_WITH(LiveSource(inline_config()), ContainsSubstring("E1804 ProductMismatch")); + } + SECTION("incompatible firmware") + { + fake_rh::fail_next_open(RH_ERR_PROTOCOL, RH_E_FIRMWARE_INCOMPATIBLE, "FirmwareIncompatible: too old", 3); + CHECK_THROWS_WITH(LiveSource(inline_config()), ContainsSubstring("E1205 FirmwareIncompatible")); + } + // Both together, then each on its own. + CHECK(fake_rh::calls().open_hands == std::vector{ RH_HANDS_BOTH, RH_HANDS_LEFT, RH_HANDS_RIGHT }); + CHECK(fake_rh::calls().kit_close == 0); + // The SDK is shut down although the constructor threw. + CHECK(fake_rh::calls().shutdown == 1); + CHECK_FALSE(fake_rh::calls().log_callback_set); +} + +TEST_CASE("live source keeps retrying a glove it cannot assign to a hand, and says why", "[revohuman][open]") +{ + FakeSdk sdk; + // The SDK reports it with the status of an absent glove; its own text stays out of the note. + const std::string sdk_text = "\xe6\x97\xa0\xe6\xb3\x95\xe7\xa1\xae\xe5\xae\x9a\xe6\x89\x8b\xe5\x88\xab"; + fake_rh::fail_next_open( + RH_HAND_LEFT, RH_ERR_NOT_FOUND, RH_E_UNREGISTERED_GLOVE, "UnregisteredGloveError: " + sdk_text, 2); + LiveSource source(left_only()); + CHECK_FALSE(source.opened(Hand::Left)); + auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK(note->text == + "waiting for the glove: RH_ERR_NOT_FOUND: E1400 UnregisteredGloveError (a glove is plugged in, " + "but its MCU serial number does not say which hand it is)"); + + source.poll(kStart); + source.poll(kStart + 2'000 * kMs); + CHECK_FALSE(source.opened(Hand::Left)); + source.poll(kStart + 4'000 * kMs); + CHECK(source.opened(Hand::Left)); + CHECK_FALSE(source.glove_note(Hand::Left)); +} + +TEST_CASE("live source retries a glove only when opening it may succeed later", "[revohuman][open]") +{ + struct Case + { + RhStatus status; + RhErrorCode code; + bool retried; + }; + const Case cases[] = { + { RH_ERR_NOT_FOUND, RH_E_REVOHUMAN_ERROR, true }, + { RH_ERR_TIMEOUT, RH_E_TRANSPORT_ERROR, true }, + { RH_ERR_DISCONNECTED, RH_E_DISCONNECTED, true }, + { RH_ERR_BUSY, RH_E_PORT_BUSY, true }, + { RH_ERR_STATE, RH_E_REVOHUMAN_ERROR, true }, + { RH_ERR_IO, RH_E_TRANSPORT_ERROR, true }, + { RH_ERR_PROTOCOL, RH_E_PROTOCOL_ERROR, true }, + { RH_ERR_PROTOCOL, RH_E_FIRMWARE_INCOMPATIBLE, false }, + { RH_ERR_PROTOCOL, RH_E_TIME_SYNC_INCOMPATIBLE, false }, + { RH_ERR_PROTOCOL, RH_E_TIME_SYNC_UNSUPPORTED, false }, + { RH_ERR_PROTOCOL, RH_E_UNSUPPORTED_FEATURE, false }, + { RH_ERR_CONFLICT, RH_E_REVOHUMAN_ERROR, true }, + { RH_ERR_CONFLICT, RH_E_PRODUCT_MISMATCH, false }, + { RH_ERR_CONFLICT, RH_E_VARIANT_MISMATCH, false }, + { RH_ERR_CONFLICT, RH_E_STREAM_RATE_CONFLICT, false }, + { RH_ERR_INVALID_ARG, RH_E_REVOHUMAN_ERROR, false }, + { RH_ERR_UNSUPPORTED, RH_E_UNSUPPORTED_FEATURE, false }, + { RH_ERR_INVALID_DATA, RH_E_PROTOCOL_ERROR, false }, + { RH_ERR_INTERNAL, RH_E_REVOHUMAN_ERROR, false }, + }; + for (const Case& c : cases) + { + CAPTURE(c.status, c.code); + FakeSdk sdk; + fake_rh::fail_next_open(RH_HAND_LEFT, c.status, c.code, "RevoHumanError: scripted"); + // The right glove is absent, so the source starts even when the left one is given up. + LiveSource source(left_only()); + CHECK_FALSE(source.opened(Hand::Left)); + auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == (c.retried ? Source::GloveNote::Kind::Waiting : Source::GloveNote::Kind::Ended)); + // Only the error class reaches the note; the SDK's own text goes to the debug log. + CHECK_THAT(note->text, ContainsSubstring("E" + std::to_string(c.code) + " RevoHumanError")); + + source.poll(kStart); + source.poll(kStart + 2'000 * kMs); + CHECK(source.opened(Hand::Left) == c.retried); + CHECK(fake_rh::open_kits() == (c.retried ? 1 : 0)); + } +} + +TEST_CASE("live source gives up on a glove whose joint names it cannot read", "[revohuman][open]") +{ + FakeSdk sdk; + SECTION("rh_joint_name fails") + { + fake_rh::set_joint_name(RH_HAND_LEFT, 7, std::nullopt); + LiveSource source(left_only()); + CHECK_FALSE(source.opened(Hand::Left)); + const auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Ended); + CHECK_THAT(note->text, ContainsSubstring("rh_joint_name 7 failed: E1000 RevoHumanError")); + } + SECTION("a name longer than the buffer") + { + fake_rh::set_joint_name(RH_HAND_LEFT, 3, std::string(200, 'j')); + LiveSource source(left_only()); + CHECK_FALSE(source.opened(Hand::Left)); + const auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Ended); + CHECK_THAT(note->text, ContainsSubstring("rh_joint_name 3 is longer than")); + } + // The kit that was opened for it is closed again, and it is not tried again. + CHECK(fake_rh::open_kits() == 0); + CHECK(fake_rh::calls().kit_close == 1); +} + +TEST_CASE("live source shuts the SDK down after closing every handle", "[revohuman][live]") +{ + FakeSdk sdk; + { + LiveSource source(inline_config()); + source.poll(kStart); + CHECK(fake_rh::calls().shutdown == 0); + } + const fake_rh::Calls& calls = fake_rh::calls(); + CHECK(calls.shutdown == 1); + CHECK(calls.kits_open_at_shutdown == 0); + // Removed after rh_shutdown, which may still log about handles it closes. + CHECK(calls.log_callback_set_at_shutdown); + CHECK_FALSE(calls.log_callback_set); +} + +TEST_CASE("live source leaves the SDK running while another live source uses it", "[revohuman][live]") +{ + FakeSdk sdk; + { + LiveSource first(left_only()); + { + // The glove is in the first source's kit, so this one keeps waiting for it. + LiveSource second(inline_config()); + CHECK_FALSE(second.opened(Hand::Left)); + CHECK(fake_rh::calls().init == 1); + } + CHECK(fake_rh::calls().shutdown == 0); + CHECK(fake_rh::calls().log_callback_set); + CHECK(first.opened(Hand::Left)); + CHECK(fake_rh::open_kits() == 1); + } + CHECK(fake_rh::calls().shutdown == 1); + CHECK(fake_rh::calls().kits_open_at_shutdown == 0); +} + +TEST_CASE("live source gives up on one glove and streams the other", "[revohuman][open]") +{ + FakeSdk sdk; + // The kit of both fails on the left glove, and so does the left glove's own. + fake_rh::fail_next_open( + RH_HAND_LEFT, RH_ERR_PROTOCOL, RH_E_FIRMWARE_INCOMPATIBLE, "FirmwareIncompatible: too old", 2); + LiveSource source(inline_config()); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK(source.opened(Hand::Right)); + const auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Ended); + CHECK_THAT(note->text, ContainsSubstring("E1205 FirmwareIncompatible")); + + fake_rh::push(RH_HAND_RIGHT, fake_rh::frame(RH_HAND_RIGHT, kStart)); + SourceBatch batch = source.poll(kStart); + CHECK(records_of(batch).size() == 1); + batch = source.poll(kStart + 10'000 * kMs); + CHECK_FALSE(batch.finished); + // A given-up glove is not tried again. + CHECK(fake_rh::calls().kit_open == 3); +} + +TEST_CASE("live source retries a re-enumerated port but not a product conflict", "[revohuman][open]") +{ + FakeSdk sdk; + fake_rh::fail_next_open(RH_ERR_CONFLICT, RH_E_REVOHUMAN_ERROR, "RevoHumanError: port re-enumerated, retry", 3); + LiveSource source(inline_config()); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK_FALSE(source.opened(Hand::Right)); + + // Both are looked for together again. + source.poll(kStart); + fake_rh::fail_next_open(RH_ERR_CONFLICT, RH_E_VARIANT_MISMATCH, "VariantMismatch: declared PTV1, found P1", 3); + SourceBatch batch = source.poll(kStart + 2'000 * kMs); + CHECK(fake_rh::calls().open_hands == std::vector{ RH_HANDS_BOTH, RH_HANDS_LEFT, RH_HANDS_RIGHT, + RH_HANDS_BOTH, RH_HANDS_LEFT, RH_HANDS_RIGHT }); + CHECK(batch.finished); + for (const Hand hand : { Hand::Left, Hand::Right }) + { + const auto note = source.glove_note(hand); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Ended); + CHECK_THAT(note->text, ContainsSubstring("E1802 VariantMismatch")); + } + // Giving up is final. + CHECK(source.poll(kStart + 10'000 * kMs).finished); + CHECK(fake_rh::calls().kit_open == 6); +} + +TEST_CASE("live source looks again for a glove that went away while it was subscribing", "[revohuman][open]") +{ + FakeSdk sdk; + SECTION("unplugged after the kit opened") + { + fake_rh::fail_next_subscribe( + RH_HAND_LEFT, RH_ERR_DISCONNECTED, RH_E_DISCONNECTED, "TransportDisconnected: unplugged"); + LiveSource source(left_only()); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK(fake_rh::open_kits() == 0); + const auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Waiting); + CHECK_THAT(note->text, ContainsSubstring("rh_glove_subscribe failed")); + + source.poll(kStart); + const SourceBatch batch = source.poll(kStart + 2'000 * kMs); + CHECK(source.opened(Hand::Left)); + CHECK(records_of(batch).size() == 1); + CHECK(fake_rh::calls().subscribe == 2); + } + SECTION("but not when the SDK refuses the subscription") + { + // In the kit of both, then in its own. + fake_rh::fail_next_subscribe( + RH_HAND_LEFT, RH_ERR_INVALID_ARG, RH_E_REVOHUMAN_ERROR, "RevoHumanError: queue_size", 2); + LiveSource source(inline_config()); + CHECK_FALSE(source.opened(Hand::Left)); + CHECK(source.opened(Hand::Right)); + const auto note = source.glove_note(Hand::Left); + REQUIRE(note); + CHECK(note->kind == Source::GloveNote::Kind::Ended); + CHECK(fake_rh::open_kits() == 1); + } +} + +TEST_CASE("live source opens a glove in the background without holding up the other", "[revohuman][open]") +{ + FakeSdk sdk; + fake_rh::set_present(RH_HAND_RIGHT, false); + fake_rh::set_variant(RH_HAND_RIGHT, "PTV1"); + const EchoFactory factory; + LiveSource source(LiveSourceConfig{}, factory.make()); + REQUIRE(source.opened(Hand::Left)); + source.poll(kStart); + + // The right glove is plugged in and takes 300 ms to open. + fake_rh::set_open_delay_ms(RH_HAND_RIGHT, 300); + fake_rh::set_present(RH_HAND_RIGHT, true); + auto t0 = WallClock::now(); + source.poll(kStart + 2'000 * kMs); + CHECK(WallClock::now() - t0 < std::chrono::milliseconds(100)); + CHECK_FALSE(source.opened(Hand::Right)); + + // The left glove's samples go on meanwhile. + fake_rh::push(RH_HAND_LEFT, fake_rh::frame(RH_HAND_LEFT, kStart + 2'005 * kMs)); + t0 = WallClock::now(); + const auto left = records_of(source.poll(kStart + 2'005 * kMs)); + CHECK(WallClock::now() - t0 < std::chrono::milliseconds(100)); + CHECK(left.size() == 1); + + std::vector batches; + REQUIRE(poll_until(source, kStart + 2'010 * kMs, batches, [&] { return source.opened(Hand::Right); })); + const auto metas = records_of(batches.back()); + REQUIRE(metas.size() == 1); + CHECK(metas[0].hand == Hand::Right); + CHECK_FALSE(batches.back().hand_restarted[1]); + CHECK(fake_rh::snapshot().subscribe == 2); + // Its kinematics were made on the worker, for the product identified there. + CHECK(factory.asked() == std::vector{ "P1", "PTV1" }); + CHECK(source.model_id(Hand::Right) == "echo:PTV1"); + + // Its kit is closed in the background too once its stream ends. + fake_rh::end(RH_HAND_RIGHT, RH_ERR_DISCONNECTED, RH_E_DISCONNECTED, "TransportDisconnected: unplugged"); + fake_rh::set_present(RH_HAND_RIGHT, false); + batches.clear(); + REQUIRE(poll_until(source, kStart + 3'000 * kMs, batches, [] { return fake_rh::open_kits() == 1; })); + CHECK_FALSE(source.opened(Hand::Right)); + CHECK(source.opened(Hand::Left)); +} + +TEST_CASE("live source closes a glove that finished opening after it was destroyed", "[revohuman][open]") +{ + FakeSdk sdk; + fake_rh::set_present(RH_HAND_RIGHT, false); + { + LiveSource source(LiveSourceConfig{}); + source.poll(kStart); + fake_rh::set_open_delay_ms(RH_HAND_RIGHT, 200); + fake_rh::set_present(RH_HAND_RIGHT, true); + const int started = fake_rh::opens_started(); + source.poll(kStart + 2'000 * kMs); + // Wait until the worker has started the open, which then outlives the source. + const auto deadline = WallClock::now() + std::chrono::seconds(5); + while (fake_rh::opens_started() == started && WallClock::now() < deadline) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + REQUIRE(fake_rh::opens_started() > started); + } + const fake_rh::Calls calls = fake_rh::snapshot(); + CHECK(fake_rh::open_kits() == 0); + CHECK(calls.kit_open == 3); + CHECK(calls.kit_close == 2); + CHECK(calls.sub_close == 2); + CHECK(calls.sub_close_after_kit_close == 0); +} + +TEST_CASE("rh_error_text falls back to the error code without a message", "[revohuman][live]") +{ + FakeSdk sdk; + CHECK(rh_error_text() == "E0"); +} diff --git a/tests/cpp/plugins/revohuman/live/test_rh_frame.cpp b/tests/cpp/plugins/revohuman/live/test_rh_frame.cpp new file mode 100644 index 0000000000..d5e6ef42d7 --- /dev/null +++ b/tests/cpp/plugins/revohuman/live/test_rh_frame.cpp @@ -0,0 +1,107 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include + +using namespace plugins::revohuman; + +namespace +{ + +RhGloveFrame blank_frame() +{ + RhGloveFrame f; + std::memset(&f, 0, sizeof(f)); + f.struct_size = sizeof(RhGloveFrame); + f.hand = RH_HAND_RIGHT; + return f; +} + +std::array radians() +{ + std::array rad{}; + for (std::size_t i = 0; i < kGloveJointCount; ++i) + { + rad[i] = 0.05 * static_cast(i); + } + return rad; +} + +} // namespace + +TEST_CASE("SDK frame without tactile converts its header fields", "[revohuman][rh_frame]") +{ + RhGloveFrame f = blank_frame(); + f.generation = 4; + f.device_tick = 0xFFFFFFF0u; + f.joints_valid_mask = 0x1FFFFFu; + f.flags = RH_FRAME_FLAG_GATE_OPEN | RH_FRAME_FLAG_HAS_SAMPLE_TIME; + f.sample_mono_ns = 987'654'321; + + const GloveSample g = from_rh_frame(f, radians()); + REQUIRE(g.hand == Hand::Right); + REQUIRE(g.generation == 4); + REQUIRE(g.device_tick == 0xFFFFFFF0u); + REQUIRE(g.joints_valid_mask == 0x1FFFFFu); + REQUIRE(g.has_sample_time); + REQUIRE(g.sample_mono_ns == 987'654'321); + REQUIRE(g.joints_rad == radians()); + REQUIRE_FALSE(g.tactile); + REQUIRE_FALSE(g.kinematics); +} + +TEST_CASE("SDK frame tactile block is copied with its flags", "[revohuman][rh_frame]") +{ + RhGloveFrame f = blank_frame(); + f.hand = RH_HAND_LEFT; + f.flags = RH_FRAME_FLAG_HAS_TACTILE; + f.tactile.point_count = RH_TACTILE_POINTS_TYPE_B; + f.tactile.valid = 1; + f.tactile.calibrated = 1; + f.tactile.has_sample_time = 1; + f.tactile.device_tick = 55; + f.tactile.sample_mono_ns = 111; + for (int i = 0; i < RH_TACTILE_POINTS_TYPE_B; ++i) + { + f.tactile.values[i] = static_cast(1000 + i); + } + + const GloveSample g = from_rh_frame(f, radians()); + REQUIRE(g.hand == Hand::Left); + REQUIRE_FALSE(g.has_sample_time); + REQUIRE(g.tactile); + REQUIRE(g.tactile->values.size() == RH_TACTILE_POINTS_TYPE_B); + REQUIRE(g.tactile->values.front() == 1000); + REQUIRE(g.tactile->values.back() == 1000 + RH_TACTILE_POINTS_TYPE_B - 1); + REQUIRE(g.tactile->valid); + REQUIRE(g.tactile->calibrated); + REQUIRE(g.tactile->has_sample_time); + REQUIRE(g.tactile->device_tick == 55); + REQUIRE(g.tactile->sample_mono_ns == 111); + + f.tactile.calibrated = 0; + REQUIRE_FALSE(from_rh_frame(f, radians()).tactile->calibrated); +} + +TEST_CASE("SDK frame conversion rejects what it cannot trust", "[revohuman][rh_frame]") +{ + RhGloveFrame f = blank_frame(); + f.struct_size = sizeof(RhGloveFrame) - 8; + REQUIRE_THROWS_AS(from_rh_frame(f, radians()), std::invalid_argument); + + f = blank_frame(); + f.hand = RH_HAND_UNKNOWN; + REQUIRE_THROWS_AS(from_rh_frame(f, radians()), std::invalid_argument); + + f = blank_frame(); + f.flags = RH_FRAME_FLAG_HAS_TACTILE; + f.tactile.point_count = RH_TACTILE_MAX_POINTS + 1; + REQUIRE_THROWS_AS(from_rh_frame(f, radians()), std::invalid_argument); + + f.flags = 0; + REQUIRE_NOTHROW(from_rh_frame(f, radians())); +} diff --git a/tests/python/core/teleop_session_manager/test_teleop_session.py b/tests/python/core/teleop_session_manager/test_teleop_session.py index 1113e0866a..06beefd356 100644 --- a/tests/python/core/teleop_session_manager/test_teleop_session.py +++ b/tests/python/core/teleop_session_manager/test_teleop_session.py @@ -1609,7 +1609,7 @@ def __new__(cls, value, *, aim_valid=True, grip_valid=True): == teleop_session_manager.DeviceState.CONNECTED ) - @pytest.mark.parametrize("plugin", ["spacemouse", "gamepad"]) + @pytest.mark.parametrize("plugin", ["spacemouse", "gamepad", "revohuman"]) def test_device_plugin_is_monitored(self, tmp_path, plugin): manager = MockPluginManager(plugin_names=[plugin]) manager.get_plugin_info = MagicMock( From 24182bc11460f7a8fe9e3807b60c6538261cd7f6 Mon Sep 17 00:00:00 2001 From: HAOTianGa03 <2022280352@email.szu.edu.cn> Date: Tue, 3 Nov 2026 01:04:25 -0500 Subject: [PATCH 2/3] docs(revohuman): build the plugin tests before running ctest install.sh builds only revohuman_plugin, so running ctest right after it found no test executables. Say so in the README and add the command that builds the three test targets. Co-Authored-By: Claude Opus 5.5 Signed-off-by: HAOTianGa03 <2022280352@email.szu.edu.cn> --- src/plugins/revohuman/README.md | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/plugins/revohuman/README.md b/src/plugins/revohuman/README.md index 2f16cba70f..f8988c60a6 100644 --- a/src/plugins/revohuman/README.md +++ b/src/plugins/revohuman/README.md @@ -209,9 +209,10 @@ Verify the alignment before using the output for control. ## Tests -With `BUILD_TESTING=ON`, the tests build together with the plugin, so without -the C SDK they are skipped with it. Each library has its own suite under -`tests/cpp/plugins/revohuman/`: +With `BUILD_TESTING=ON`, the test targets are configured with the plugin, so +without the C SDK they are skipped with it. `install.sh` builds only the +plugin, so build the tests before running them. Each library has its own +suite under `tests/cpp/plugins/revohuman/`: - `core/` (`revohuman_core_tests`): the sample pipeline, tactile serialization and skinning, device status, event log and pose math. @@ -226,8 +227,13 @@ the C SDK they are skipped with it. Each library has its own suite under source variable and flag. The integration suite and the two plugin checks need the SDK library; with only -its header, the core and live suites still build. Run them all with -`ctest --test-dir build-revohuman/tests/cpp/plugins/revohuman --output-on-failure`. +its header, the core and live suites still build. After the install above, +build and run them all with: + +```bash +cmake --build build-revohuman --target revohuman_core_tests revohuman_live_tests revohuman_integration_tests +ctest --test-dir build-revohuman/tests/cpp/plugins/revohuman --output-on-failure +``` ## Tactile layout From 3256f319da2ffc08e6451dbb6e0d9f8c3ccfc41d Mon Sep 17 00:00:00 2001 From: HAOTianGa03 <2022280352@email.szu.edu.cn> Date: Sun, 8 Nov 2026 22:02:17 -0500 Subject: [PATCH 3/3] build(revohuman): pin the re-published C SDK 0.1.3 with knuckle poses C SDK 0.1.3 was published again with one new function, rh_hand_knuckles(), and its result struct. It returns the nominal MCP joint centers of the four fingers in the tracker frame. The existing API, error codes and hand models are the same as in the package pinned now, so only the two SHA-256 values change in install.sh. Co-Authored-By: Claude Opus 5.5 Signed-off-by: HAOTianGa03 <2022280352@email.szu.edu.cn> --- src/plugins/revohuman/install.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/plugins/revohuman/install.sh b/src/plugins/revohuman/install.sh index 29853dd86b..0baafa6797 100755 --- a/src/plugins/revohuman/install.sh +++ b/src/plugins/revohuman/install.sh @@ -16,8 +16,8 @@ REVOHUMAN_SDK_C_TAG="c-sdk-v0.1.3" # (every SDK version bump needs a matching commit here, same tradeoff as # wuji_glove's install.sh). declare -A REVOHUMAN_SDK_C_SHA256=( - ["x86_64"]="773dc63fd5daf62e4acceb90278e7bca493dc7fada3aad4febe45fb8934759dc" - ["aarch64"]="bdd4a9ae3c1777f4c2dccf6c2be35d3a6ccba2098e289ff1677527042cc0f642" + ["x86_64"]="aca14b639c0fda1b0c00d210a63d797e420d4162c1be7f6beb0a97e6fd22bc7a" + ["aarch64"]="74c1c8dd78c719f2e52379cd34e76e38768cae5eab039cd2124f7b98ae1db96a" ) usage() {