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..f8988c60a6 --- /dev/null +++ b/src/plugins/revohuman/README.md @@ -0,0 +1,277 @@ + + +# 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 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. +- `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. 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 + +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..0baafa6797 --- /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"]="aca14b639c0fda1b0c00d210a63d797e420d4162c1be7f6beb0a97e6fd22bc7a" + ["aarch64"]="74c1c8dd78c719f2e52379cd34e76e38768cae5eab039cd2124f7b98ae1db96a" +) + +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 1b271f62aa..d5c2b64bb9 100644 --- a/src/python/isaaccapture/teleop_session_manager/teleop_session.py +++ b/src/python/isaaccapture/teleop_session_manager/teleop_session.py @@ -63,7 +63,7 @@ # Plugins that publish device status (see PluginDeviceStatusPublisher); others launch unmonitored. _MONITORED_PLUGIN_NAMES = frozenset( - {"manus_hand_plugin", "avatar_hand_plugin", "spacemouse", "gamepad"} + {"manus_hand_plugin", "avatar_hand_plugin", "spacemouse", "gamepad", "revohuman"} ) diff --git a/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index 46847194f9..1fa16aef37 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -16,3 +16,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 ec7b4d70bf..4d6b9f2384 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,9 @@ def __new__(cls, value, *, aim_valid=True, grip_valid=True): == teleop_session_manager.DeviceState.CONNECTED ) - @pytest.mark.parametrize("plugin", ["spacemouse", "gamepad", "avatar_hand_plugin"]) + @pytest.mark.parametrize( + "plugin", ["spacemouse", "gamepad", "avatar_hand_plugin", "revohuman"] + ) def test_device_plugin_is_monitored(self, tmp_path, plugin): manager = MockPluginManager(plugin_names=[plugin]) manager.get_plugin_info = MagicMock(