diff --git a/CMakeLists.txt b/CMakeLists.txt index 521ec03c30..0d8c085142 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 only; downloads the RevoHuman C SDK)" 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/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..8e196bbbdf --- /dev/null +++ b/docs/source/device/hand_tracking/revohuman.rst @@ -0,0 +1,229 @@ +.. 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 in newtons, as the SDK reports them, + 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`. +- **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 CMake configure 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 +------------ + +With ``BUILD_PLUGIN_REVOHUMAN=ON``, the CMake configure downloads the pinned C +SDK release into the build directory and verifies it against a +per-architecture SHA-256. Build and install the plugin with the SDK library and +its licence next to it: + +.. code-block:: bash + + cmake -B build -DBUILD_PLUGIN_REVOHUMAN=ON + cmake --build build --target revohuman_plugin + cmake --install build --component revohuman + +``-DREVOHUMAN_SDK_ROOT=`` uses an SDK package you already extracted +instead of the download. + +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's autokit, at most one left and one +right, with nothing to declare; the SDK opens, retries and reopens them in the +background. Gloves plugged in at start-up open together in one two-hand SDK kit, +synced on one USB frame where the SDK can; otherwise each opens on its own, and +a missing glove is looked for again at the SDK autokit's retry interval +(``retry_ms`` in ``revohuman.h``) with the SDK's reason in its status. The +SDK reconnects a glove after a short dropout and re-syncs a pair itself. When a +stream of the pair ends anyway, both gloves close and reopen as a pair once both +are back; if only one is back for the autokit's ``pair_wait_ms`` it opens on its +own, the other follows on its own, and the two stay unpaired until the plugin +restarts. Gloves streaming without hardware sync to each other (paired in host +time, unpaired after a lost pair, or opened separately) report their ``/glove`` +status as degraded with the reason. + +From start-up until a hand's first glove sample, for at most 5 s +(``kStartupHoldNs`` in the plugin's pipeline), the plugin injects that hand with +no valid joints. A client reading OpenXR hand tracking then gets no joints while the +gloves open, not the headset's optical hand followed by the glove's. + +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, with incompatible firmware or with a hand model the +plugin is not fitted to is reported as failed; the plugin keeps running. The +plugin is fitted to the models of the C SDK release that its CMake pins. + +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. 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 +12.6 mm towards the palm. 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 +-------------- + +When a glove opens, the plugin builds its hand's ``TactileMetadata`` from the C +SDK's taxel layout for the glove's 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 taxel count and whether the metadata was built; the debug log +names the SDK's layout source. When the SDK has no layout for the glove's taxel +count, the plugin warns why, pushes no tactile data and reports the hand's +tactile as failed; 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 + * - CMake configuration fails with a hash mismatch for ``revohuman_sdk`` + - The downloaded package does not match the pinned release. Reconfigure to + rule out a corrupted download; if it persists, do not bypass the check + — verify the release assets upstream. + * - CMake configuration fails naming the RevoHuman SDK + - The SDK at ``REVOHUMAN_SDK_ROOT`` is incomplete: its library, headers or + licence are missing. Point it at a complete C SDK release, or clear it + to download the pinned one, 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 reason: a product or variant the plugin does not + support, incompatible firmware, or a hand model the plugin is not fitted + to, as from a C SDK release other than the one the plugin's CMake + pins. These are not retried. + * - Hands appear but a downstream client sees no tactile data + - A P1 glove has no tactile sensors. Otherwise check the tactile status: + failed means there is no ``TactileMetadata`` (the start-up log warns + why, such as an SDK without the glove's taxel layout); otherwise it names + the reason a sample was not pushed. diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index c26d9384c2..f23cca20f1 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -44,3 +44,7 @@ endif() if(BUILD_PLUGIN_SHARPA_AVATAR) add_subdirectory(sharpa_avatar) endif() +# Gated: Linux. Downloads the pinned RevoHuman C SDK release unless REVOHUMAN_SDK_ROOT is set. +if(BUILD_PLUGIN_REVOHUMAN) + add_subdirectory(revohuman) +endif() diff --git a/src/plugins/revohuman/CMakeLists.txt b/src/plugins/revohuman/CMakeLists.txt new file mode 100644 index 0000000000..4d958335d2 --- /dev/null +++ b/src/plugins/revohuman/CMakeLists.txt @@ -0,0 +1,68 @@ +# 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, pushed via OpenXR. + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(STATUS "Skipping revohuman plugin (Linux only)") + return() +endif() + +# The pinned RevoHuman C SDK release. The hand fits in core/hand_fit.inc are for the hand models of this +# release, so the version, the hashes and the fits change together. +set(_revohuman_sdk_version "0.1.3") +set(_revohuman_sdk_sha256_x86_64 "c2ed6ead4bb105e1c0c0e637452e1c53b27ce977c21179bb3f655b8824e8e9c5") +set(_revohuman_sdk_sha256_aarch64 "5912011644542b34b62224440ace74b5626010d38a6956af545dc144d7511a08") + +set(REVOHUMAN_SDK_ROOT "" CACHE PATH + "Optional RevoHuman C SDK root (include/, lib/, LICENSE); if empty, CMake downloads the pinned release") + +if(REVOHUMAN_SDK_ROOT) + if(NOT IS_DIRECTORY "${REVOHUMAN_SDK_ROOT}") + message(FATAL_ERROR + "REVOHUMAN_SDK_ROOT does not exist: ${REVOHUMAN_SDK_ROOT}\n" + "Clear REVOHUMAN_SDK_ROOT to let CMake download the pinned release, or set it to a release package." + ) + endif() + get_filename_component(_revohuman_sdk_dir "${REVOHUMAN_SDK_ROOT}" ABSOLUTE) +else() + if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(aarch64|arm64)$") + set(_revohuman_sdk_arch "aarch64") + elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^(x86_64|AMD64|amd64)$") + set(_revohuman_sdk_arch "x86_64") + else() + message(STATUS "Skipping revohuman plugin: no RevoHuman C SDK release for ${CMAKE_SYSTEM_PROCESSOR}; " + "set REVOHUMAN_SDK_ROOT to an SDK built for it") + return() + endif() + include(FetchContent) + FetchContent_Declare(revohuman_sdk + URL "https://github.com/BrainCoTech/brainco-revohuman-sdk/releases/download/c-sdk-v${_revohuman_sdk_version}/revohuman-c-sdk-${_revohuman_sdk_version}-linux-${_revohuman_sdk_arch}.tar.gz" + URL_HASH "SHA256=${_revohuman_sdk_sha256_${_revohuman_sdk_arch}}" + ) + # The release has no CMakeLists.txt, so this only downloads, checks and extracts it. + FetchContent_MakeAvailable(revohuman_sdk) + set(_revohuman_sdk_dir "${revohuman_sdk_SOURCE_DIR}") +endif() + +# Never reuse a cached library path: the headers and the library must come from the same SDK. +unset(REVOHUMAN_SDK_LIBRARY CACHE) +find_library(REVOHUMAN_SDK_LIBRARY NAMES revohuman PATHS "${_revohuman_sdk_dir}/lib" NO_DEFAULT_PATH NO_CACHE) +if(NOT REVOHUMAN_SDK_LIBRARY OR NOT EXISTS "${_revohuman_sdk_dir}/include/revohuman.h" + OR NOT EXISTS "${_revohuman_sdk_dir}/include/revohuman_fk.h" OR NOT EXISTS "${_revohuman_sdk_dir}/LICENSE") + message(FATAL_ERROR "RevoHuman C SDK at ${_revohuman_sdk_dir} is incomplete: it needs lib/librevohuman.so, " + "include/revohuman.h, include/revohuman_fk.h and LICENSE. " + "Clear REVOHUMAN_SDK_ROOT to let CMake download the pinned release, " + "or set it to a release package.") +endif() +message(STATUS "RevoHuman C SDK: ${_revohuman_sdk_dir}") + +add_library(revohuman_plugin::sdk UNKNOWN IMPORTED GLOBAL) +set_target_properties(revohuman_plugin::sdk PROPERTIES + IMPORTED_LOCATION "${REVOHUMAN_SDK_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${_revohuman_sdk_dir}/include") + +add_subdirectory(core) +add_subdirectory(app) + +install(FILES plugin.yaml README.md DESTINATION plugins/revohuman COMPONENT revohuman) diff --git a/src/plugins/revohuman/README.md b/src/plugins/revohuman/README.md new file mode 100644 index 0000000000..21321e9f32 --- /dev/null +++ b/src/plugins/revohuman/README.md @@ -0,0 +1,170 @@ + + +# RevoHuman plugin + +The plugin reads RevoHuman gloves through the unmodified RevoHuman C SDK and +pushes, per hand, an injected OpenXR hand, the 21 encoder joint states and the +taxel forces with their TactileMetadata. The `isaaccapture` wheel contains +neither the plugin nor the SDK; start it from the CMake install tree. + +Prerequisites, running, the wrist pose and troubleshooting are on the +[RevoHuman Glove](../../../docs/source/device/hand_tracking/revohuman.rst) +device page. This README adds the build, the hand mapping and the tactile +contract. + +| Directory | Contents | +| --- | --- | +| `core/` | `HandModel` (SDK hand points to OpenXR joints, TactileMetadata; the fitted keypoint offsets are in `hand_fit.inc`) and `HandPipeline` (what each hand pushes and when); unit-tested without a runtime | +| `app/` | `GloveSource` (gloves through the SDK), the OpenXR outputs, device status and `main` | + +## Build and install + +Follow the repository's build-from-source prerequisites first. With +`BUILD_PLUGIN_REVOHUMAN=ON`, the configure downloads the C SDK release pinned in +`CMakeLists.txt` and checks its SHA-256; `-DREVOHUMAN_SDK_ROOT=` uses an +extracted package instead. A Python SDK wheel cannot replace the C headers and +library. From the repository root: + +```bash +cmake -B build -DBUILD_PLUGIN_REVOHUMAN=ON +cmake --build build --target revohuman_plugin +cmake --install build --component revohuman +``` + +`/plugins/revohuman` then holds the plugin, the SDK shared +library (found through `$ORIGIN`; keep its symlinks) and the SDK licence. An +incomplete SDK fails the configure. + +## Gloves + +The plugin finds at most one left and one right glove through the C SDK's +autokit (`rh_autokit_*`), which opens, retries and reopens them on its own +thread; `app/glove_source.cpp` turns each opened stream into hand metadata and +feeds its frames to the pipelines. Two gloves plugged in while neither is open, +as at start-up, open together in one two-hand kit, synced on one USB frame where +the SDK can (`RH_SYNC_BEST_EFFORT`). Otherwise, or when the two-hand kit cannot +open, each glove opens on its own, and a missing glove is looked for again every +`retry_ms` (the SDK default, which the plugin keeps; see `RhAutoKitOptions` in +`revohuman.h`) with the SDK reason in its `/glove` status. A glove that arrives +while the other already streams on its own also opens on its own, without +interrupting the other, and the two stay unpaired; restart the plugin with both +gloves plugged in to pair them. + +The SDK reconnects a glove through dropouts and replugs itself without ending +its stream; when it reconnects one glove of a synced pair, it re-syncs the pair, +and the other glove's samples pause briefly. A stream ends only on a fatal SDK +error. A glove on its own then reopens after `retry_ms`; for a pair, both +gloves close and reopen as a pair once both are back, and if only one is back +for `pair_wait_ms` (also an SDK default) it opens on its own, the other follows on +its own, and they stay unpaired until the plugin restarts. Gloves streaming +without hardware sync to each other (host-time pairing, unpaired after a lost +pair, or opened separately) report `/glove` DEGRADED with the reason. While the +SDK's capture gate is closed a glove gives no samples, and `/glove` reports +DEGRADED with `capture gate closed ()`, or UNKNOWN before its first +sample. + +A wrong product or incompatible firmware makes the SDK give the glove up, and a +glove whose hand model the plugin cannot map to OpenXR joints is not used; both +report `/glove` FAILED with the reason. The plugin keeps running, even with both +gloves given up. Do not run the plugin while the SDK viewer holds a glove's +port. + +Started by hand, the plugin publishes `/device_status` only with +`--plugin-root-id=`; the plugin launcher passes it. + +## Hand mapping + +Each glove's mechanical model follows its identified product through +`rh_variant_hand_model`, and `rh_hand_model_info` gives the model's URDF +SHA-256. Every sample's angles go to `rh_hand_points`, the SDK's link poses and +fingertip points. The plugin is fitted to the PTV1 and P1 models, pinned by +URDF hash in `core/hand_fit.inc` (`kFittedModelHashes`), and fails a glove of +any other model. It places the keypoints at the OpenXR joints (wrist, joint +centres inside the fingers, tips and palm) by fixed offsets in the frames of +the SDK link poses (`kFingerFits`, `kPalmPad`, `kWristPad`, one table for +both products); the palm frame is a fixed basis in `palm_link` (`kPalmBasis`). + +An SDK release that changes either pinned model's URDF moves it off the fitted +table, so refit and update `kFittedModelHashes` together with the SDK pin in +`CMakeLists.txt`. The glove's open log line names the hand model and its hash. + +`core/hand_model.cpp` re-expresses the keypoints from WRIST and orients the +finger joints from the SDK link poses; WRIST and PALM keep fixed axes in +`palm_link`. The four non-thumb metacarpals are placeholders and never valid; +the four non-thumb proximal joints ride on the MCP link like the taxels bound +to it. + +WRIST is a model point, not the anatomical wrist: `palm_link` moved along its ++X (towards the palm) by a depth `w`, with OpenXR X = -palm Y, Y = -palm X, +Z = -palm Z. `w` is `kWristPad` in `core/hand_fit.inc`. +The anatomical wrist centre is nominally about 4 cm towards the fingers. The +plugin puts this WRIST at the host's wrist pose, so with optical hand tracking +the injected hand sits that far off the tracked one; with a controller, set +`--aim-to-wrist-left/right` to end at the glove's WRIST. In the Tactile Metadata +Editor (`scripts/tactile_metadata`), a model rooted at `palm_link` needs URDF +Alignment Position `(0, 0, -w)`; the editor does not save it. + +## Tactile + +| Collection | Contract | +| --- | --- | +| `__tactile` | `TactileDataArray`: taxel-local forces in newtons along +Z (outward normal) | +| `__tactile/metadata` | bare `TactileMetadata`: LBS bound at zero angles, each taxel rigid on one OpenXR joint | + +The metadata is built when a glove opens, from the SDK taxel layout of its model +(`rh_hand_model_taxel_layout`); a glove without tactile sensors (P1) gets no +tactile collections and a disabled tactile status. A glove whose taxel count +has no SDK layout for its model gets no tactile collections either; its tactile +status is failed and the log warns why. Forces are the SDK's newtons +(`rh_tactile_newtons`), unchanged. +A sample is pushed when the firmware has not marked it invalid, it is +calibrated, has the layout's taxel count, carries a sampling time no older than +`kTactileMaxAgeNs` (100 ms, in `core/inc/revohuman_plugin/pipeline.hpp`), +and both its sampling time and its device tick advanced. A skipped sample does +not remove forces already pushed; they are removed once older than that, and +the status names the skip reason. Consumers must check the tactile status as +well as data presence. + +The plugin opens tactile split from the glove frames (`split_tactile` in +`RhGloveOptions`). Whether a glove splits it depends on its firmware and is +settled when the stream opens. A glove that splits it sends every tactile +sample on its own at about 215 Hz, above the 200 Hz frame rate, and the plugin +pushes each sample in sampling order; the frames' copies of the latest sample +are not used. A glove that does not split it sends tactile with the frames, and +the plugin takes one sample per frame once the stream's first `kSplitWaitFrames` +frames with tactile (`app/glove_source.hpp`) have arrived without a split +sample. The log names the source each glove uses. + +### Taring the tactile sensors + +Send `SIGUSR1` to the plugin (`kill -USR1 `) to tare the tactile sensors: +each streaming glove with tactile sensors gets one request and zeroes every +taxel at its current reading. This writes to the gloves, so keep both hands +still and the tactile surfaces untouched for about 3 s. The tactile stream +pauses briefly while the module tares; joint states are not affected. + +Requests are never resent, and the log reports how each one ends: sent, or +otherwise a warning with the SDK's reason (for example the glove refused it, or +its result could not be confirmed). A request that cannot be made is logged with +why: the glove is not streaming, has no tactile sensors, is still running an +earlier request or is reconnecting, or its tare support is not known yet. Sent +means the glove has passed the tare to its tactile module; the glove does not +report when the module finishes it. A glove that cannot tare on request is tared +by holding its button for about 2 s. The log also reports tares started from the +button or another host, a reconnect or glove restart after which the last tare +is no longer confirmed, and a glove that reopened before its request's result +was known. + +## Tests + +`revohuman_core_tests` is configured with the plugin while `BUILD_TESTING` is +ON, the top-level default. It links the SDK library, not a fake, and needs +neither a glove nor an OpenXR runtime: + +```bash +cmake --build build --target revohuman_core_tests +ctest --test-dir build/tests/cpp/plugins/revohuman --output-on-failure +``` diff --git a/src/plugins/revohuman/app/CMakeLists.txt b/src/plugins/revohuman/app/CMakeLists.txt new file mode 100644 index 0000000000..8718667de6 --- /dev/null +++ b/src/plugins/revohuman/app/CMakeLists.txt @@ -0,0 +1,41 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(revohuman_plugin + main.cpp + glove_source.cpp + glove_source.hpp + revohuman_plugin.cpp + revohuman_plugin.hpp +) + +target_link_libraries(revohuman_plugin PRIVATE + revohuman_plugin::revohuman_core + revohuman_plugin::sdk + Teleop::plugin_utils + pusherio::pusherio + deviceio::deviceio_session + oxr::oxr_core + oxr::oxr_utils + isaacteleop_schema + log_bridge::log_bridge_core +) + +# The SDK library is installed next to the plugin. BUILD_RPATH adds $ORIGIN to the default build-tree RPATH, +# which keeps the 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 (.so -> .so. -> .so.); install(FILES) keeps symlinks. +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) +install(FILES "${_revohuman_sdk_dir}/LICENSE" + DESTINATION plugins/revohuman + COMPONENT revohuman + RENAME LICENSE.revohuman-sdk-c +) diff --git a/src/plugins/revohuman/app/glove_source.cpp b/src/plugins/revohuman/app/glove_source.cpp new file mode 100644 index 0000000000..5dccb6462e --- /dev/null +++ b/src/plugins/revohuman/app/glove_source.cpp @@ -0,0 +1,503 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "glove_source.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +constexpr std::int64_t kDropLogNs = 1'000'000'000; +// The SDK reads the glove's tare record every 100 ms while a request runs and every second otherwise. +constexpr std::int64_t kTarePollNs = 100'000'000; + +const std::shared_ptr& logger() +{ + static const auto instance = isaaccapture::Logger::get("isaaccapture.plugins.revohuman.GloveSource"); + return instance; +} + +spdlog::level::level_enum to_spdlog(RhLogLevel level) +{ + if (level >= RH_LOG_ERROR) + return spdlog::level::err; + if (level >= RH_LOG_WARNING) + return spdlog::level::warn; + if (level >= RH_LOG_INFO) + return spdlog::level::info; + return spdlog::level::debug; +} + +// The SDK's own lines, at their level; it calls this under its lock, so it must not block. +void forward_sdk_log(RhLogLevel level, const char* message, void*) +{ + static const auto sdk = isaaccapture::Logger::get("isaaccapture.plugins.revohuman.sdk"); + sdk->log(to_spdlog(level), "{}", message != nullptr ? message : ""); +} + +const char* cause_name(RhSyncCause cause) +{ + constexpr const char* kNames[] = { "none", "clock_not_ready", "clock_degraded", "host_clock_event", + "device_reset", "link_unstable" }; + static_assert(std::size(kNames) == RH_CAUSE_LINK_UNSTABLE + 1); + const auto i = static_cast(cause); + return i < std::size(kNames) ? kNames[i] : "unknown"; +} + +// False when the stream ended after the status was read (RH_ERR_STATE). +bool stream_query(RhStatus status, const char* what) +{ + if (status == RH_ERR_STATE) + return false; + if (status != RH_OK) + throw std::runtime_error(std::string(what) + " failed: " + sdk_error()); + return true; +} + +// Whether rh_autokit_recv or rh_autokit_recv_tactile popped an item. RH_ERR_TIMEOUT is an empty queue and RH_ERR_STATE +// a hand that is not streaming. RH_ERR_DISCONNECTED and RH_ERR_PROTOCOL (incompatible firmware) end the stream: the +// SDK reopens it or gives up on its own thread, and the next status says which. +bool popped(RhStatus status, const char* what) +{ + switch (status) + { + case RH_OK: + return true; + case RH_ERR_TIMEOUT: + case RH_ERR_STATE: + case RH_ERR_DISCONNECTED: + case RH_ERR_PROTOCOL: + return false; + default: + throw std::runtime_error(std::string(what) + " failed: " + sdk_error()); + } +} + +TactileSample to_sample(const RhTactile& t) +{ + TactileSample sample{ .device_tick = t.device_tick, + .sample_mono_ns = t.has_sample_time ? std::optional(t.sample_mono_ns) : std::nullopt, + .valid = t.valid != 0 }; + std::array newtons; + const RhStatus status = rh_tactile_newtons(&t, newtons.data(), newtons.size()); + if (status == RH_OK) + { + sample.calibrated = true; + sample.values.assign(newtons.begin(), newtons.begin() + t.point_count); + } + else if (status != RH_ERR_UNSUPPORTED) // Raw counts, not forces. + throw std::runtime_error("rh_tactile_newtons failed: " + sdk_error()); + return sample; +} + +// True for 1, 10, 100, ...: repeated problems are logged ever more rarely. +bool worth_logging(std::uint64_t count) +{ + while (count >= 10 && count % 10 == 0) + count /= 10; + return count == 1; +} + +} // namespace + +GloveSource::Sdk::Sdk() +{ + RhInitOptions init; + rh_init_options_init(&init); + if (rh_init(&init) != RH_OK) + throw std::runtime_error("rh_init failed: " + sdk_error()); + rh_set_log_callback(&forward_sdk_log, nullptr); + logger()->info("RevoHuman C SDK {}", rh_version()); +} + +GloveSource::Sdk::~Sdk() +{ + rh_shutdown(); + rh_set_log_callback(nullptr, nullptr); // The SDK no longer calls into a plugin that may be unloading. +} + +GloveSource::GloveSource() +{ + RhAutoKitOptions options; + rh_autokit_options_init(&options); + options.queue_size = kQueueSize; + // A glove streaming on its own is not reopened as a pair when the other arrives: that stops its samples for a whole + // open, long enough for its hand to be removed. + options.upgrade_to_pair = 0; + // Every tactile sample, not only the latest one at each frame; firmware that cannot split keeps tactile on frames. + options.kit.glove.split_tactile = 1; + RhAutoKit* kit = nullptr; + if (rh_autokit_open(&options, &kit) != RH_OK) + throw std::runtime_error("rh_autokit_open failed: " + sdk_error()); + kit_.reset(kit); +} + +std::optional GloveSource::refresh(Hand hand, std::int64_t now_ns) +{ + RhAutoHandStatus status = RH_STRUCT_INIT(RhAutoHandStatus); + if (rh_autokit_status(kit_.get(), to_rh(hand), &status) != RH_OK) + throw std::runtime_error("rh_autokit_status failed: " + sdk_error()); + report(hand, status, now_ns); + Slot& slot = slots_[index_of(hand)]; + slot.status = status; + if (status.state != RH_AUTO_STREAMING || status.open_count == slot.adopted.open_count) + return std::nullopt; + // Identity and info belong to one stream; if it ended or reopened since the status, the next poll tries again. + Opened opened; + std::uint32_t identity_open = 0; + std::uint32_t info_open = 0; + if (!stream_query( + rh_autokit_identity(kit_.get(), to_rh(hand), &opened.identity, &identity_open), "rh_autokit_identity") || + !stream_query(rh_autokit_glove_info(kit_.get(), to_rh(hand), &opened.info, &info_open), "rh_autokit_glove_info") || + identity_open != status.open_count || info_open != status.open_count) + return std::nullopt; + return opened; +} + +bool GloveSource::streaming(Hand hand) const +{ + const Slot& slot = slots_[index_of(hand)]; + return slot.status.state == RH_AUTO_STREAMING && slot.status.open_count == slot.adopted.open_count; +} + +// Logs what changed since the last status; the operator learns of every open, end, give-up and sync change. +void GloveSource::report(Hand hand, const RhAutoHandStatus& now, std::int64_t now_ns) +{ + Slot& slot = slots_[index_of(hand)]; + const RhAutoHandStatus& was = slot.status; + const char* side = hand_name(hand); + const std::string error = sdk_text(now.last_error_text); + // Each retry passes through OPENING, so an error is logged when its text changes, not on every attempt. + std::string& logged = slot.logged_error; + if (now.state == RH_AUTO_STREAMING) + logged.clear(); + else if (now.state == RH_AUTO_FAILED && was.state != RH_AUTO_FAILED) + logger()->error("{}: giving up on the glove: {}", side, error); + else if (was.state == RH_AUTO_STREAMING && now.state == RH_AUTO_AWAIT_PAIR) + logger()->warn("{}: glove stream ended ({}); waiting for both gloves to reopen the pair", side, error); + else if (was.state == RH_AUTO_STREAMING && now.state == RH_AUTO_ABSENT) + logger()->warn("{}: glove stream ended ({}); looking for the glove again", side, error); + else if (now.state == RH_AUTO_ABSENT && !error.empty() && error != logged) + { + if (now.absent) + logger()->info("{}: no glove ({}); looking for it again", side, error); + else + logger()->warn("{}: cannot open the glove yet: {}", side, error); + } + if (now.state != RH_AUTO_STREAMING && now.state != RH_AUTO_OPENING) + logged = error; + if (now.state != RH_AUTO_STREAMING) + return; + if (now.pair_lost && !(was.state == RH_AUTO_STREAMING && was.pair_lost)) + logger()->warn("{}: glove open on its own after the pair was lost; the gloves stay unpaired", side); + if (now.paired && was.state == RH_AUTO_STREAMING && was.paired && now.synchronized != was.synchronized) + { + if (now.synchronized) + logger()->info("{}: gloves hardware-synced again", side); + else + logger()->warn("{}: gloves not hardware-synced; pairing in host time", side); + } + if (was.state == RH_AUTO_STREAMING ? now.capture_ok != was.capture_ok || now.capture_cause != was.capture_cause : + !now.capture_ok) + { + if (now.capture_ok) + logger()->info("{}: capture gate open", side); + else + logger()->warn( + "{}: capture gate closed ({}); no samples until it opens", side, cause_name(now.capture_cause)); + } + if (now.open_count != slot.adopted.open_count) // A new stream counts its drops from zero. + slot.dropped_logged = slot.tactile_dropped_logged = 0; + if ((now.dropped > slot.dropped_logged || now.tactile_dropped > slot.tactile_dropped_logged) && + (!slot.last_drop_log_ns || now_ns - *slot.last_drop_log_ns >= kDropLogNs)) + { + logger()->warn("{}: the SDK dropped {} glove frames and {} tactile samples; the plugin polls too slowly", side, + now.dropped - slot.dropped_logged, now.tactile_dropped - slot.tactile_dropped_logged); + slot.dropped_logged = now.dropped; + slot.tactile_dropped_logged = now.tactile_dropped; + slot.last_drop_log_ns = now_ns; + } +} + +std::optional GloveSource::adopt(Hand hand, const Opened& opened) +{ + Slot& slot = slots_[index_of(hand)]; + const RhAutoHandStatus& status = slot.status; + // The SDK starts each stream's tare status from scratch; a request on an older stream ended with it. + slot.adopted = AdoptedStream{ .open_count = status.open_count }; + AdoptedStream& adopted = slot.adopted; + if (slot.tare_open && *slot.tare_open != status.open_count) + { + logger()->warn( + "{}: the glove reopened before the tactile tare result was known; check the glove LED", hand_name(hand)); + slot.tare_open.reset(); + } + HandMeta meta; + meta.product_code = sdk_text(opened.identity.variant_code); + meta.has_tactile = (opened.identity.variant_sensors & RH_SENSOR_TACTILE) != 0; + adopted.has_tactile = meta.has_tactile; + // The firmware SN is not unique (both gloves of a pair may report the same one), so the hand keys the device and + // the SN only traces the board. + const std::string sn = sdk_text(opened.info.sn); + meta.device_id = std::string("revohuman_") + hand_name(hand) + (sn.empty() ? "" : "_" + sn); + try + { + HandModel model(meta.product_code); + 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("rh_joint_name " + std::to_string(i) + " failed: " + sdk_error()); + if (static_cast(length) >= sizeof(name)) + throw std::runtime_error("rh_joint_name " + std::to_string(i) + " is longer than " + + std::to_string(sizeof(name) - 1) + " characters"); + meta.joint_names[i] = name; + } + adopted.model = std::move(model); + } + catch (const std::exception& e) + { + const std::string& why = adopted.model.emplace(e.what()); + logger()->error("{}: cannot use the glove: {}", hand_name(hand), why); + return std::nullopt; + } + const HandModel& model = std::get(adopted.model); + logger()->info("{}: glove {} (product {}, port {}, firmware {}, {} taxels), hand model {}, {}", hand_name(hand), + meta.device_id, meta.product_code, sdk_text(opened.info.port), sdk_text(opened.info.fw_version), + opened.info.tactile_point_count, model.id(), status.paired ? "paired" : "on its own"); + if (meta.has_tactile) + { + // The SDK release the plugin pins has a taxel layout for the taxel count of every product with tactile + // sensors; without one the hand is still pushed, and device status reports its tactile as failed. + try + { + meta.tactile_metadata = model.tactile_metadata(hand, opened.info.tactile_point_count); + meta.taxel_count = opened.info.tactile_point_count; + } + catch (const std::runtime_error& e) + { + logger()->warn("{}: the glove's tactile is not pushed: {}", hand_name(hand), e.what()); + } + } + adopted.product_code = meta.product_code; + return meta; +} + +std::optional GloveSource::receive(Hand hand) +{ + AdoptedStream& adopted = slots_[index_of(hand)].adopted; + RhGloveFrame frame = RH_STRUCT_INIT(RhGloveFrame); + std::uint32_t open_count = 0; + // A frame of a newer stream than the adopted one is popped and dropped; the next poll adopts that stream. + while (popped(rh_autokit_recv(kit_.get(), to_rh(hand), 0, &frame, &open_count), "rh_autokit_recv") && + open_count == adopted.open_count) + { + const HandModel* model = std::get_if(&adopted.model); + if (model == nullptr) + continue; + if (frame.struct_size < sizeof(RhGloveFrame)) + throw std::runtime_error("the RevoHuman C SDK library is older than the header it was built with"); + GloveSample sample; + // Gated subscriptions deliver only frames with a sampling time. + if (frame.hand != to_rh(hand) || !(frame.flags & RH_FRAME_FLAG_HAS_SAMPLE_TIME) || + frame.tactile.point_count > RH_TACTILE_MAX_POINTS || + rh_joints_to_radians(adopted.product_code.c_str(), frame.joints_deg, sample.joints_rad.data()) != RH_OK) + { + if (worth_logging(++adopted.rejected)) + logger()->warn("{}: unusable glove frame dropped ({} so far)", hand_name(hand), adopted.rejected); + continue; + } + sample.generation = frame.generation; + sample.sample_mono_ns = frame.sample_mono_ns; + sample.joints_valid_mask = frame.joints_valid_mask; + if ((frame.flags & RH_FRAME_FLAG_HAS_TACTILE) && tactile_on_frames(hand)) + sample.tactile = to_sample(frame.tactile); + sample.joint_poses = model->joint_poses(hand, sample.joints_rad, sample.joints_valid_mask); + return sample; + } + return std::nullopt; +} + +std::optional GloveSource::receive_tactile(Hand hand) +{ + AdoptedStream& adopted = slots_[index_of(hand)].adopted; + RhTactileSample sample = RH_STRUCT_INIT(RhTactileSample); + std::uint32_t open_count = 0; + // As receive(); RH_ERR_TIMEOUT for good when the firmware does not split tactile. + while (popped(rh_autokit_recv_tactile(kit_.get(), to_rh(hand), 0, &sample, &open_count), "rh_autokit_recv_tactile") && + open_count == adopted.open_count) + { + if (!std::holds_alternative(adopted.model)) + continue; + if (sample.struct_size < sizeof(RhTactileSample)) + throw std::runtime_error("the RevoHuman C SDK library is older than the header it was built with"); + if (sample.hand != to_rh(hand) || sample.tactile.point_count > RH_TACTILE_MAX_POINTS) + { + if (worth_logging(++adopted.tactile_rejected)) + logger()->warn( + "{}: unusable tactile sample dropped ({} so far)", hand_name(hand), adopted.tactile_rejected); + continue; + } + if (std::exchange(adopted.tactile_from, TactileFrom::Split) != TactileFrom::Split) + logger()->info("{}: tactile split from the glove frames, every sample", hand_name(hand)); + return SplitTactile{ sample.generation, to_sample(sample.tactile) }; + } + return std::nullopt; +} + +// A split stream's frames repeat its latest tactile sample: feeding both would push samples out of order, so a stream +// takes its tactile from one place. Frames are used only when a new stream sends no split sample for a while. +bool GloveSource::tactile_on_frames(Hand hand) +{ + AdoptedStream& adopted = slots_[index_of(hand)].adopted; + if (adopted.tactile_from == TactileFrom::Undecided && ++adopted.tactile_frames >= kSplitWaitFrames) + { + adopted.tactile_from = TactileFrom::Frames; + logger()->info("{}: tactile with the glove frames (the firmware does not split it)", hand_name(hand)); + } + return adopted.tactile_from == TactileFrom::Frames; +} + +void GloveSource::watch_tare(Hand hand, std::int64_t now_ns) +{ + Slot& slot = slots_[index_of(hand)]; + AdoptedStream& adopted = slot.adopted; + if (!adopted.has_tactile || slot.status.open_count != adopted.open_count || + (adopted.last_tare_poll_ns && now_ns - *adopted.last_tare_poll_ns < kTarePollNs)) + return; + adopted.last_tare_poll_ns = now_ns; + RhTactileTareStatus now = RH_STRUCT_INIT(RhTactileTareStatus); + std::uint32_t open_count = 0; + if (!stream_query(rh_autokit_tactile_tare_status(kit_.get(), to_rh(hand), &now, &open_count), + "rh_autokit_tactile_tare_status") || + open_count != adopted.open_count) + return; + RhTactileTareStatus& was = adopted.tare; + if (now.version == was.version) + return; + const char* side = hand_name(hand); + if (now.support == RH_TARE_SUPPORT_UNSUPPORTED && was.support != RH_TARE_SUPPORT_UNSUPPORTED) + logger()->info( + "{}: the glove cannot tare its tactile sensors on request ({}); hold its button for about 2 s to " + "tare instead", + side, sdk_text(now.support_message)); + // This plugin's own request; request_wall_ns is set when its result is. + if (now.request_wall_ns != 0 && now.request_wall_ns != was.request_wall_ns) + { + if (now.request_result == RH_TARE_RESULT_SENT) + logger()->info("{}: tactile tare: {}", side, sdk_text(now.request_message)); + else + logger()->warn("{}: tactile tare request: {}", side, sdk_text(now.request_message)); + if (slot.tare_open == open_count) + slot.tare_open.reset(); + } + // A tare from the glove's button or another host while this stream runs; otherwise the last tare's zero point + // after a reconnect or a controller restart. The message says which. + const bool external = now.origin == RH_TARE_ORIGIN_BUTTON || now.origin == RH_TARE_ORIGIN_OTHER_HOST; + if (external && was.has_record && (now.sequence != was.sequence || now.outcome != was.outcome)) + logger()->log(now.outcome == RH_TARE_OUTCOME_PENDING || now.outcome == RH_TARE_OUTCOME_SENT ? + spdlog::level::info : + spdlog::level::warn, + "{}: tactile tare: {}", side, sdk_text(now.message)); + else if ((now.stale && !was.stale && now.outcome == RH_TARE_OUTCOME_SENT) || (now.mcu_reset && !was.mcu_reset)) + logger()->info("{}: tactile tare: {}", side, sdk_text(now.message)); + was = now; +} + +void GloveSource::request_tare() +{ + for (const Hand hand : kHands) + { + Slot& slot = slots_[index_of(hand)]; + const char* side = hand_name(hand); + if (!streaming(hand) || !slot.adopted.has_tactile) + { + logger()->info("{}: tactile tare not requested: the glove {}", side, + streaming(hand) ? "has no tactile sensors" : "is not streaming"); + continue; + } + std::uint32_t open_count = 0; + const RhStatus status = rh_autokit_tactile_tare_request(kit_.get(), to_rh(hand), &open_count); + if (status == RH_OK) + { + slot.tare_open = open_count; + slot.adopted.last_tare_poll_ns.reset(); + logger()->info( + "{}: tactile tare requested; keep the hand still and the tactile surface untouched until the " + "command is sent (about 3 s)", + side); + continue; + } + const RhErrorCode code = rh_last_error_code(); + const std::string error = sdk_error(); + const RhTactileTareStatus& tare = slot.adopted.tare; + const char* why = nullptr; + if (status == RH_ERR_UNSUPPORTED) + why = code == RH_E_TACTILE_UNSUPPORTED ? + "the glove has no tactile sensors" : + "the glove cannot tare on request; hold its button for about 2 s instead"; + else if (status == RH_ERR_STATE) + why = tare.in_progress ? "the previous request is still running" : + tare.support == RH_TARE_SUPPORT_UNKNOWN ? + "the glove's tare support is not known yet; try again in a few seconds" : + "the glove stream is reconnecting or busy; try again in a few seconds"; + if (why) + logger()->warn("{}: tactile tare not requested ({}): {}", side, error, why); + else + logger()->warn("{}: tactile tare request failed: {}", side, error); + } +} + +std::optional GloveSource::note(Hand hand) const +{ + const Slot& slot = slots_[index_of(hand)]; + const RhAutoHandStatus& s = slot.status; + const std::string error = sdk_text(s.last_error_text); + const std::string because = error.empty() ? "" : ": " + error; + switch (s.state) + { + case RH_AUTO_FAILED: + return GloveNote{ GloveNote::Kind::Ended, "cannot open the glove" + because }; + case RH_AUTO_AWAIT_PAIR: + return GloveNote{ GloveNote::Kind::Waiting, "waiting for both gloves to reopen the pair" + because }; + case RH_AUTO_STREAMING: + break; + default: + return GloveNote{ GloveNote::Kind::Waiting, "waiting for the glove" + because }; + } + if (const auto* why = std::get_if(&slot.adopted.model)) + return GloveNote{ GloveNote::Kind::Ended, "cannot use the glove: " + *why }; + if (!s.capture_ok) + return GloveNote{ GloveNote::Kind::NotReady, + std::string("capture gate closed (") + cause_name(s.capture_cause) + ")" }; + if (s.pair_lost) + return GloveNote{ GloveNote::Kind::Unsynced, "unpaired after the pair was lost" }; + if (s.paired && !s.synchronized) + return GloveNote{ GloveNote::Kind::Unsynced, "not hardware-synced; pairing in host time" }; + if (!s.paired && slots_[1 - index_of(hand)].status.state == RH_AUTO_STREAMING) // The other glove. + return GloveNote{ GloveNote::Kind::Unsynced, "not hardware-synced; the gloves opened separately" }; + return std::nullopt; +} + +bool GloveSource::finished() const +{ + for (const Slot& slot : slots_) + if (slot.status.state != RH_AUTO_FAILED && !std::holds_alternative(slot.adopted.model)) + return false; + return true; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/app/glove_source.hpp b/src/plugins/revohuman/app/glove_source.hpp new file mode 100644 index 0000000000..03d7c544a0 --- /dev/null +++ b/src/plugins/revohuman/app/glove_source.hpp @@ -0,0 +1,189 @@ +// 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 + +namespace plugins +{ +namespace revohuman +{ + +//! What device status says about a glove that is not streaming normally. +struct GloveNote +{ + enum class Kind + { + Waiting, //!< Not open; looked for again. + NotReady, //!< Open, but the capture gate is closed. + Unsynced, //!< Streaming without hardware sync to the other glove. + Ended, //!< Given up, or open with a hand model the plugin cannot use. + }; + Kind kind = Kind::Waiting; + std::string text; +}; + +/*! + * @brief Gloves through the RevoHuman C SDK's autokit (rh_autokit_*), at most one per hand, found without + * configuration. The SDK opens, retries, pairs and reopens them on its own thread (its rules are in revohuman.h); + * this class turns each newly opened stream into hand metadata, feeds its frames to the pipelines and reports. + * Tactile is opened split from the frames: a glove whose firmware splits it sends every tactile sample (about 215 Hz, + * above the 200 Hz frame rate) on its own, and the frames carry only the latest; other gloves send tactile with the + * frames. Which of the two a glove does is settled when its stream opens. + */ +class GloveSource +{ +public: + //! Throws std::runtime_error when the SDK cannot start. Gloves that never open are reported, not thrown. + GloveSource(); + GloveSource(const GloveSource&) = delete; + GloveSource& operator=(const GloveSource&) = delete; + + //! Feeds what the SDK queued since the last call to the hand pipelines (indexed by Hand). A newly opened stream + //! restarts its pipeline, on the stream's HandMeta when its hand model can be used. + template + void poll(std::int64_t now_ns, const std::array& pipelines) + { + for (const Hand hand : kHands) + { + Pipeline& pipeline = *pipelines[index_of(hand)]; + if (const std::optional opened = refresh(hand, now_ns)) + { + const std::optional meta = adopt(hand, *opened); + pipeline.reset(); + if (meta) + pipeline.set_meta(*meta); + } + if (!streaming(hand)) + continue; + watch_tare(hand, now_ns); + const std::int64_t received_ns = core::os_monotonic_now_ns(); + // Tactile first: the SDK queues a split sample before the frames that carry it. + for (std::uint32_t n = 0; n < kTactileQueueSize; ++n) + { + const std::optional split = receive_tactile(hand); + if (!split) + break; + pipeline.on_tactile(split->generation, split->tactile, received_ns); + } + for (std::uint32_t n = 0; n < kQueueSize; ++n) + { + const std::optional sample = receive(hand); + if (!sample) + break; + pipeline.on_glove(*sample, received_ns); + } + } + } + //! nullopt while the hand's glove streams normally. + std::optional note(Hand hand) const; + //! Both gloves were given up. + bool finished() const; + //! Asks each streaming glove with tactile sensors to tare them, a write to the glove: it zeroes the taxels at their + //! current readings. The SDK sends the request once and never resends it; poll() logs how it ends. + void request_tare(); + +private: + //! Frames the SDK buffers per glove between polls (1.3 s at 200 Hz). + static constexpr std::uint32_t kQueueSize = 256; + static constexpr std::uint32_t kTactileQueueSize = 512; //!< The SDK's split tactile queue per glove. + //! Frames of a new stream with tactile but no split sample before its tactile is taken from the frames (0.5 s). + static constexpr std::uint32_t kSplitWaitFrames = 100; + + enum class TactileFrom + { + Undecided, //!< No split sample yet; the frames' tactile is not used. + Frames, //!< The firmware does not split tactile. + Split, //!< Every sample on its own; the frames' copies are not used. + }; + //! The stream the pipeline holds metadata for; adopt() starts it afresh. + struct AdoptedStream + { + std::uint32_t open_count = 0; //!< The SDK's open count of the stream, from 1. + //! Its hand model, or why the model cannot be used, when its frames are dropped. None before a stream. + std::variant model; + std::string product_code; + bool has_tactile = false; + std::uint64_t rejected = 0; + std::uint64_t tactile_rejected = 0; + TactileFrom tactile_from = TactileFrom::Undecided; + std::uint32_t tactile_frames = 0; //!< Frames with tactile while undecided. + RhTactileTareStatus tare = RH_STRUCT_INIT(RhTactileTareStatus); //!< As last logged. + std::optional last_tare_poll_ns; + }; + struct Slot + { + RhAutoHandStatus status = RH_STRUCT_INIT(RhAutoHandStatus); + AdoptedStream adopted; + std::string logged_error; //!< The last open error logged, so retries with the same error stay quiet. + std::uint64_t dropped_logged = 0; //!< Of the stream in the status. + std::uint64_t tactile_dropped_logged = 0; + std::optional last_drop_log_ns; + //! The stream a tare was requested on whose result is not logged yet; it may end before the result. + std::optional tare_open; + }; + + //! A newly opened stream, as refresh() found it. + struct Opened + { + RhProductIdentity identity = RH_STRUCT_INIT(RhProductIdentity); + RhGloveInfo info = RH_STRUCT_INIT(RhGloveInfo); + }; + //! A split tactile sample of the adopted stream. + struct SplitTactile + { + std::uint32_t generation = 0; + TactileSample tactile; + }; + //! rh_init and the SDK log forwarding, for as long as the source lives. + struct Sdk + { + Sdk(); + ~Sdk(); + Sdk(const Sdk&) = delete; + Sdk& operator=(const Sdk&) = delete; + }; + struct CloseKit + { + //! Waits for an open in progress, then closes every subscription and kit. + void operator()(RhAutoKit* kit) const + { + rh_autokit_close(kit); + } + }; + + //! Reads and reports the hand's status. Set for a stream that is not adopted yet; a new stream whose identity + //! could not be read yet is tried again on the next call. + std::optional refresh(Hand hand, std::int64_t now_ns); + void report(Hand hand, const RhAutoHandStatus& now, std::int64_t now_ns); + //! The hand streams on the adopted stream. + bool streaming(Hand hand) const; + //! Logs what changed in the adopted stream's tactile tare. + void watch_tare(Hand hand, std::int64_t now_ns); + //! The HandMeta of the newly opened stream, or nullopt when its hand model cannot be used. + std::optional adopt(Hand hand, const Opened& opened); + //! The next usable frame of the adopted stream; unusable ones are dropped. nullopt when there is none. + std::optional receive(Hand hand); + //! The next usable split tactile sample, like receive(). + std::optional receive_tactile(Hand hand); + //! Whether this frame's tactile is the stream's tactile (see TactileFrom). + bool tactile_on_frames(Hand hand); + + Sdk sdk_; + std::unique_ptr kit_; //!< After sdk_, so it is closed before rh_shutdown. + std::array slots_; +}; + +} // 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..4dce03c638 --- /dev/null +++ b/src/plugins/revohuman/app/main.cpp @@ -0,0 +1,160 @@ +// 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 +#include +#include + +using namespace plugins::revohuman; + +namespace +{ + +constexpr std::string_view kUsage = + "Usage: revohuman_plugin\n" + " [--collection-prefix=NAME]\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" + " [--plugin-root-id=ID]\n" + "SIGINT or SIGTERM stops the plugin. SIGUSR1 tares the tactile sensors of the streaming gloves: keep the hands\n" + "still and the tactile surfaces untouched for about 3 s."; + +volatile std::sig_atomic_t g_stop = 0; +volatile std::sig_atomic_t g_tare = 0; + +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; +} + +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 + "'"); +} + +// "px,py,pz,qx,qy,qz,qw" in meters, quaternion normalized. +XrPosef parse_pose(const std::string& text, std::string_view flag) +{ + std::array v{}; + std::size_t count = 0; + bool ok = true; + for (std::size_t begin = 0; ok && begin <= text.size(); ++count) + { + const std::size_t end = std::min(text.find(',', begin), text.size()); + const char* const item_end = text.data() + end; + double value = 0; + // std::from_chars skips no blanks, so an item must be the number alone. + const auto [parsed_end, error] = std::from_chars(text.data() + begin, item_end, value); + ok = count < v.size() && error == std::errc() && parsed_end == item_end && std::isfinite(value) && + std::abs(value) <= std::numeric_limits::max(); + if (ok) + v[count] = value; + begin = end + 1; + } + const double n = std::sqrt(v[3] * v[3] + v[4] * v[4] + v[5] * v[5] + v[6] * v[6]); + if (!ok || count != v.size() || !(n > 1e-6)) + throw std::invalid_argument(std::string(flag) + " needs seven finite numbers px,py,pz,qx,qy,qz,qw with a " + + "non-zero quaternion, got '" + text + "'"); + return XrPosef{ { float(v[3] / n), float(v[4] / n), float(v[5] / n), float(v[6] / n) }, + { float(v[0]), float(v[1]), float(v[2]) } }; +} + +} // namespace + +int main(int argc, char** argv) +try +{ + auto logger = isaaccapture::Logger::get("isaaccapture.plugins.revohuman.main"); + RevoHumanPluginConfig config; + std::optional wrist_flag; + for (int i = 1; i < argc; ++i) + { + const std::string_view arg = argv[i]; + if (arg == "--help" || arg == "-h") + { + std::cout << kUsage << std::endl; // Usage text is terminal UX, not a diagnostic. + return 0; + } + if (auto value = flag_value(arg, "--collection-prefix")) + config.collection_prefix = *value; + else if (auto value = flag_value(arg, "--wrist-source")) + config.wrist.mode = wrist_source_mode(*(wrist_flag = *value), "--wrist-source"); + else if (auto value = flag_value(arg, "--aim-to-wrist-left")) + config.wrist.left_aim_to_wrist = parse_pose(*value, "--aim-to-wrist-left"); + else if (auto value = flag_value(arg, "--aim-to-wrist-right")) + config.wrist.right_aim_to_wrist = parse_pose(*value, "--aim-to-wrist-right"); + 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"); + if (config.plugin_root_id && config.plugin_root_id->empty()) + throw std::invalid_argument("--plugin-root-id must not be empty"); + // The hand capture labels a take with this variable, so a flag that disagrees is refused. + if (const char* env = std::getenv("REVOHUMAN_WRIST_SOURCE"); env != nullptr && *env != '\0') + { + if (wrist_flag && *wrist_flag != env) + throw std::invalid_argument("--wrist-source=" + *wrist_flag + " disagrees with REVOHUMAN_WRIST_SOURCE=" + env); + config.wrist.mode = wrist_source_mode(env, "REVOHUMAN_WRIST_SOURCE"); + } + + std::signal(SIGINT, [](int) { g_stop = 1; }); + std::signal(SIGTERM, [](int) { g_stop = 1; }); + std::signal(SIGUSR1, [](int) { g_tare = 1; }); + RevoHumanPlugin plugin(config); + logger->info("RevoHuman (collections: {}__)", config.collection_prefix); + + // Tick every millisecond, faster than any glove stream; report device status at 100 Hz. + const auto start = std::chrono::steady_clock::now(); + bool reported_finish = false; + for (std::size_t tick = 0; !g_stop; ++tick) + { + plugin.update(core::os_monotonic_now_ns()); + if (g_tare != 0) + { + g_tare = 0; + logger->info("SIGUSR1: requesting a tactile tare"); + plugin.request_tare(); + } + if (tick % 10 == 0) + plugin.publish_status(core::os_monotonic_now_ns()); + if (plugin.finished() && !std::exchange(reported_finish, true)) + logger->info("both gloves given up; waiting for a stop signal"); // Status keeps showing why. + std::this_thread::sleep_until(start + std::chrono::milliseconds(tick + 1)); + } + logger->info("stopping"); + return 0; +} +catch (const std::exception& e) +{ + isaaccapture::Logger::get("isaaccapture.plugins.revohuman.main")->error("{}: {}", argv[0], e.what()); + 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..29c97206e2 --- /dev/null +++ b/src/plugins/revohuman/app/revohuman_plugin.cpp @@ -0,0 +1,273 @@ +// 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 + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +using plugin_utils::PluginDeviceStatusEntry; + +// OpenXRSession enables the XrTimeConverter extensions itself; the status publisher's are enabled even without +// --plugin-root-id. +std::vector required_extensions(const plugin_utils::WristPoseSource::Requirements& wrist) +{ + std::vector out = core::DeviceIOSession::get_required_extensions(wrist.trackers); + const auto add = [&out](const auto& more) + { + for (const auto& extension : more) + if (std::find(out.begin(), out.end(), extension) == out.end()) + out.emplace_back(extension); + }; + add(core::SchemaPusher::get_required_extensions()); + add(plugin_utils::PluginDeviceStatusPublisher::get_required_extensions()); + add(wrist.extensions); + add(std::vector{ XR_NVX1_DEVICE_INTERFACE_BASE_EXTENSION_NAME }); + return out; +} + +core::SchemaPusherConfig pusher_config(const std::string& collection, std::size_t max_bytes, std::string tensor, Hand hand) +{ + return core::SchemaPusherConfig{ .collection_id = collection, + .max_flatbuffer_size = max_bytes, + .tensor_identifier = tensor, + .localized_name = std::string("RevoHuman ") + hand_name(hand) + " " + tensor, + .app_name = "RevoHumanPlugin" }; +} + +// A device that gave data before and stopped is DISCONNECTED; one that never did is UNKNOWN, so a plugin +// started before its gloves are plugged in is not reported as having lost them. +core::PluginDeviceState lost_or_unknown(bool seen) +{ + return seen ? core::PluginDeviceState_DISCONNECTED : core::PluginDeviceState_UNKNOWN; +} + +// The /hand, /glove and /tactile rows of one hand, using only the state/reason pairs TeleopSession accepts +// (teleop_session_manager/status_monitor.py). +std::array hand_status(Hand hand, const HandHealth& h, const std::optional& note) +{ + using namespace core; + const std::string side = hand_name(hand); + PluginDeviceStatusEntry hand_row{ + "/hand/" + side, lost_or_unknown(h.glove_seen), PluginDeviceReason_NO_HARDWARE_SIGNAL, {} + }; + if (h.hand_active) + hand_row = + h.wrist_tracked ? + PluginDeviceStatusEntry{ hand_row.path, PluginDeviceState_CONNECTED, PluginDeviceReason_NONE, {} } : + PluginDeviceStatusEntry{ hand_row.path, PluginDeviceState_DEGRADED, PluginDeviceReason_NO_CURRENT_DATA, + "no tracked wrist pose from the host" }; + + PluginDeviceStatusEntry glove_row{ + "/glove/revohuman_" + side, PluginDeviceState_CONNECTED, PluginDeviceReason_NONE, {} + }; + if (note && note->kind == GloveNote::Kind::Ended) + glove_row = { glove_row.path, PluginDeviceState_FAILED, PluginDeviceReason_DEVICE_ERROR, note->text }; + else if (note && note->kind == GloveNote::Kind::Waiting) + glove_row = { glove_row.path, lost_or_unknown(h.glove_seen), PluginDeviceReason_NO_HARDWARE_SIGNAL, note->text }; + else if (note && note->kind == GloveNote::Kind::NotReady) + glove_row = { glove_row.path, h.glove_seen ? PluginDeviceState_DEGRADED : PluginDeviceState_UNKNOWN, + PluginDeviceReason_NO_CURRENT_DATA, note->text }; + else if (!h.glove_live) + glove_row = { glove_row.path, lost_or_unknown(h.glove_seen), PluginDeviceReason_NO_HARDWARE_SIGNAL, + h.glove_seen ? "no recent glove sample" : "" }; + else if (note) // Unsynced: the samples flow, their timing against the other glove is degraded. + glove_row = { glove_row.path, PluginDeviceState_DEGRADED, PluginDeviceReason_DEVICE_ERROR, note->text }; + + PluginDeviceStatusEntry tactile_row{ + "/tactile/revohuman_" + side, lost_or_unknown(h.tactile_seen), PluginDeviceReason_NO_HARDWARE_SIGNAL, {} + }; + if (h.tactile_live) + tactile_row = { tactile_row.path, PluginDeviceState_CONNECTED, PluginDeviceReason_NONE, {} }; + else if (h.tactile_skip == TactileSkip::NoTactile) + tactile_row = { tactile_row.path, PluginDeviceState_DISABLED, PluginDeviceReason_DISABLED_BY_CONFIGURATION, + "this glove has no tactile sensors" }; + else if (h.tactile_skip == TactileSkip::NoShape) // Not expected with the C SDK release the plugin pins. + tactile_row = { tactile_row.path, PluginDeviceState_FAILED, PluginDeviceReason_DEVICE_ERROR, + "the C SDK has no taxel layout for this glove" }; + else if (h.glove_live && h.tactile_skip == TactileSkip::Uncalibrated) + tactile_row = { tactile_row.path, PluginDeviceState_DEGRADED, PluginDeviceReason_CALIBRATION_FAILED, + "uncalibrated" }; + else if (h.glove_live && h.tactile_skip == TactileSkip::CountMismatch) + tactile_row = { tactile_row.path, PluginDeviceState_FAILED, PluginDeviceReason_DEVICE_ERROR, + "taxel count differs from the TactileMetadata" }; + else if (h.glove_live && h.tactile_skip != TactileSkip::None) + tactile_row = { tactile_row.path, PluginDeviceState_DEGRADED, PluginDeviceReason_NO_CURRENT_DATA, + tactile_skip_name(h.tactile_skip) }; + return { std::move(hand_row), std::move(glove_row), std::move(tactile_row) }; +} + +} // namespace + +//! One hand's OpenXR outputs. Each exists only while the pipeline says so. +class RevoHumanPlugin::HandOutput +{ +public: + HandOutput(const core::OpenXRSessionHandles& handles, + plugin_utils::WristPoseSource& wrists, + Hand hand, + const std::string& prefix) + : handles_(handles), + time_converter_(handles), + wrists_(wrists), + hand_(hand), + collection_(prefix + "_" + hand_name(hand)) + { + } + + void push_joint_state(std::span buffer, std::int64_t sample_ns) + { + if (!joint_state_) + joint_state_ = std::make_unique( + handles_, pusher_config(collection_ + "_joint_state", kJointStateMaxBytes, "joint_state", hand_)); + joint_state_->push_buffer(buffer.data(), buffer.size(), sample_ns, sample_ns); + } + void clear_joint_state() + { + joint_state_.reset(); + } + void push_tactile(std::span buffer, std::int64_t sample_ns, std::int64_t raw_ns) + { + if (!tactile_) + tactile_ = std::make_unique( + handles_, pusher_config(collection_ + "_tactile", kTactileMaxBytes, "tactile", hand_)); + tactile_->push_buffer(buffer.data(), buffer.size(), sample_ns, raw_ns); + } + void clear_tactile() + { + tactile_.reset(); + } + void push_shape(std::span buffer, std::int64_t now_ns) + { + // Sized to the encoded table, so a glove with another layout gets a new collection. + if (!shape_ || shape_size_ != buffer.size()) + { + shape_.reset(); + shape_ = std::make_unique( + handles_, pusher_config(core::tracker_metadata_collection_id(collection_ + "_tactile"), buffer.size(), + std::string(core::TRACKER_METADATA_TENSOR_IDENTIFIER), hand_)); + shape_size_ = buffer.size(); + } + shape_->push_buffer(buffer.data(), buffer.size(), now_ns, now_ns); + } + void clear_shape() + { + shape_.reset(); + } + + //! The injected hand; destroying it removes the hand, and readers then see isActive = false. + class InjectedHand + { + public: + explicit InjectedHand(HandOutput& output) + : output_(&output), + injector_(std::make_unique( + output.handles_.instance, + output.handles_.session, + output.hand_ == Hand::Left ? XR_HAND_LEFT_EXT : XR_HAND_RIGHT_EXT, + output.handles_.space)) + { + } + + bool push(const HandJointPoses& poses, std::int64_t sample_ns) + { + const XrTime time = output_->time_converter_.convert_monotonic_ns_to_xrtime(sample_ns); + const plugin_utils::WristSample wrist = output_->wrists_.query(output_->hand_ == Hand::Left, time); + const bool tracked = wrist.valid && wrist.tracked; + std::array joints{}; + for (std::size_t i = 0; i < joints.size(); ++i) + { + const XrPosef local = oxr_utils::to_xr_posef(poses.joints[i]); + const bool position = (poses.position_valid_mask >> i) & 1u; + const bool orientation = (poses.orientation_valid_mask >> i) & 1u; + joints[i].locationFlags = (position ? XR_SPACE_LOCATION_POSITION_VALID_BIT : 0) | + (orientation ? XR_SPACE_LOCATION_ORIENTATION_VALID_BIT : 0) | + (position && tracked ? XR_SPACE_LOCATION_POSITION_TRACKED_BIT : 0) | + (orientation && tracked ? XR_SPACE_LOCATION_ORIENTATION_TRACKED_BIT : 0); + // Without a wrist pose the hand stays at the stage origin. + joints[i].pose = wrist.valid ? oxr_utils::multiply_poses(wrist.pose, local) : local; + joints[i].radius = 0.01f; // Nominal, for visualization. + } + injector_->push(joints.data(), time); + return tracked; + } + + private: + HandOutput* output_; + std::unique_ptr injector_; // HandInjector itself cannot move. + }; + + InjectedHand create_hand() + { + return InjectedHand(*this); + } + +private: + core::OpenXRSessionHandles handles_; + core::XrTimeConverter time_converter_; + plugin_utils::WristPoseSource& wrists_; + Hand hand_; + std::string collection_; + std::unique_ptr joint_state_; + std::unique_ptr tactile_; + std::unique_ptr shape_; + std::size_t shape_size_ = 0; +}; + +RevoHumanPlugin::RevoHumanPlugin(const RevoHumanPluginConfig& config) +{ + auto requirements = plugin_utils::WristPoseSource::collect_requirements(config.wrist.mode); + session_ = std::make_shared("RevoHumanPlugin", required_extensions(requirements)); + const auto handles = session_->get_handles(); + deviceio_ = core::DeviceIOSession::run(requirements.trackers, handles); + wrists_ = std::make_unique( + config.wrist, handles, deviceio_.get(), std::move(requirements.controller_tracker)); + for (const Hand hand : kHands) + { + auto& output = outputs_[index_of(hand)]; + output = std::make_unique(handles, *wrists_, hand, config.collection_prefix); + pipelines_[index_of(hand)] = std::make_unique>(hand, *output); + } + if (config.plugin_root_id) + status_ = std::make_unique(handles, *config.plugin_root_id); +} + +RevoHumanPlugin::~RevoHumanPlugin() = default; + +void RevoHumanPlugin::update(std::int64_t now_ns) +{ + deviceio_->update(); // WristPoseSource reads the controllers through this session, on this thread. + source_.poll(now_ns, std::array{ pipelines_[0].get(), pipelines_[1].get() }); + for (auto& pipeline : pipelines_) + 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) + for (auto& row : hand_status(hand, pipelines_[index_of(hand)]->health(now_ns), source_.note(hand))) + entries.push_back(std::move(row)); + 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..d5401a72b2 --- /dev/null +++ b/src/plugins/revohuman/app/revohuman_plugin.hpp @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "glove_source.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +struct RevoHumanPluginConfig +{ + //! Collections are __, e.g. revohuman_left_tactile. + std::string collection_prefix = "revohuman"; + //! The aim-to-wrist offsets end at the glove's WRIST; identity until a glove mount is calibrated. + plugin_utils::WristSourceConfig wrist; + //! Set by the plugin launcher; enables device status reporting. + std::optional plugin_root_id; +}; + +//! Pushes RevoHuman gloves into OpenXR: per hand an injected hand, the encoder joint states, and the taxel +//! forces with their TactileMetadata. +class RevoHumanPlugin +{ +public: + explicit RevoHumanPlugin(const RevoHumanPluginConfig& config); + ~RevoHumanPlugin(); + + void update(std::int64_t now_ns); + //! Call at least once a second. + void publish_status(std::int64_t now_ns); + bool finished() const + { + return source_.finished(); + } + //! See GloveSource::request_tare(). + void request_tare() + { + source_.request_tare(); + } + +private: + class HandOutput; + + // Destroyed in reverse order: the hands go before the session. + std::shared_ptr session_; + std::unique_ptr deviceio_; + std::unique_ptr wrists_; + std::array, 2> outputs_; + std::array>, 2> pipelines_; + GloveSource source_; + std::unique_ptr status_; +}; + +} // 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..0094810cea --- /dev/null +++ b/src/plugins/revohuman/core/CMakeLists.txt @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# The hand mapping and the per-hand push pipeline, apart from OpenXR so they are unit-tested without a runtime. +add_library(revohuman_core STATIC + hand_model.cpp + hand_fit.inc + hand_points.hpp + inc/revohuman_plugin/hand_model.hpp + inc/revohuman_plugin/pipeline.hpp + inc/revohuman_plugin/sdk.hpp + inc/revohuman_plugin/types.hpp +) + +target_include_directories(revohuman_core + INTERFACE + $ +) + +target_link_libraries(revohuman_core + PUBLIC + isaacteleop_schema + log_bridge::log_bridge_core + revohuman_plugin::sdk +) + +add_library(revohuman_plugin::revohuman_core ALIAS revohuman_core) diff --git a/src/plugins/revohuman/core/hand_fit.inc b/src/plugins/revohuman/core/hand_fit.inc new file mode 100644 index 0000000000..0692f28653 --- /dev/null +++ b/src/plugins/revohuman/core/hand_fit.inc @@ -0,0 +1,32 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Fitted offsets for the PTV1 and P1 models (one table for both), in metres. The SDK's TIP points carry the tip +// extension. A finger's side shift moves its three distal points; side shifts are section midpoints, which agree within +// 0.01 mm over DIP-local Z = 20..25 mm, and the thumb's is the measured PTV1 fingertip sleeve centre. The pads are +// fitted separately. +constexpr std::array kFittedModelHashes = { + "63960181839d5486cc0952bf7a4a5c09b6e9eca78fa4035294a74e13cef32805", + "ea8e2e3a05091ba9fb229b1d932c612e2740004cfc4262a20471afebf1610d55", +}; +constexpr M kPalmBasis = { -4.3368086899420177e-19, -0.99951526706354676, 0.031132473510575541, + -0.99592691449259607, 0.0028070342275837352, 0.090120483510145774, + -0.090164189062272859, -0.031005668283909979, -0.99544415591484126 }; +constexpr V kPalmPad{ 0.018276321340487119, 0, 0 }; +constexpr V kWristPad{ 0.012600000174487119, 0, 0 }; +// Per finger in kFingers order; the thumb has no placeholder. +struct FingerFit +{ + V side, proximal_pad, placeholder_pad; +}; +constexpr std::array kFingerFits = { { + { { 0.0021043985844464278, -0.011934637433200775, -0.0021037908489140032 }, + { 0.016150847149399378, 0.002847830113737538, 0 }, + { 0.016733295878679642, 0.0029505315395890099, 0 } }, + { { 0, -0.012292507172327119, -0.0004292638094403375 }, { 0.016400000000000799, 0, 0 }, { 0.016311316751695868, 0, 0 } }, + { { 0, -0.012253194786528032, 0.0010720156357961448 }, { 0.016399999999997639, 0, 0 }, { 0.017310832018759124, 0, 0 } }, + { { -0.0027006949801747799, -0.011697995150430627, 0.0026745384806665844 }, + { 0.015979669062478725, -0.0036891972912384647, 0 }, + { 0.016417065890189413, -0.0037901783056560599, 0 } }, + { { 0.006935, 0.01201177235048925, 0 }, { 0.010, 0, 0 }, {} }, +} }; diff --git a/src/plugins/revohuman/core/hand_model.cpp b/src/plugins/revohuman/core/hand_model.cpp new file mode 100644 index 0000000000..463dd10a91 --- /dev/null +++ b/src/plugins/revohuman/core/hand_model.cpp @@ -0,0 +1,393 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/revohuman_plugin/hand_model.hpp" + +#include "hand_points.hpp" +#include "inc/revohuman_plugin/sdk.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ +namespace +{ + +// A new point set or taxel layout version may reorder points or change their rules: refit the mapping first. +static_assert(RH_HAND_POINT_COUNT == 27 && RH_HAND_POINTS_VERSION == 1); +static_assert(RH_TAXEL_LAYOUT_VERSION == 1); + +using V = std::array; +using M = std::array; // Row-major, as the SDK writes rotations. + +// Link axes point their -X to the back of the hand on every SDK model. +constexpr V kDorsal{ -1, 0, 0 }; +// WRIST axes, a relabeling of palm_link: OpenXR X = -palm Y, Y = -palm X, Z = -palm Z. Do not aim WRIST at a +// finger: every pose and taxel is expressed in this one fixed basis. +constexpr M kWristAxes{ 0, -1, 0, -1, 0, 0, 0, 0, -1 }; +const core::Pose kIdentityPose(core::Point(0, 0, 0), core::Quaternion(0, 0, 0, 1)); + +// Per finger (index, middle, ring, little, thumb): its SDK points and the OpenXR joint at its proximal pad (the +// next three joints follow it). The pad hangs from the pad point in the axes of the pad_axes link; the knuckle +// link orients the proximal joint and, on the non-thumb fingers, carries the pad with it, since their MPR link +// only spreads the finger sideways. +struct Finger +{ + SdkPoint dip, pip, knuckle, pad, pad_axes, tip; + core::HandJoint proximal; +}; +constexpr std::array kFingers = { { + { kIndexDip, kIndexPip, kIndexMcp, kIndexMpr, kIndexMpr, kIndexTip, core::HandJoint_INDEX_PROXIMAL }, + { kMiddleDip, kMiddlePip, kMiddleMcp, kMiddleMpr, kMiddleMpr, kMiddleTip, core::HandJoint_MIDDLE_PROXIMAL }, + { kRingDip, kRingPip, kRingMcp, kRingMpr, kRingMpr, kRingTip, core::HandJoint_RING_PROXIMAL }, + { kLittleDip, kLittlePip, kLittleMcp, kLittleMpr, kLittleMpr, kLittleTip, core::HandJoint_LITTLE_PROXIMAL }, + { kThumbDip, kThumbPip, kThumbCmr, kThumbCmp, kThumbCmr, kThumbTip, core::HandJoint_THUMB_METACARPAL }, +} }; +constexpr std::size_t kThumb = kFingers.size() - 1; +// The placeholders are the non-thumb metacarpals, the OpenXR joint before each proximal: the joints +// kMappedHandMask leaves out. +static_assert( + [] + { + std::uint32_t placeholders = 0; + for (std::size_t i = 0; i < kThumb; ++i) + placeholders |= 1u << (kFingers[i].proximal - 1); + return placeholders == (((1u << kHandJointCount) - 1u) & ~kMappedHandMask); + }()); +// Placeholder positions in the PALM basis. +constexpr std::array kPlaceholderAt = { + { { .018, .003, -.055 }, { .005, .002, -.055 }, { -.008, .003, -.055 }, { -.022, .005, -.05 } } +}; + +#include "hand_fit.inc" + +bool fitted_model(const std::string& sha) +{ + return std::find(kFittedModelHashes.begin(), kFittedModelHashes.end(), sha) != kFittedModelHashes.end(); +} + +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] }; +} +// a / |a|, or NaN for a vector too short to have a direction. +template +std::array unit(std::array a) +{ + double n = 0; + for (double v : a) + n += v * v; + n = std::sqrt(n); + for (double& v : a) + v = n > 1e-12 ? v / n : std::numeric_limits::quiet_NaN(); + return a; +} +V mv(const 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(const M& a, const 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(const M& a) +{ + return { a[0], a[3], a[6], a[1], a[4], a[7], a[2], a[5], a[8] }; +} +// Rotation whose Z is z and whose Y is y made orthogonal to it. +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); + const 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(const M& a) +{ + std::array q{}; + const double trace = a[0] + a[4] + a[8]; + if (trace > 0) + { + const 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; + const int j = (i + 1) % 3, k = (i + 2) % 3; + const 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; + } + return unit(q); +} +bool finite(const auto& values) +{ + return std::all_of(values.begin(), values.end(), [](double x) { return std::isfinite(x); }); +} +V mirrored(V v, bool right) +{ + if (right) + v[1] = -v[1]; + return v; +} +V position_of(const RhLinkPose& pose) +{ + return { pose.p[0], pose.p[1], pose.p[2] }; +} +M rotation_of(const RhLinkPose& pose) +{ + M r{}; + std::copy_n(pose.r, 9, r.begin()); + return r; +} + +// The OpenXR points placed by the fitted offsets (hand_fit.inc) in the frames of the SDK link poses, in the WRIST +// frame and oriented from those poses. Invalid SDK quantities are NaN, stay NaN through the arithmetic and keep the +// identity pose; the placeholders are never valid and keep their fitted poses only when @p retain_unmapped is set. +HandJointPoses map_points(Hand hand, const RhHandPoints& sdk, bool retain_unmapped) +{ + const bool right = hand == Hand::Right; + // The SDK points in palm_link; a TIP is rigid on its finger's DIP link. + const V palm = position_of(sdk.points[kPalmLink]); + const M to_palm = transpose(rotation_of(sdk.points[kPalmLink])); + std::array p{}; + std::array r{}; + for (std::size_t i = 0; i < p.size(); ++i) + p[i] = mv(to_palm, sub(position_of(sdk.points[i]), palm)); + for (std::size_t i = 0; i < r.size(); ++i) + r[i] = mul(to_palm, rotation_of(sdk.points[i])); + // The PALM axes in palm_link; the right hand mirrors palm Y and OpenXR X. + M palm_basis = kPalmBasis; + if (right) + for (int i : { 0, 4, 5, 6 }) + palm_basis[i] = -palm_basis[i]; + + std::array points{}; + std::array frames{}; + // Offset from the tracker origin, not palm_link. + points[core::HandJoint_PALM] = add(mv(to_palm, scale(palm, -1)), kPalmPad); + points[core::HandJoint_WRIST] = kWristPad; + frames[core::HandJoint_PALM] = palm_basis; + frames[core::HandJoint_WRIST] = kWristAxes; + for (std::size_t i = 0; i < kFingers.size(); ++i) + { + const Finger& f = kFingers[i]; + const FingerFit& fit = kFingerFits[i]; + const M& driver = r[f.pad_axes]; + const auto shifted = [&](const V& at, const V& offset) { return add(at, mv(driver, mirrored(offset, right))); }; + const V pad = shifted(p[f.pad], fit.proximal_pad); + points[f.proximal] = + i == kThumb ? pad : add(p[f.knuckle], mv(mul(r[f.knuckle], transpose(driver)), sub(pad, p[f.knuckle]))); + points[f.proximal + 1] = shifted(p[f.pip], fit.side); + points[f.proximal + 2] = shifted(p[f.dip], fit.side); + points[f.proximal + 3] = shifted(p[f.tip], fit.side); + const V tip_dir = unit(sub(p[f.tip], p[f.dip])); + if (i != kThumb) + { + V at = kPlaceholderAt[i]; + if (right) + at[0] = -at[0]; + points[f.proximal - 1] = add(mv(palm_basis, at), mirrored(fit.placeholder_pad, right)); + frames[f.proximal - 1] = palm_basis; + } + frames[f.proximal + 2] = frames[f.proximal + 3] = frame(scale(tip_dir, -1), mv(r[f.dip], kDorsal)); + } + for (const Finger& f : kFingers) + { + frames[f.proximal] = frame(sub(points[f.proximal], points[f.proximal + 1]), mv(r[f.knuckle], kDorsal)); + frames[f.proximal + 1] = frame(sub(points[f.proximal + 1], points[f.proximal + 2]), mv(r[f.pip], kDorsal)); + } + HandJointPoses out; + out.joints.fill(kIdentityPose); + const M inv = transpose(frames[core::HandJoint_WRIST]); + for (std::size_t i = 0; i < kHandJointCount; ++i) + { + const V at = mv(inv, sub(points[i], points[core::HandJoint_WRIST])); + const auto q = quaternion(mul(inv, frames[i])); + const std::uint32_t mapped = kMappedHandMask & (1u << i); + if (finite(at) && (mapped || retain_unmapped)) + { + out.joints[i].mutable_position() = core::Point(at[0], at[1], at[2]); + out.position_valid_mask |= mapped; + } + if (finite(q) && (mapped || retain_unmapped)) + { + out.joints[i].mutable_orientation() = core::Quaternion(q[0], q[1], q[2], q[3]); + out.orientation_valid_mask |= mapped; + } + } + return out; +} + +// The OpenXR joint that moves with SDK point @p link; none for the MPR links, the thumb MCP/CMR/CMP, the TIPs and +// palm_link. +std::optional joint_of_link(std::uint32_t link) +{ + for (std::size_t i = 0; i < kFingers.size(); ++i) + { + const Finger& f = kFingers[i]; + if (link == f.dip) + return static_cast(f.proximal + 2); + if (link == f.pip) + return static_cast(f.proximal + 1); + if (link == f.knuckle && i != kThumb) + return f.proximal; + } + return std::nullopt; +} + +} // namespace + +HandJointPoses map_hand_points(Hand hand, const RhHandPoints& points, bool retain_unmapped) +{ + if (points.struct_size != sizeof(RhHandPoints) || points.points_version != RH_HAND_POINTS_VERSION || + points.hand != to_rh(hand)) + throw std::runtime_error("incompatible SDK hand point set"); + return map_points(hand, points, retain_unmapped); +} + +HandModel::HandModel(const std::string& product_code) +{ + if (rh_variant_hand_model(product_code.c_str(), &sdk_model_) != RH_OK) + throw std::runtime_error("the C SDK has no hand model for product '" + product_code + "': " + sdk_error()); + RhHandModelInfo info = RH_STRUCT_INIT(RhHandModelInfo); + if (rh_hand_model_info(sdk_model_, &info) != RH_OK) + throw std::runtime_error("the C SDK cannot describe the hand model of product '" + product_code + + "': " + sdk_error()); + if (info.points_version != RH_HAND_POINTS_VERSION) + throw std::runtime_error("the C SDK gives hand point set version " + std::to_string(info.points_version) + + " for product '" + product_code + "'; the plugin maps version " + + std::to_string(RH_HAND_POINTS_VERSION)); + const std::string name = sdk_text(info.name); + const std::string sha = sdk_text(info.urdf_sha256); + if (!fitted_model(sha)) + throw std::runtime_error("the plugin is not fitted to hand model '" + name + "' (URDF SHA-256 " + sha + + "); use the C SDK release the plugin pins"); + id_ = "sdk-points:" + name + ":" + sha; +} + +HandJointPoses HandModel::joint_poses(Hand hand, + const std::array& joints_rad, + std::uint32_t joints_valid_mask) const +{ + RhHandPoints sdk = RH_STRUCT_INIT(RhHandPoints); + if (rh_hand_points(sdk_model_, to_rh(hand), joints_rad.data(), joints_valid_mask, &sdk) != RH_OK) + throw std::runtime_error("the C SDK cannot compute the hand points: " + sdk_error()); + return map_points(hand, sdk, false); +} + +std::vector HandModel::tactile_metadata(Hand hand, std::uint32_t taxel_count) const +{ + const RhHand rh = to_rh(hand); + const std::array zero{}; + RhHandPoints sdk = RH_STRUCT_INIT(RhHandPoints); + if (rh_hand_points(sdk_model_, rh, zero.data(), (1u << kGloveJointCount) - 1u, &sdk) != RH_OK) + throw std::runtime_error("the C SDK cannot compute the hand points: " + sdk_error()); + auto layout = std::make_unique(); // About 19 KB: off the stack. + layout->struct_size = sizeof(RhTaxelLayout); + if (rh_hand_model_taxel_layout(sdk_model_, rh, taxel_count, layout.get()) != RH_OK) + throw std::runtime_error("the C SDK has no taxel layout for " + std::to_string(taxel_count) + + " taxels: " + sdk_error()); + isaaccapture::Logger::get("isaaccapture.plugins.revohuman.HandModel") + ->debug("{}: taxel layout {}", hand_name(hand), sdk_text(layout->source)); + + // WRIST in tracker_link, as map_points() places it. + const RhLinkPose& palm = sdk.points[kPalmLink]; + const V wrist = add(position_of(palm), mv(rotation_of(palm), kWristPad)); + const M to_wrist = transpose(mul(rotation_of(palm), kWristAxes)); + + core::LBST lbs; + core::HandSkeletonRefT skeleton; + const HandJointPoses bind = map_points(hand, sdk, true); + lbs.segment_bind_poses.assign(bind.joints.begin(), bind.joints.end()); // quaternion() normalizes. + for (std::size_t i = 0; i < kHandJointCount; ++i) + skeleton.joint_ids.push_back(static_cast(i)); + for (std::uint32_t i = 0; i < layout->count; ++i) + { + const RhTaxel& taxel = layout->taxels[i]; + const std::optional joint = joint_of_link(taxel.link); + if (!joint) + throw std::runtime_error("the C SDK puts taxel " + std::to_string(i) + " on link " + + std::to_string(taxel.link) + ", which no OpenXR joint follows"); + const RhLinkPose& link = sdk.points[taxel.link]; + const M link_rotation = rotation_of(link); + V at = mv(link_rotation, { taxel.p[0], taxel.p[1], taxel.p[2] }); + for (int k = 0; k < 3; ++k) + at[k] += link.p[k] - wrist[k]; + const V normal = unit(mv(to_wrist, mv(link_rotation, { taxel.n[0], taxel.n[1], taxel.n[2] }))); + // Shortest-arc rotation taking +Z to the outward normal; a half turn about X for -Z. + const std::array q = 1 + normal[2] >= 1e-12 ? + unit(std::array{ -normal[1], normal[0], 0, 1 + normal[2] }) : + std::array{ 1, 0, 0, 0 }; + const V p = mv(to_wrist, at); + lbs.taxel_bind_poses.emplace_back(core::Point(p[0], p[1], p[2]), core::Quaternion(q[0], q[1], q[2], q[3])); + const std::array segments{ static_cast(*joint), 0, 0, 0 }; + const std::array weights{ 1, 0, 0, 0 }; + lbs.taxel_segment_indices.emplace_back(flatbuffers::span(segments)); + lbs.taxel_segment_weights.emplace_back(flatbuffers::span(weights)); + } + core::TactileMetadataT metadata; + metadata.geometry_mapping.Set(std::move(lbs)); + metadata.segment_source.Set(std::move(skeleton)); + const auto packed = core::pack(metadata); + if (packed.buffer().size() > core::TRACKER_METADATA_MAX_SIZE) + throw std::runtime_error("TactileMetadata exceeds the tracker metadata cap"); + core::verify_tactile_metadata(*packed); + return { packed.buffer().begin(), packed.buffer().end() }; +} + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/hand_points.hpp b/src/plugins/revohuman/core/hand_points.hpp new file mode 100644 index 0000000000..3ab4dbc880 --- /dev/null +++ b/src/plugins/revohuman/core/hand_points.hpp @@ -0,0 +1,58 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Not installed: the SDK hand point names and the point mapping, for hand_model.cpp and the tests. + +#pragma once + +#include "inc/revohuman_plugin/types.hpp" + +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +//! The SDK hand points of RH_HAND_POINTS_VERSION 1 (rh_hand_point_name): the joint links (each finger from its DIP), +//! the TIPs, then the palm_link origin. +enum SdkPoint : std::uint32_t +{ + kIndexDip, + kIndexPip, + kIndexMcp, + kIndexMpr, + kMiddleDip, + kMiddlePip, + kMiddleMcp, + kMiddleMpr, + kRingDip, + kRingPip, + kRingMcp, + kRingMpr, + kLittleDip, + kLittlePip, + kLittleMcp, + kLittleMpr, + kThumbDip, + kThumbPip, + kThumbMcp, + kThumbCmr, + kThumbCmp, + kIndexTip, + kMiddleTip, + kRingTip, + kLittleTip, + kThumbTip, + kPalmLink, +}; +static_assert(kThumbCmp + 1 == RH_JOINT_COUNT && kPalmLink + 1 == RH_HAND_POINT_COUNT); + +//! The joints HandModel::joint_poses() gives for an SDK hand point set of a fitted model; the tests feed point sets +//! the SDK does not produce. Throws on an incompatible point set. @p retain_unmapped keeps the placeholders' fitted +//! poses for inspection instead of the identity. +HandJointPoses map_hand_points(Hand hand, const RhHandPoints& points, bool retain_unmapped = false); + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/hand_model.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/hand_model.hpp new file mode 100644 index 0000000000..f76ac06da2 --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/hand_model.hpp @@ -0,0 +1,49 @@ +// 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 +{ + +//! The SDK link poses of a fitted model (kFittedModelHashes) mapped onto the OpenXR joints. +class HandModel +{ +public: + //! The model of a product the SDK identified ("P1", "PTV1"). Throws std::runtime_error when the SDK + //! has no model for it or the model is not fitted. + explicit HandModel(const std::string& product_code); + + //! For the logs: SDK model name and model hash. + const std::string& id() const + { + return id_; + } + + //! The OpenXR joints in the WRIST frame. The SDK fails only on arguments the constructor already checked, + //! so a failure throws std::runtime_error. + HandJointPoses joint_poses(Hand hand, + const std::array& joints_rad, + std::uint32_t joints_valid_mask) const; + + //! Verified TactileMetadata for the SDK taxel layout: one segment per OpenXR joint bound at zero + //! joint angles, each taxel rigid on the joint that moves with its SDK link, +Z along its outward + //! normal. Throws std::runtime_error when the SDK has no layout for this model. + std::vector tactile_metadata(Hand hand, std::uint32_t taxel_count) const; + +private: + std::int32_t sdk_model_ = 0; + std::string id_; +}; + +} // 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..530351c563 --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/pipeline.hpp @@ -0,0 +1,364 @@ +// 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 +#include +#include +#include + +namespace plugins +{ +namespace revohuman +{ + +constexpr std::size_t kJointStateMaxBytes = 4096; +constexpr std::size_t kTactileMaxBytes = 16384; +// TactileDataArray: table, one 12-byte Point per taxel, slack. +static_assert(40 + 12 * kMaxTaxels + 32 <= kTactileMaxBytes, "a full glove must fit one tactile push"); + +constexpr std::int64_t kStallNs = 50'000'000; //!< No glove sample for this long removes the hand and the joint state. +//! From the first tick until the glove's first sample, at most this long, the hand exists with no valid joints, so +//! readers do not show the headset's hand tracking in its place while the glove opens. +constexpr std::int64_t kStartupHoldNs = 5'000'000'000; +constexpr std::int64_t kShapeResendNs = 1'000'000'000; +constexpr std::int64_t kHealthTimeoutNs = 500'000'000; +constexpr std::int64_t kTactileMaxAgeNs = 100'000'000; + +//! Why tactile is not live. +enum class TactileSkip +{ + None, + NoShape, //!< The SDK has no taxel layout for the glove. + NoTactile, //!< The product has no tactile sensors. + Invalid, + Uncalibrated, + CountMismatch, + Stale, +}; + +inline const char* tactile_skip_name(TactileSkip skip) +{ + constexpr const char* kNames[] = { "none", "no_shape", "no_tactile", "invalid", + "uncalibrated", "count_mismatch", "stale_sample" }; + static_assert(std::size(kNames) == static_cast(TactileSkip::Stale) + 1); + return kNames[static_cast(skip)]; +} + +struct HandHealth +{ + bool hand_active = false; + bool glove_live = false; + bool glove_seen = false; + bool wrist_tracked = false; + bool tactile_live = false; + bool tactile_seen = false; + TactileSkip tactile_skip = TactileSkip::None; +}; + +/*! + * @brief One glove between the SDK and OpenXR: joint states, the injected hand and taxel-local forces at their own + * sampling time. Single-threaded. + * + * @tparam Sink Where the pipeline delivers; times are CLOCK_MONOTONIC ns, tactile raw times the device sampling + * clock. It has push_joint_state(buffer, sample_ns), push_tactile(buffer, sample_ns, raw_ns), push_shape(buffer, + * now_ns), clear_joint_state(), clear_tactile() and clear_shape() (the tracker retains the last sample, so stale data + * must be removed explicitly), and create_hand(), which injects the hand and returns a move-only handle that removes + * it when destroyed. The handle's bool push(joints, sample_ns) places the WRIST-frame joints at the host's wrist pose + * at the sample time and returns whether it was tracked. + */ +template +class HandPipeline +{ +public: + HandPipeline(Hand hand, Sink& sink) : hand_(hand), sink_(sink) + { + } + + //! Glove samples before the first meta are dropped. Forces and layout of an earlier glove on this hand do not + //! belong to this one. + void set_meta(const HandMeta& meta) + { + meta_ = meta; + clear_tactile(); + if (meta.has_tactile && meta.tactile_metadata) + shape_push_ns_.reset(); // Pushed again before the first forces. + else + clear_shape(); + } + + void on_glove(const GloveSample& sample, std::int64_t now_ns) + { + assert(meta_ && "the source delivers HandMeta before its first glove sample"); + stream_.last_glove_ns = now_ns; + glove_seen_ = true; + // A device clock mapped slightly ahead of CLOCK_MONOTONIC must not put samples in the future. + const std::int64_t t_ns = std::min(sample.sample_mono_ns, now_ns); + + builder_.Clear(); + std::array, kGloveJointCount> joints; + for (std::size_t i = 0; i < kGloveJointCount; ++i) + joints[i] = core::CreateJointStateDirect(builder_, meta_->joint_names[i].c_str(), + static_cast(sample.joints_rad[i]), 0, 0, + ((sample.joints_valid_mask >> i) & 1u) != 0); + builder_.Finish(core::CreateJointStateOutput( + builder_, builder_.CreateVector(joints.data(), joints.size()), builder_.CreateString(meta_->device_id))); + sink_.push_joint_state({ builder_.GetBufferPointer(), builder_.GetSize() }, t_ns); + stream_.joint_state_published = true; + + if (sample.tactile && meta_->has_tactile) + { + follow_tactile_generation(sample.generation); + accept_tactile(*sample.tactile, now_ns); + } + + if (!stream_.hand) + { + if (held_hand_) // A held hand becomes the glove's. + stream_.hand = std::exchange(held_hand_, std::nullopt); + else + stream_.hand = sink_.create_hand(); + logger()->info("{}: hand injected", hand_name(hand_)); + } + stream_.wrist_tracked = stream_.hand->push(sample.joint_poses, t_ns); + } + + //! A tactile sample delivered apart from the glove samples (the SDK's split tactile transport), in sampling + //! order. A stream's tactile comes either this way or with its glove samples, never both. + void on_tactile(std::uint32_t generation, const TactileSample& tactile, std::int64_t now_ns) + { + assert(meta_ && "the source delivers HandMeta before its first tactile sample"); + follow_tactile_generation(generation); + if (meta_->has_tactile) + accept_tactile(tactile, now_ns); + } + + //! Holds the hand until the first glove sample, removes a stalled glove's hand and joint state, expires old + //! forces and resends the TactileMetadata. + void on_tick(std::int64_t now_ns) + { + hold_until_first_sample(now_ns); + if (stream_.last_glove_ns && now_ns - *stream_.last_glove_ns > kStallNs) + remove_glove_outputs("stall"); + if (stream_.tactile_published && now_ns - *stream_.clock.sample_ns > kTactileMaxAgeNs) + { + clear_tactile(); + if (stream_.tactile_skip == TactileSkip::None) + note_skip(TactileSkip::Stale, now_ns); + } + push_shape_if_due(now_ns); + } + + //! The glove was reopened: everything derived from its earlier meta and samples goes. + void reset() + { + remove_glove_outputs("glove reopened"); + clear_tactile(); + clear_shape(); + meta_.reset(); + stream_ = {}; + } + + HandHealth health(std::int64_t now_ns) const + { + HandHealth h; + h.hand_active = stream_.hand.has_value(); + h.glove_seen = glove_seen_; + h.glove_live = stream_.last_glove_ns && now_ns - *stream_.last_glove_ns <= kHealthTimeoutNs; + h.wrist_tracked = h.hand_active && stream_.wrist_tracked; + // A skipped sample does not end tactile while the last pushed forces are fresh. + h.tactile_live = stream_.tactile_published && now_ns - *stream_.clock.sample_ns <= kTactileMaxAgeNs; + h.tactile_seen = tactile_seen_; + if (meta_ && !meta_->has_tactile) + h.tactile_skip = TactileSkip::NoTactile; + else if (meta_ && !meta_->tactile_metadata) + h.tactile_skip = TactileSkip::NoShape; + else if (!h.tactile_live) + h.tactile_skip = stream_.tactile_skip; + return h; + } + +private: + //! The tactile device clock of one stream generation; a new generation can reuse its ticks. + struct TactileClock + { + std::optional last_tick; + std::int64_t tick_ms = 0; //!< Ticks unwrapped past 2^32. + std::optional sample_ns; //!< Of the last pushed forces. + std::optional raw_ns; + }; + using InjectedHand = decltype(std::declval().create_hand()); + struct Stream + { + std::optional tactile_generation; //!< Of the tactile samples only. + TactileClock clock; + std::optional last_glove_ns; + std::optional hand; //!< Pushed from the glove samples. + bool joint_state_published = false; + bool tactile_published = false; + bool wrist_tracked = false; + TactileSkip tactile_skip = TactileSkip::None; + std::optional last_skip_log_ns; + }; + + static constexpr std::int64_t kHeldPushPeriodNs = 5'000'000; //!< The glove's sample period. + + static const std::shared_ptr& logger() + { + static const auto instance = isaaccapture::Logger::get("isaaccapture.plugins.revohuman.HandPipeline"); + return instance; + } + + void hold_until_first_sample(std::int64_t now_ns) + { + if (glove_seen_) + return; + if (!first_tick_ns_) + first_tick_ns_ = now_ns; + if (now_ns - *first_tick_ns_ >= kStartupHoldNs) + { + if (held_hand_) + { + held_hand_.reset(); + logger()->info("{}: hand released, no glove sample since start-up", hand_name(hand_)); + } + return; + } + if (!held_hand_) + { + held_hand_ = sink_.create_hand(); + logger()->info("{}: hand held with no valid joints until the glove streams", hand_name(hand_)); + } + else if (now_ns - held_push_ns_ < kHeldPushPeriodNs) + return; + held_hand_->push(HandJointPoses{}, now_ns); + held_push_ns_ = now_ns; + } + + //! Only tactile samples move it: glove samples drained after a split stream's tactile can still be of the stream + //! before its restart, and would clear the restarted stream's forces and clock. + void follow_tactile_generation(std::uint32_t generation) + { + if (stream_.tactile_generation && *stream_.tactile_generation != generation) + { + clear_tactile(); + stream_.clock = {}; + } + stream_.tactile_generation = generation; + } + + void accept_tactile(const TactileSample& tactile, std::int64_t now_ns) + { + push_shape_if_due(now_ns); // Before the first forces that need it. + // Unwrap every tick, pushed or not, so the device clock stays continuous. + TactileClock& clock = stream_.clock; + if (clock.last_tick) + clock.tick_ms += static_cast(tactile.device_tick - *clock.last_tick); + else + clock.tick_ms = tactile.device_tick; + clock.last_tick = tactile.device_tick; + const std::int64_t raw_ns = clock.tick_ms * 1'000'000; + const auto t_ns = + tactile.sample_mono_ns ? std::optional(std::min(*tactile.sample_mono_ns, now_ns)) : std::nullopt; + TactileSkip skip = TactileSkip::None; + if (!meta_->tactile_metadata) + skip = TactileSkip::NoShape; + else if (!tactile.valid) + skip = TactileSkip::Invalid; + else if (!tactile.calibrated) + skip = TactileSkip::Uncalibrated; + else if (tactile.values.size() != meta_->taxel_count) + skip = TactileSkip::CountMismatch; + else if (!t_ns || now_ns - *t_ns > kTactileMaxAgeNs || (clock.sample_ns && *t_ns <= *clock.sample_ns) || + (clock.raw_ns && raw_ns <= *clock.raw_ns)) + skip = TactileSkip::Stale; + note_skip(skip, now_ns); + if (skip != TactileSkip::None) + return; + // Taxel-local normal force, in the SDK's newtons. + builder_.Clear(); + core::Point* forces = nullptr; + const auto vector = builder_.CreateUninitializedVectorOfStructs(tactile.values.size(), &forces); + for (std::size_t i = 0; i < tactile.values.size(); ++i) + forces[i] = core::Point(0.0f, 0.0f, tactile.values[i]); + builder_.Finish(core::CreateTactileDataArray(builder_, vector)); + sink_.push_tactile({ builder_.GetBufferPointer(), builder_.GetSize() }, *t_ns, raw_ns); + stream_.tactile_published = true; + clock.sample_ns = t_ns; + clock.raw_ns = raw_ns; + tactile_seen_ = true; + } + + void note_skip(TactileSkip skip, std::int64_t now_ns) + { + if (skip == stream_.tactile_skip) + return; + stream_.tactile_skip = skip; + if (skip == TactileSkip::None) + logger()->debug("{}: tactile pushed again", hand_name(hand_)); + else if (!stream_.last_skip_log_ns || now_ns - *stream_.last_skip_log_ns >= 1'000'000'000) + { + logger()->warn("{}: tactile not pushed ({})", hand_name(hand_), tactile_skip_name(skip)); + stream_.last_skip_log_ns = now_ns; + } + } + + //! The hand and the joint state come from the glove samples, and a new sample brings both back. + void remove_glove_outputs(const char* reason) + { + if (std::exchange(stream_.joint_state_published, false)) + sink_.clear_joint_state(); + if (stream_.hand) + { + stream_.hand.reset(); + logger()->info("{}: hand and joint state removed ({})", hand_name(hand_), reason); + } + } + + void clear_tactile() + { + if (std::exchange(stream_.tactile_published, false)) + sink_.clear_tactile(); + } + + void clear_shape() + { + if (std::exchange(shape_push_ns_, std::nullopt)) + sink_.clear_shape(); + } + + void push_shape_if_due(std::int64_t now_ns) + { + if (!meta_ || !meta_->has_tactile || !meta_->tactile_metadata || + (shape_push_ns_ && now_ns - *shape_push_ns_ < kShapeResendNs)) + return; + sink_.push_shape(*meta_->tactile_metadata, now_ns); + shape_push_ns_ = now_ns; + } + + Hand hand_; + Sink& sink_; + std::optional meta_; + std::optional shape_push_ns_; //!< Set while TactileMetadata is published. + std::optional first_tick_ns_; + std::optional held_hand_; //!< Held with no valid joints before the first glove sample. + std::int64_t held_push_ns_ = 0; + bool glove_seen_ = false; + bool tactile_seen_ = false; + Stream stream_; + flatbuffers::FlatBufferBuilder builder_{ kTactileMaxBytes }; +}; + +} // namespace revohuman +} // namespace plugins diff --git a/src/plugins/revohuman/core/inc/revohuman_plugin/sdk.hpp b/src/plugins/revohuman/core/inc/revohuman_plugin/sdk.hpp new file mode 100644 index 0000000000..61cc65b1b0 --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/sdk.hpp @@ -0,0 +1,52 @@ +// 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 + +namespace plugins +{ +namespace revohuman +{ + +inline RhHand to_rh(Hand hand) +{ + return hand == Hand::Left ? RH_HAND_LEFT : RH_HAND_RIGHT; +} + +//! A fixed-size C SDK text field, NUL-terminated unless it fills the field. +template +std::string sdk_text(const char (&chars)[N]) +{ + return std::string(chars, strnlen(chars, N)); +} + +//! "E1101 TransportDisconnected" for this thread's last C SDK error; the SDK's full text goes to the debug log. +inline std::string sdk_error() +{ + const RhErrorCode code = rh_last_error_code(); + std::string head; + char buf[512]; + if (rh_last_error(buf, sizeof(buf)) > 0) + { + isaaccapture::Logger::get("isaaccapture.plugins.revohuman.sdk")->debug("SDK error: {}", buf); + head = std::string(buf).substr(0, std::string(buf).find(':')); + } + if (head.empty()) + { + const char* name = rh_error_class_name(code); + head = "E" + std::to_string(code) + (name != nullptr ? std::string(" ") + name : ""); + } + return head; +} + +} // 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..6ac09f16e5 --- /dev/null +++ b/src/plugins/revohuman/core/inc/revohuman_plugin/types.hpp @@ -0,0 +1,92 @@ +// 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 + +namespace plugins +{ +namespace revohuman +{ + +//! Encoder joints per glove, in SDK order. +constexpr std::size_t kGloveJointCount = RH_JOINT_COUNT; +//! OpenXR hand joints. +constexpr std::size_t kHandJointCount = core::HandJoint_NUM_JOINTS; +//! The four non-thumb metacarpals are placeholders and never valid. +constexpr std::uint32_t kMappedHandMask = + ((1u << kHandJointCount) - 1u) & + ~((1u << core::HandJoint_INDEX_METACARPAL) | (1u << core::HandJoint_MIDDLE_METACARPAL) | + (1u << core::HandJoint_RING_METACARPAL) | (1u << core::HandJoint_LITTLE_METACARPAL)); +//! Protocol upper bound on taxels per glove. +constexpr std::size_t kMaxTaxels = RH_TACTILE_MAX_POINTS; + +enum class Hand : std::uint8_t +{ + Left = 0, + Right = 1, +}; + +constexpr std::array kHands = { Hand::Left, Hand::Right }; + +constexpr std::size_t index_of(Hand hand) +{ + return static_cast(hand); +} + +constexpr const char* hand_name(Hand hand) +{ + return hand == Hand::Left ? "left" : "right"; +} + +//! The OpenXR hand joints with per-joint validity. +struct HandJointPoses +{ + std::array joints{}; + 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::optional sample_mono_ns; + bool valid = false; + bool calibrated = false; //!< The SDK reports forces, not raw counts. + std::vector values; //!< Newtons per taxel when calibrated. +}; + +struct GloveSample +{ + std::uint32_t generation = 0; //!< Changes on every (re)start of the device stream. + std::int64_t sample_mono_ns = 0; //!< Encoder sampling time. + std::uint32_t joints_valid_mask = 0; + std::array joints_rad{}; + std::optional tactile; + HandJointPoses joint_poses; //!< WRIST frame. +}; + +struct HandMeta +{ + std::string product_code; + std::string device_id; + std::array joint_names; + //! The product has tactile sensors (a P1 glove has none). + bool has_tactile = false; + //! Serialized TactileMetadata; absent when the SDK has no taxel layout for the glove. + std::optional> tactile_metadata; + std::uint32_t taxel_count = 0; +}; + +} // 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 2a696e2d15..726b8a3a91 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 fd7e2d2e68..32611b0ca2 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -21,3 +21,8 @@ endif() if(TARGET manus_calibration_core) add_subdirectory(manus_calibration) endif() + +# Skipped with the plugin: off Linux, or on an arch with no RevoHuman C SDK release. +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..d9b3cc7f75 --- /dev/null +++ b/tests/cpp/plugins/revohuman/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_hand_model.cpp + ptv1_neutral_points.inc + test_pipeline.cpp +) + +# hand_points.hpp is private to the core library. +target_include_directories(revohuman_core_tests PRIVATE + ${CMAKE_SOURCE_DIR}/src/plugins/revohuman/core +) + +target_link_libraries(revohuman_core_tests + PRIVATE + Catch2::Catch2WithMain + revohuman_plugin::sdk + revohuman_plugin::revohuman_core + isaacteleop_schema +) + +catch_discover_tests(revohuman_core_tests) diff --git a/tests/cpp/plugins/revohuman/ptv1_neutral_points.inc b/tests/cpp/plugins/revohuman/ptv1_neutral_points.inc new file mode 100644 index 0000000000..fb61cff9a3 --- /dev/null +++ b/tests/cpp/plugins/revohuman/ptv1_neutral_points.inc @@ -0,0 +1,33 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// SDK hand point positions of the fitted PTV1 model at the neutral pose, in the tracker frame (metres). +constexpr std::array, 27> kNeutralPoints = { { + { 0.020314749347559731, -0.032418728320167858, 0.11729681821762047 }, + { 0.019542529378066911, -0.028039251246438978, 0.09167995156926928 }, + { 0.0064774040835088101, -0.023048489490109798, 0.050298859291164288 }, + { 0.0038031370338417699, -0.033220557516356095, 0.03341610691436149 }, + { 0.018276321340487119, 0.0003185352956217493, 0.1307266474625321 }, + { 0.018276321340487119, 0.0013131709516427306, 0.1022440088924878 }, + { 0.0062763213404866203, 0.0031104950318209205, 0.050775381301004396 }, + { 0.0018763213404863199, -0.00865851968996878, 0.0353552550862751 }, + { 0.018276321340486831, 0.032328601744614827, 0.11535587276575202 }, + { 0.018276321340486831, 0.030062552433175689, 0.089454810615366309 }, + { 0.0062763213404864304, 0.026402011237774199, 0.047614633295512993 }, + { 0.00187632134048919, 0.0128414803100313, 0.033743728459932801 }, + { 0.025164483509248606, 0.057661464225064116, 0.089457664532868075 }, + { 0.024186206678176961, 0.053424081732904916, 0.069936200344519889 }, + { 0.010928522971039261, 0.049343682397576805, 0.038701857643164597 }, + { 0.0032068920825053999, 0.035457435017139799, 0.026735297982570294 }, + { 0.044266211341648334, -0.06986475163036257, 0.039883348507541577 }, + { 0.044266211341648334, -0.06986475163036257, -0.00011665149246001261 }, + { 0.033873906496242126, -0.063864751630355904, -0.03811665149245861 }, + { 0.02619377947731661, -0.035658519689972398, -0.053116651492431909 }, + { 0.023506779477318201, -0.035658519689972398, -0.045816651492397407 }, + { 0.021063457640120596, -0.036664864047963847, 0.14213373159301937 }, + { 0.018276321340487119, -0.00056122453780476809, 0.15591967288067707 }, + { 0.018276321340486831, 0.034525656880468089, 0.1404683278806052 }, + { 0.026397027168433344, 0.063000197348826253, 0.1140530102868843 }, + { 0.044266211341648334, -0.06986475163036257, 0.070653087807554166 }, + { 0.005676321166, 0.0023414999999999998, -0.086147702830000006 }, +} }; diff --git a/tests/cpp/plugins/revohuman/test_hand_model.cpp b/tests/cpp/plugins/revohuman/test_hand_model.cpp new file mode 100644 index 0000000000..628e6ee3ff --- /dev/null +++ b/tests/cpp/plugins/revohuman/test_hand_model.cpp @@ -0,0 +1,245 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "hand_points.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace plugins::revohuman; +using Catch::Approx; + +namespace +{ + +constexpr std::uint32_t kAllJoints = (1u << kGloveJointCount) - 1u; + +std::array bent() +{ + std::array q{}; + for (std::size_t i = 0; i < q.size(); ++i) + q[i] = 0.05 * static_cast(i % 7); + return q; +} + +std::array position(const core::Pose& pose) +{ + return { pose.position().x(), pose.position().y(), pose.position().z() }; +} + +std::array orientation(const core::Pose& pose) +{ + return { pose.orientation().x(), pose.orientation().y(), pose.orientation().z(), pose.orientation().w() }; +} + +} // namespace + +TEST_CASE("hand model: products the SDK knows map onto the fitted models", "[unit]") +{ + CHECK_NOTHROW(HandModel("PTV1")); + CHECK_NOTHROW(HandModel("P1")); + CHECK_THROWS_AS(HandModel("auto"), std::runtime_error); +} + +TEST_CASE("hand points: the SDK names its points in SdkPoint order", "[unit]") +{ + const std::array, RH_HAND_POINT_COUNT> expected = { { + { kIndexDip, "index_DIP_Link" }, { kIndexPip, "index_PIP_Link" }, { kIndexMcp, "index_MCP_Link" }, + { kIndexMpr, "index_MPR_Link" }, { kMiddleDip, "middle_DIP_Link" }, { kMiddlePip, "middle_PIP_Link" }, + { kMiddleMcp, "middle_MCP_Link" }, { kMiddleMpr, "middle_MPR_Link" }, { kRingDip, "ring_DIP_Link" }, + { kRingPip, "ring_PIP_Link" }, { kRingMcp, "ring_MCP_Link" }, { kRingMpr, "ring_MPR_Link" }, + { kLittleDip, "little_DIP_Link" }, { kLittlePip, "little_PIP_Link" }, { kLittleMcp, "little_MCP_Link" }, + { kLittleMpr, "little_MPR_Link" }, { kThumbDip, "thumb_DIP_Link" }, { kThumbPip, "thumb_PIP_Link" }, + { kThumbMcp, "thumb_MCP_Link" }, { kThumbCmr, "thumb_CMR_Link" }, { kThumbCmp, "thumb_CMP_Link" }, + { kIndexTip, "index_TIP" }, { kMiddleTip, "middle_TIP" }, { kRingTip, "ring_TIP" }, + { kLittleTip, "little_TIP" }, { kThumbTip, "thumb_TIP" }, { kPalmLink, "palm_link" }, + } }; + for (const auto& [point, name] : expected) + { + char buf[64]; + REQUIRE(rh_hand_point_name(RH_HAND_LEFT, point, buf, sizeof(buf)) > 0); + CHECK(std::string(buf) == std::string("left_") + name); + } +} + +TEST_CASE("hand model: WRIST is the origin and only mapped joints are valid", "[unit]") +{ + for (const char* product : { "PTV1", "P1" }) + { + const auto poses = HandModel(product).joint_poses(Hand::Left, bent(), kAllJoints); + CHECK(poses.position_valid_mask == kMappedHandMask); + CHECK(poses.orientation_valid_mask == kMappedHandMask); + for (double v : position(poses.joints[core::HandJoint_WRIST])) + CHECK(v == Approx(0).margin(1e-12)); + CHECK(std::abs(orientation(poses.joints[core::HandJoint_WRIST])[3]) == Approx(1).margin(1e-12)); + for (const core::Pose& joint : poses.joints) + { + const auto p = position(joint); + const auto q = orientation(joint); + CHECK(std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]) == Approx(1).margin(1e-6)); + CHECK(std::hypot(p[0], p[1], p[2]) < 0.25); + } + } +} + +TEST_CASE("hand model: the right hand mirrors the left across OpenXR X", "[unit]") +{ + const HandModel model("PTV1"); + const auto left = model.joint_poses(Hand::Left, bent(), kAllJoints); + const auto right = model.joint_poses(Hand::Right, bent(), kAllJoints); + for (std::size_t j = 0; j < kHandJointCount; ++j) + { + const auto l = position(left.joints[j]); + const auto r = position(right.joints[j]); + CHECK(r[0] == Approx(-l[0]).margin(1e-7)); + CHECK(r[1] == Approx(l[1]).margin(1e-7)); + CHECK(r[2] == Approx(l[2]).margin(1e-7)); + } +} + +TEST_CASE("hand model: an invalid joint invalidates what it moves", "[unit]") +{ + // The index DIP moves the index distal and tip: they lose their orientation. + const auto poses = HandModel("PTV1").joint_poses(Hand::Left, bent(), kAllJoints & ~(1u << kIndexDip)); + CHECK((poses.orientation_valid_mask & (1u << core::HandJoint_INDEX_DISTAL)) == 0); + CHECK((poses.orientation_valid_mask & (1u << core::HandJoint_INDEX_TIP)) == 0); + CHECK((poses.orientation_valid_mask & (1u << core::HandJoint_MIDDLE_PROXIMAL)) != 0); +} + +TEST_CASE("hand model: TactileMetadata binds every taxel rigidly to one joint", "[unit]") +{ + const auto bytes = HandModel("PTV1").tactile_metadata(Hand::Right, 241); + const auto* metadata = flatbuffers::GetRoot(bytes.data()); + const auto* lbs = metadata->geometry_mapping_as_LBS(); + REQUIRE(lbs != nullptr); + CHECK(lbs->segment_bind_poses()->size() == kHandJointCount); + CHECK(lbs->taxel_bind_poses()->size() == 241); + for (const auto* weights : *lbs->taxel_segment_weights()) + CHECK(weights->values()->Get(0) == 1.0f); + + CHECK_THROWS_AS(HandModel("PTV1").tactile_metadata(Hand::Right, 240), std::runtime_error); + CHECK_THROWS_AS(HandModel("P1").tactile_metadata(Hand::Right, 241), std::runtime_error); +} + +namespace +{ +#include "ptv1_neutral_points.inc" + +RhHandPoints neutral_points() +{ + RhHandPoints raw = RH_STRUCT_INIT(RhHandPoints); + raw.points_version = RH_HAND_POINTS_VERSION; + raw.hand = RH_HAND_LEFT; + raw.position_valid_mask = raw.orientation_valid_mask = (1u << RH_HAND_POINT_COUNT) - 1u; + for (std::size_t i = 0; i < kNeutralPoints.size(); ++i) + { + std::copy(kNeutralPoints[i].begin(), kNeutralPoints[i].end(), raw.points[i].p); + raw.points[i].r[0] = raw.points[i].r[4] = raw.points[i].r[8] = 1; + } + return raw; +} +} // namespace + +TEST_CASE("hand fit: raw point contract", "[unit]") +{ + auto raw = neutral_points(); + CHECK_THROWS_AS(map_hand_points(Hand::Right, raw), std::runtime_error); + raw.points_version++; + CHECK_THROWS_AS(map_hand_points(Hand::Left, raw), std::runtime_error); +} + +TEST_CASE("hand fit: the wrist depth and the thumb's own fit are applied", "[unit]") +{ + const auto out = map_hand_points(Hand::Left, neutral_points()); + CHECK(out.position_valid_mask == kMappedHandMask); + CHECK(out.orientation_valid_mask == kMappedHandMask); + for (double x : position(out.joints[core::HandJoint_WRIST])) + CHECK(x == Approx(0).margin(2e-8)); + CHECK(orientation(out.joints[core::HandJoint_WRIST])[3] == Approx(1).margin(2e-8)); + // Tracker origin with its own +X pad, then re-expressed relative to the shifted WRIST. + const auto palm = position(out.joints[core::HandJoint_PALM]); + CHECK(palm[0] == Approx(.0023415).margin(2e-8)); + CHECK(palm[1] == Approx(0).margin(2e-8)); + CHECK(palm[2] == Approx(-.08614770283).margin(2e-8)); + const auto thumb = position(out.joints[core::HandJoint_THUMB_METACARPAL]); + CHECK(thumb[1] == Approx(-.01523045813683108).margin(2e-8)); + CHECK(thumb[0] == Approx(.0380000196899724).margin(2e-8)); + CHECK(thumb[2] == Approx(-.0403310513376026).margin(2e-8)); + for (core::HandJoint placeholder : { core::HandJoint_INDEX_METACARPAL, core::HandJoint_MIDDLE_METACARPAL, + core::HandJoint_RING_METACARPAL, core::HandJoint_LITTLE_METACARPAL }) + CHECK(position(out.joints[placeholder]) == std::array{}); + const auto inspected = map_hand_points(Hand::Left, neutral_points(), true); + CHECK(inspected.position_valid_mask == kMappedHandMask); + CHECK(std::abs(position(inspected.joints[core::HandJoint_INDEX_METACARPAL])[2]) > .01); +} + +TEST_CASE("hand fit: both hands mirror and rotating the tracker axes preserves the output", "[unit]") +{ + auto left = neutral_points(), right = left, moved = left; + right.hand = RH_HAND_RIGHT; + for (int i = 0; i < RH_HAND_POINT_COUNT; ++i) + { + right.points[i].p[1] *= -1; + // Tracker origin is implicit zero; rotate its axes without adding a world-space translation. + moved.points[i].p[0] = -left.points[i].p[1]; + moved.points[i].p[1] = left.points[i].p[0]; + const double rotation[9]{ 0, -1, 0, 1, 0, 0, 0, 0, 1 }; + std::copy_n(rotation, 9, moved.points[i].r); + } + const auto a = map_hand_points(Hand::Left, left, true); + const auto b = map_hand_points(Hand::Right, right, true); + const auto c = map_hand_points(Hand::Left, moved, true); + for (std::size_t i = 0; i < kHandJointCount; ++i) + for (int k = 0; k < 3; ++k) + { + CHECK(position(b.joints[i])[k] == Approx(position(a.joints[i])[k] * (k == 0 ? -1 : 1)).margin(2e-8)); + CHECK(position(c.joints[i])[k] == Approx(position(a.joints[i])[k]).margin(2e-8)); + } +} + +TEST_CASE("hand fit: SDK tip movement reaches the output and invalid poses stay invalid", "[unit]") +{ + auto raw = neutral_points(); + const auto before = map_hand_points(Hand::Left, raw); + // Simulate a terminal joint moving its rigid tip: the fitter must not rebuild it along PIP to DIP. + raw.points[kIndexTip].p[0] += .006; + raw.points[kIndexTip].p[2] -= .004; + auto after = map_hand_points(Hand::Left, raw); + const auto tip_before = position(before.joints[core::HandJoint_INDEX_TIP]); + const auto tip_after = position(after.joints[core::HandJoint_INDEX_TIP]); + CHECK(tip_after[1] - tip_before[1] == Approx(-.006).margin(2e-8)); + CHECK(tip_after[2] - tip_before[2] == Approx(.004).margin(2e-8)); + raw.points[kIndexTip].p[0] = std::numeric_limits::quiet_NaN(); + after = map_hand_points(Hand::Left, raw); + CHECK((after.position_valid_mask & (1u << core::HandJoint_INDEX_TIP)) == 0); + CHECK((after.position_valid_mask & (1u << core::HandJoint_MIDDLE_TIP)) != 0); +} + +TEST_CASE("hand fit: proximal remains MCP-carried and thumb height follows CMR", "[unit]") +{ + auto raw = neutral_points(); + const auto before = map_hand_points(Hand::Left, raw); + const double turn[9]{ 0, -1, 0, 1, 0, 0, 0, 0, 1 }; + std::copy_n(turn, 9, raw.points[kIndexMcp].r); + std::copy_n(turn, 9, raw.points[kThumbCmr].r); + const auto after = map_hand_points(Hand::Left, raw); + const auto index_before = position(before.joints[core::HandJoint_INDEX_PROXIMAL]); + const auto index_after = position(after.joints[core::HandJoint_INDEX_PROXIMAL]); + CHECK(std::hypot(index_after[0] - index_before[0], index_after[1] - index_before[1]) > .01); + const auto thumb_before = position(before.joints[core::HandJoint_THUMB_METACARPAL]); + const auto thumb_after = position(after.joints[core::HandJoint_THUMB_METACARPAL]); + CHECK(thumb_after[0] - thumb_before[0] == Approx(-.010).margin(2e-8)); + CHECK(thumb_after[1] - thumb_before[1] == Approx(.010).margin(2e-8)); + CHECK(thumb_after[2] == Approx(thumb_before[2]).margin(2e-8)); +} diff --git a/tests/cpp/plugins/revohuman/test_pipeline.cpp b/tests/cpp/plugins/revohuman/test_pipeline.cpp new file mode 100644 index 0000000000..5193333a3b --- /dev/null +++ b/tests/cpp/plugins/revohuman/test_pipeline.cpp @@ -0,0 +1,446 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 BrainCo. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#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; + HandJointPoses joints; +}; + +class RecordingSink +{ +public: + //! Removes the hand when destroyed, like the plugin's handle. + class MockHand + { + public: + explicit MockHand(RecordingSink& sink) : sink_(&sink) + { + sink_->calls.push_back({ "create" }); + } + MockHand(MockHand&& other) noexcept : sink_(std::exchange(other.sink_, nullptr)) + { + } + MockHand& operator=(MockHand&& other) noexcept + { + if (this != &other) + { + release(); + sink_ = std::exchange(other.sink_, nullptr); + } + return *this; + } + ~MockHand() + { + release(); + } + bool push(const HandJointPoses& joints, std::int64_t t) + { + sink_->calls.push_back({ "hand", {}, t, 0, joints }); + return sink_->wrist_tracked; + } + + private: + void release() + { + if (sink_) + std::exchange(sink_, nullptr)->calls.push_back({ "destroy" }); + } + RecordingSink* sink_; + }; + + void push_joint_state(std::span b, std::int64_t t) + { + calls.push_back({ "joint_state", { b.begin(), b.end() }, t, t, {} }); + } + void clear_joint_state() + { + calls.push_back({ "clear_joint_state" }); + } + void push_tactile(std::span b, std::int64_t t, std::int64_t raw) + { + calls.push_back({ "tactile", { b.begin(), b.end() }, t, raw, {} }); + } + void clear_tactile() + { + calls.push_back({ "clear_tactile" }); + } + void push_shape(std::span b, std::int64_t t) + { + calls.push_back({ "shape", { b.begin(), b.end() }, t, t, {} }); + } + void clear_shape() + { + calls.push_back({ "clear_shape" }); + } + MockHand create_hand() + { + return MockHand(*this); + } + + std::vector kinds() const + { + std::vector out; + for (const auto& c : calls) + out.push_back(c.kind); + return out; + } + 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; + } + + std::vector calls; + bool wrist_tracked = false; //!< What MockHand::push() reports about the host's wrist pose. +}; + +HandMeta meta(bool tactile = true) +{ + HandMeta m; + m.product_code = tactile ? "PTV1" : "P1"; + m.device_id = "SN1"; + for (std::size_t i = 0; i < kGloveJointCount; ++i) + m.joint_names[i] = "j" + std::to_string(i); + m.has_tactile = tactile; + if (tactile) + { + m.tactile_metadata = std::vector{ 1, 2, 3 }; + m.taxel_count = 2; + } + return m; +} + +GloveSample glove(std::int64_t t_ns) +{ + GloveSample s; + s.sample_mono_ns = t_ns; + s.joints_valid_mask = 1u; + s.joints_rad[0] = 0.5; + s.joint_poses.joints[2].mutable_position() = core::Point(0.1f, 0, 0); + s.joint_poses.position_valid_mask = s.joint_poses.orientation_valid_mask = (1u << kHandJointCount) - 1u; + return s; +} + +// Newtons per taxel. +const std::vector kTwoTaxels{ 1.5f, 0.0f }; +const std::vector kOneTaxel{ 0.001f }; + +GloveSample with_tactile(GloveSample s, std::uint32_t tick, std::int64_t t_ns, std::vector newtons = kTwoTaxels) +{ + s.tactile = TactileSample{ tick, t_ns, true, true, std::move(newtons) }; + return s; +} + +struct Fixture +{ + RecordingSink sink; + HandPipeline pipeline{ Hand::Left, sink }; +}; + +} // namespace + +TEST_CASE("pipeline: joint states go out at the sample time, never ahead of now", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_glove(glove(10 * kMs), 20 * kMs); + f.pipeline.on_glove(glove(30 * kMs), 25 * kMs); + const auto states = f.sink.of("joint_state"); + REQUIRE(states.size() == 2); + CHECK(states[0].t_ns == 10 * kMs); + CHECK(states[1].t_ns == 25 * kMs); + const auto* out = flatbuffers::GetRoot(states[0].buffer.data()); + CHECK(out->device_id()->str() == "SN1"); + REQUIRE(out->joints()->size() == kGloveJointCount); + CHECK(out->joints()->Get(0)->position() == Approx(0.5)); + CHECK(out->joints()->Get(0)->valid()); + CHECK_FALSE(out->joints()->Get(1)->valid()); +} + +TEST_CASE("pipeline: the hand goes out in the WRIST frame and is removed after a stall", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta(false)); + f.sink.wrist_tracked = true; + f.pipeline.on_glove(glove(10 * kMs), 10 * kMs); + f.pipeline.on_glove(glove(15 * kMs), 15 * kMs); + CHECK(f.sink.of("create").size() == 1); + const auto hands = f.sink.of("hand"); + REQUIRE(hands.size() == 2); + CHECK(hands[0].t_ns == 10 * kMs); + CHECK(hands[0].joints.joints[2].position().x() == Approx(0.1)); // The sink places it at the host's wrist. + CHECK(f.pipeline.health(16 * kMs).wrist_tracked); + f.sink.wrist_tracked = false; + f.pipeline.on_glove(glove(20 * kMs), 20 * kMs); + CHECK_FALSE(f.pipeline.health(21 * kMs).wrist_tracked); + + f.pipeline.on_tick(20 * kMs + kStallNs + 1); + CHECK(f.sink.kinds().back() == "destroy"); + CHECK_FALSE(f.pipeline.health(80 * kMs).hand_active); +} + +TEST_CASE("pipeline: before the first glove sample the hand is held with no valid joints", "[unit]") +{ + Fixture f; + f.pipeline.on_tick(0); + f.pipeline.on_tick(1 * kMs); + f.pipeline.on_tick(5 * kMs); + CHECK(f.sink.kinds() == std::vector{ "create", "hand", "hand" }); + for (const auto& held : f.sink.of("hand")) + { + CHECK(held.joints.position_valid_mask == 0u); + CHECK(held.joints.orientation_valid_mask == 0u); + } + CHECK_FALSE(f.pipeline.health(5 * kMs).hand_active); + + // The held hand becomes the glove's, and holding ends with the first sample. + f.pipeline.set_meta(meta(false)); + f.pipeline.on_glove(glove(6 * kMs), 6 * kMs); + f.pipeline.on_tick(20 * kMs); + const auto kinds = f.sink.kinds(); + CHECK(std::count(kinds.begin(), kinds.end(), "create") == 1); + CHECK(std::count(kinds.begin(), kinds.end(), "destroy") == 0); + CHECK(f.sink.of("hand").back().joints.position_valid_mask != 0u); + CHECK(f.pipeline.health(7 * kMs).hand_active); +} + +TEST_CASE("pipeline: a held hand is released when no glove streams in time", "[unit]") +{ + Fixture f; + const std::int64_t hold = kStartupHoldNs; + f.pipeline.on_tick(10 * kMs); + f.pipeline.on_tick(10 * kMs + hold); + f.pipeline.on_tick(20 * kMs + hold); + CHECK(f.sink.kinds() == std::vector{ "create", "hand", "destroy" }); + + // A glove streaming later is injected as usual. + f.pipeline.set_meta(meta(false)); + f.pipeline.on_glove(glove(30 * kMs + hold), 30 * kMs + hold); + CHECK(f.sink.kinds().back() == "hand"); + CHECK(f.sink.of("create").size() == 2); +} + +TEST_CASE("pipeline: forces go out in newtons after their TactileMetadata, once per tick", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_glove(with_tactile(glove(10 * kMs), 7, 9 * kMs), 10 * kMs); + f.pipeline.on_glove(with_tactile(glove(15 * kMs), 7, 9 * kMs), 15 * kMs); // Same snapshot again. + const auto kinds = f.sink.kinds(); + CHECK(std::find(kinds.begin(), kinds.end(), "shape") < std::find(kinds.begin(), kinds.end(), "tactile")); + const auto tactile = f.sink.of("tactile"); + REQUIRE(tactile.size() == 1); + CHECK(tactile[0].t_ns == 9 * kMs); + CHECK(tactile[0].raw_ns == 7 * kMs); + const auto* forces = flatbuffers::GetRoot(tactile[0].buffer.data())->taxel_forces(); + REQUIRE(forces->size() == 2); + CHECK(forces->Get(0)->z() == Approx(1.5f)); + CHECK(forces->Get(0)->x() == 0.0f); + CHECK(f.pipeline.health(12 * kMs).tactile_live); + + // Forces expire after their maximum age; the metadata stays. + f.pipeline.on_tick(9 * kMs + kTactileMaxAgeNs + 1); + CHECK(f.sink.kinds().back() == "clear_tactile"); + const auto h = f.pipeline.health(200 * kMs); + CHECK_FALSE(h.tactile_live); + CHECK(h.tactile_skip == TactileSkip::Stale); +} + +TEST_CASE("pipeline: tactile stamped ahead of now goes out at now, like the joint states", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_glove(with_tactile(glove(10 * kMs), 1, 12 * kMs), 10 * kMs); + const auto tactile = f.sink.of("tactile"); + REQUIRE(tactile.size() == 1); + CHECK(tactile[0].t_ns == 10 * kMs); +} + +TEST_CASE("pipeline: unusable tactile samples are skipped with a reason", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + auto uncalibrated = with_tactile(glove(10 * kMs), 1, 10 * kMs); + uncalibrated.tactile->calibrated = false; + f.pipeline.on_glove(uncalibrated, 10 * kMs); + CHECK(f.pipeline.health(10 * kMs).tactile_skip == TactileSkip::Uncalibrated); + f.pipeline.on_glove(with_tactile(glove(20 * kMs), 2, 20 * kMs, kOneTaxel), 20 * kMs); + CHECK(f.pipeline.health(20 * kMs).tactile_skip == TactileSkip::CountMismatch); + f.pipeline.on_glove(with_tactile(glove(400 * kMs), 3, 200 * kMs), 400 * kMs); + CHECK(f.pipeline.health(400 * kMs).tactile_skip == TactileSkip::Stale); + CHECK(f.sink.of("tactile").empty()); + + // A failed board read is re-sent marked invalid; fresh forces stay live through it. + f.pipeline.on_glove(with_tactile(glove(500 * kMs), 4, 500 * kMs), 500 * kMs); + auto invalid = with_tactile(glove(505 * kMs), 4, 500 * kMs); + invalid.tactile->valid = false; + f.pipeline.on_glove(invalid, 505 * kMs); + CHECK(f.pipeline.health(505 * kMs).tactile_live); + CHECK(f.pipeline.health(700 * kMs).tactile_skip == TactileSkip::Invalid); +} + +TEST_CASE("pipeline: a glove without tactile sensors removes the earlier glove's tactile", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_glove(with_tactile(glove(10 * kMs), 1, 10 * kMs), 10 * kMs); + f.pipeline.reset(); + f.pipeline.set_meta(meta(false)); + const auto kinds = f.sink.kinds(); + CHECK(std::count(kinds.begin(), kinds.end(), "destroy") == 1); + CHECK(std::count(kinds.begin(), kinds.end(), "clear_tactile") == 1); + CHECK(kinds.back() == "clear_shape"); + CHECK(f.pipeline.health(20 * kMs).tactile_skip == TactileSkip::NoTactile); + + HandMeta no_layout = meta(); + no_layout.tactile_metadata.reset(); + f.pipeline.set_meta(no_layout); + CHECK(f.pipeline.health(20 * kMs).tactile_skip == TactileSkip::NoShape); +} + +TEST_CASE("pipeline: a reopened glove keeps nothing of its earlier meta", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_glove(with_tactile(glove(10 * kMs), 1, 10 * kMs), 10 * kMs); + f.pipeline.reset(); + CHECK(f.sink.kinds() == std::vector{ "joint_state", "shape", "tactile", "create", "hand", + "clear_joint_state", "destroy", "clear_tactile", "clear_shape" }); + // The source sets no meta for a reopened stream it cannot use: the earlier TactileMetadata is not resent and the + // earlier glove's tactile reasons are gone. + f.pipeline.on_tick(10 * kMs + kShapeResendNs + 1); + CHECK(f.sink.of("shape").size() == 1); + const auto h = f.pipeline.health(20 * kMs); + CHECK_FALSE(h.hand_active); + CHECK(h.tactile_skip == TactileSkip::None); +} + +TEST_CASE("pipeline: a new stream generation may reuse tactile ticks", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_glove(with_tactile(glove(10 * kMs), 50, 10 * kMs), 10 * kMs); + auto restarted = with_tactile(glove(20 * kMs), 5, 20 * kMs); + restarted.generation = 1; + f.pipeline.on_glove(restarted, 20 * kMs); + CHECK(f.sink.of("tactile").size() == 2); +} + +TEST_CASE("pipeline: split tactile samples go out at their own rate, between glove samples", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_glove(glove(10 * kMs), 10 * kMs); + f.pipeline.on_tactile(0, TactileSample{ 8, 8 * kMs, true, true, kTwoTaxels }, 11 * kMs); + f.pipeline.on_tactile(0, TactileSample{ 12, 12 * kMs, true, true, kTwoTaxels }, 13 * kMs); + f.pipeline.on_glove(glove(15 * kMs), 15 * kMs); + f.pipeline.on_tactile(0, TactileSample{ 13, 13 * kMs, true, true, kTwoTaxels }, 16 * kMs); + const auto kinds = f.sink.kinds(); + CHECK(std::find(kinds.begin(), kinds.end(), "shape") < std::find(kinds.begin(), kinds.end(), "tactile")); + const auto tactile = f.sink.of("tactile"); + REQUIRE(tactile.size() == 3); + CHECK(tactile[0].raw_ns == 8 * kMs); + CHECK(tactile[1].raw_ns == 12 * kMs); + CHECK(tactile[2].t_ns == 13 * kMs); + CHECK(f.pipeline.health(16 * kMs).tactile_live); + + // A health-only sample while the tactile board is unreadable: no forces, and they age out. + f.pipeline.on_tactile(0, TactileSample{ 14, std::nullopt, false, true, {} }, 17 * kMs); + CHECK(f.sink.of("tactile").size() == 3); + CHECK(f.pipeline.health(200 * kMs).tactile_skip == TactileSkip::Invalid); +} + +TEST_CASE("pipeline: split tactile of a new stream generation may reuse ticks", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_tactile(0, TactileSample{ 50, 10 * kMs, true, true, kTwoTaxels }, 10 * kMs); + f.pipeline.on_tactile(1, TactileSample{ 5, 20 * kMs, true, true, kTwoTaxels }, 20 * kMs); + const auto kinds = f.sink.kinds(); + CHECK(std::count(kinds.begin(), kinds.end(), "clear_tactile") == 1); + CHECK(f.sink.of("tactile").size() == 2); + // The glove samples of that generation do not restart it, and their copy of the latest sample is not pushed again. + auto same_generation = with_tactile(glove(25 * kMs), 5, 20 * kMs); + same_generation.generation = 1; + f.pipeline.on_glove(same_generation, 25 * kMs); + CHECK(f.sink.of("tactile").size() == 2); + CHECK(f.sink.of("clear_tactile").size() == 1); +} + +TEST_CASE("pipeline: glove samples queued before a split stream restarted leave its tactile alone", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta()); + f.pipeline.on_tactile(0, TactileSample{ 50, 10 * kMs, true, true, kTwoTaxels }, 10 * kMs); + // One poll: the restarted stream's tactile is drained before the glove samples still queued from the earlier one. + f.pipeline.on_tactile(1, TactileSample{ 5, 20 * kMs, true, true, kTwoTaxels }, 21 * kMs); + f.pipeline.on_glove(glove(19 * kMs), 21 * kMs); + CHECK(f.sink.of("clear_tactile").size() == 1); + CHECK(f.pipeline.health(21 * kMs).tactile_live); + // Its clock is kept too: the same sample is not pushed again. + f.pipeline.on_tactile(1, TactileSample{ 5, 20 * kMs, true, true, kTwoTaxels }, 22 * kMs); + CHECK(f.sink.of("tactile").size() == 2); +} + +TEST_CASE("pipeline: split tactile of a glove without tactile sensors is ignored", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta(false)); + f.pipeline.on_tactile(0, TactileSample{ 1, 10 * kMs, true, true, kTwoTaxels }, 10 * kMs); + CHECK(f.sink.of("tactile").empty()); + CHECK(f.sink.of("shape").empty()); +} + +TEST_CASE("pipeline: the joint state of a stalled glove is removed and comes back with its next sample", "[unit]") +{ + Fixture f; + f.pipeline.set_meta(meta(false)); + f.pipeline.on_glove(glove(10 * kMs), 10 * kMs); + f.pipeline.on_tick(10 * kMs + kStallNs); + CHECK(f.sink.of("clear_joint_state").empty()); + + f.pipeline.on_tick(10 * kMs + kStallNs + 1); + f.pipeline.on_tick(10 * kMs + kStallNs + 2); // Removed once. + const std::vector removed{ "joint_state", "create", "hand", "clear_joint_state", "destroy" }; + CHECK(f.sink.kinds() == removed); + + f.pipeline.on_glove(glove(100 * kMs), 100 * kMs); + const auto joint_states = f.sink.of("joint_state"); + REQUIRE(joint_states.size() == 2); + CHECK(joint_states[1].t_ns == 100 * kMs); + CHECK(f.pipeline.health(100 * kMs).hand_active); + + // A reopened glove leaves no joint state of the earlier stream. + f.pipeline.reset(); + CHECK(f.sink.of("clear_joint_state").size() == 2); + f.pipeline.reset(); + CHECK(f.sink.of("clear_joint_state").size() == 2); +} 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 a924d57084..02da1db02d 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(