diff --git a/CMakeLists.txt b/CMakeLists.txt index 521ec03c30..2b0d8c196b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -70,6 +70,7 @@ option(BUILD_PLUGIN_OAK_CAMERA "Build OAK camera plugin (requires vcpkg for Dept option(BUILD_PLUGIN_NOITOM_MOCAP "Build Noitom mocap plugin (downloads MocapApi SDK)" OFF) 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_OMNIGLOVE "Build AgiLink OmniGlove plugin (fetches a pinned SDK)" OFF) option(BUILD_PLUGIN_SHARPA_AVATAR "Build Sharpa AvatarExo Glove plugin (Linux; skips if the Avatar SDK is missing)" OFF) option(BUILD_EXAMPLES "Build examples" ON) option(BUILD_EXAMPLE_TELEOP_ROS2 "Build only the teleop_ros2 ROS 2 reference integration (e.g. for Docker)" OFF) diff --git a/docs/source/_data/devices.yaml b/docs/source/_data/devices.yaml index 92facf6c24..0070c93979 100644 --- a/docs/source/_data/devices.yaml +++ b/docs/source/_data/devices.yaml @@ -273,6 +273,26 @@ devices: url: https://wuji.tech/en/ - email: sales@wuji.tech + - id: agilink-omniglove + name: AgiLink OmniGlove + url: https://www.agilink-ai.com + group: peripheral + modes: High-fidelity hand tracking + since: main + details: + setup: + - label: OmniGlove docs + doc: /device/hand_tracking/omniglove + - label: OmniGlove Plugin + url: https://github.com/NVIDIA/IsaacCapture/tree/main/src/plugins/omniglove + requirements: + - Linux, x86_64 or aarch64 + - AgiLink OmniGlove SDK + - USB glove connection and host udev rule + acquire: + - label: AgiLink OmniGlove SDK + url: https://github.com/AgibotTech/agilink_omniglove_sdk + - 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..dae40b38d2 100644 --- a/docs/source/device/hand_tracking/index.rst +++ b/docs/source/device/hand_tracking/index.rst @@ -12,6 +12,7 @@ and hand exoskeletons. See :doc:`/device/trackers/index` for the common tracking :titlesonly: manus + omniglove wuji_glove sharpa_avatar haptikos diff --git a/docs/source/device/hand_tracking/omniglove.rst b/docs/source/device/hand_tracking/omniglove.rst new file mode 100644 index 0000000000..54e14cbd1b --- /dev/null +++ b/docs/source/device/hand_tracking/omniglove.rst @@ -0,0 +1,189 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +.. SPDX-License-Identifier: Apache-2.0 + +AgiLink OmniGlove +================= + +OmniGlove is a USB glove that measures the operator's finger skeleton. Its +Linux C++ plugin wraps the AgiLink OmniGlove SDK and publishes standard OpenXR +hand tracking for Isaac Capture. Robot-specific joint mapping runs in the +host's hand retargeter. + +.. contents:: On this page + :local: + :depth: 2 + +Hardware and prerequisites +-------------------------- + +- Linux x86_64 or aarch64 with the system libusb runtime (``libusb-1.0-0`` + on Ubuntu); the current hardware validation used x86_64. +- The Isaac Capture source-build dependencies and the + `AgiLink OmniGlove SDK `_. +- One left and one right OmniGlove for two-hand CTS, connected over USB. + USB identifiers are ``cafe:4001`` (left) and ``cafe:4002`` (right). +- Host USB permission, CloudXR and a supported XR headset. +- Optical wrist tracking, or controllers rigidly mounted to the gloves with + separately measured left/right controller-aim-to-wrist transforms. + +Data flow +--------- + +The SDK supplies ``OMNIGLOVE_STREAM_POST_DATA`` in DIRECT mode at a nominal +125 Hz: a wrist-relative skeleton with 21 landmarks. The plugin converts SDK +joint axes, combines the skeleton with a world-space wrist pose from +``plugin_utils::WristPoseSource``, and pushes each hand through its own +``plugin_utils::HandInjector``. The host reads the injected hands using the +standard ``HandsSource``. + +.. code-block:: text + + OmniGlove SDK (finger skeleton) ─┐ + ├─► OmniGlove plugin ─► OpenXR ─► HandsSource + Optical hand / controller pose ─┘ + +Installation +------------ + +Install the USB rule on the host: + +.. code-block:: bash + + ./src/plugins/omniglove/install_udev_rules.sh + +CMake retrieves the pinned SDK into the build directory and validates its +release checksums. The target architecture selects the SDK package. The SDK +runtime library and its license notices are installed with the plugin. libusb +is supplied by the operating system. + +.. code-block:: bash + + cmake -S . -B build -DBUILD_PLUGIN_OMNIGLOVE=ON + cmake --build build + cmake --install build --component omniglove + +Local SDK checkouts use CMake's standard +``-DFETCHCONTENT_SOURCE_DIR_OMNIGLOVE_SDK=/path/to/agilink_omniglove_sdk`` override. + +Running +------- + +Start CloudXR and connect the headset: + +.. code-block:: bash + + python -m isaaccapture.cloudxr.service start + +Source the runtime environment, select the wrist source +as described below, and launch the installed plugin: + +.. code-block:: bash + + source ~/.cloudxr/run/cloudxr.env + ./install/plugins/omniglove/omniglove_plugin + +Wrist source and calibration +---------------------------- + +``OMNIGLOVE_WRIST_SOURCE`` selects ``hand_tracking`` (default), ``controller`` +or ``auto``. Optical mode needs the headset to track the wrist while the glove +is worn. Controller mode uses controller aim poses and per-hand installation +offsets. Auto mode allows both sources; validate each source and the transition +before using it. + +Rigidly attach each controller using its final mount. From simultaneous +controller aim and trusted wrist poses, calculate: + +.. code-block:: text + + T_aim_to_wrist = inverse(T_world_aim) * T_world_wrist + T_world_joint = T_world_aim * T_aim_to_wrist * T_wrist_joint + +Use multiple poses to estimate the fixed transform, then verify it with new +positions and wrist rotations. Keep the controller mounted rigidly; changes +to its installation require recalibration. Each transform is a translation +in metres followed by a unit quaternion in ``(x, y, z, w)`` order: + +.. code-block:: bash + + export OMNIGLOVE_WRIST_SOURCE=controller + export OMNIGLOVE_AIM_TO_WRIST_LEFT="px,py,pz,qx,qy,qz,qw" + export OMNIGLOVE_AIM_TO_WRIST_RIGHT="px,py,pz,qx,qy,qz,qw" + +These are format placeholders; replace them with measured values for the +current mount. Controller and Auto mode reject missing transforms. Unknown +modes and invalid offsets are errors. Optical mode does not read the offsets. + +Published collections and joint contract +---------------------------------------- + +``plugin.yaml`` declares ``/hand/left`` and ``/hand/right``, both with +``type: hand_tracker``. The plugin publishes ``XR_EXT_hand_tracking`` joints; +the host records them as ``core.HandPoseRecord`` from ``hand.fbs``. + +- Each frame has ``XrHandJointLocationEXT[26]`` slots in ``XrHandJointEXT`` + order. The wrist and four joints per finger are valid: 21 required joints. +- Palm and the four non-thumb metacarpals remain invalid because the SDK + does not supply them. No neighbouring joint is copied into an absent slot. +- Valid joints have both ``POSITION_VALID`` and ``ORIENTATION_VALID`` set. + Tracking bits additionally follow ``wrist.tracked``. A valid but untracked + wrist is still published with the tracking bits cleared. +- Positions are metres and orientations are unit ``(x, y, z, w)`` + quaternions, in the base space passed to ``HandInjector``. +- OpenXR joint ``+Z`` points away from the fingertip and ``+Y`` toward the + back of each digit. SDK digit orientations receive a local Y half-turn, + followed for the thumb by a local Z rotation of -90 degrees on the left + and +90 degrees on the right to align the flexion hinge with local +X. + The right wrist basis is converted separately. Wrist and finger poses + are composed with the same world-space wrist transform. +- SDK monotonic sample times are converted into runtime ``XrTime`` through + ``XrTimeConverter``; the wrist is queried at that same time. +- Unmapped, unconverged IK, invalid bone data, non-finite poses, zero + quaternions and stale frames are withheld. A sample is pushed only once. + Each hand is reported inactive when fresh, usable glove data or a valid + wrist pose is unavailable. Tracking resumes when both are available. + +The plugin publishes the operator's skeleton. It emits no robot joint +commands or hand ``JointStateOutput`` data and defines no vendor hand schema. + +Device status and CTS +--------------------- + +The shared device status publisher reports left and right hand availability +under the configured plugin root. A glove that stops streaming reports a +disconnected hand. A glove still streaming without a usable skeleton or wrist +reports a degraded hand. A transient rejected sample retains the last usable +pose until it becomes stale. Stream configuration or startup failures and +incompatible SDK sample contracts stop the plugin with an error. + +Follow :doc:`/conformance/setup`, :doc:`/conformance/hand` and +:doc:`/conformance/submit` for recording, review and submission. + +Troubleshooting +--------------- + +- **No USB glove:** install the udev rule on the host, reconnect the device + and check user access to ``cafe:4001`` / ``cafe:4002``. +- **No world-space hand:** connect the headset, load ``cloudxr.env`` and + check that the selected wrist source is available. +- **Misplaced or rotated hand:** verify the final controller mount and its + aim-to-wrist transform, multiplication order, units and quaternion order. +- **Inactive hand:** the plugin logs ``hand inactive`` when SDK data or the + wrist becomes invalid. On recovery, ``hand active`` includes the SDK + connection generation and source sequence. Do not expect a frozen pose. +- **Persistent IK rejection:** check glove fit and SDK/firmware state; + invalid skeleton frames are intentionally withheld. +- **Header/runtime ABI mismatch:** rebuild against the selected SDK and + verify that the installed executable loads its accompanying runtime library. + +License +------- + +The plugin source is Apache-2.0. The AgiLink OmniGlove SDK is licensed under +MulanPSL-2.0; its shared library is redistributed with the plugin, together with +``LICENSE.omniglove-sdk`` and ``THIRD_PARTY_NOTICES.omniglove-sdk.md``. + +The SDK depends on libusb, licensed under LGPL-2.1-or-later. This plugin uses +the operating system's libusb and does not redistribute the SDK's bundled +libusb binary. The SDK notices describe the vendor's complete release; +only the SDK runtime library is copied into this plugin's install tree. diff --git a/docs/source/device/hand_tracking/wuji_glove.rst b/docs/source/device/hand_tracking/wuji_glove.rst index b293283d99..889ea36b00 100644 --- a/docs/source/device/hand_tracking/wuji_glove.rst +++ b/docs/source/device/hand_tracking/wuji_glove.rst @@ -108,7 +108,9 @@ controller offsets when needed: export WUJI_GLOVE_AIM_TO_WRIST_RIGHT="px,py,pz,qx,qy,qz,qw" Position values are in meters and quaternion values use ``(x, y, z, w)``. -The built-in offsets assume mirrored left/right mounts. +The built-in offsets assume mirrored left/right mounts. Unknown wrist-source +modes and malformed explicit offsets stop the plugin with an error; unset +offsets use the built-in values. Troubleshooting --------------- diff --git a/src/plugins/CMakeLists.txt b/src/plugins/CMakeLists.txt index c26d9384c2..737b8b4899 100644 --- a/src/plugins/CMakeLists.txt +++ b/src/plugins/CMakeLists.txt @@ -40,6 +40,9 @@ endif() if(BUILD_PLUGIN_WUJI_GLOVE) add_subdirectory(wuji_glove) endif() +if(BUILD_PLUGIN_OMNIGLOVE) + add_subdirectory(omniglove) +endif() # Gated: Linux + Avatar SDK under /opt/avatar-sdk. Missing SDK skips the plugin. if(BUILD_PLUGIN_SHARPA_AVATAR) add_subdirectory(sharpa_avatar) diff --git a/src/plugins/omniglove/60-omniglove-usb.rules b/src/plugins/omniglove/60-omniglove-usb.rules new file mode 100644 index 0000000000..331a24eb9c --- /dev/null +++ b/src/plugins/omniglove/60-omniglove-usb.rules @@ -0,0 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +# Grant access to USB devices with the listed runtime product IDs. +ACTION=="add|change", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ATTR{idVendor}=="cafe", ATTR{idProduct}=="4001", GROUP="plugdev", MODE="0660", TAG+="uaccess" +ACTION=="add|change", SUBSYSTEM=="usb", ENV{DEVTYPE}=="usb_device", ATTR{idVendor}=="cafe", ATTR{idProduct}=="4002", GROUP="plugdev", MODE="0660", TAG+="uaccess" diff --git a/src/plugins/omniglove/CMakeLists.txt b/src/plugins/omniglove/CMakeLists.txt new file mode 100644 index 0000000000..90d154f334 --- /dev/null +++ b/src/plugins/omniglove/CMakeLists.txt @@ -0,0 +1,42 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +include(GNUInstallDirs) +include("${CMAKE_CURRENT_LIST_DIR}/sdk.cmake") + +add_subdirectory(core) +add_subdirectory(app) + +# Install the verified target artifact; SDK linker aliases may not be in its manifest. +get_target_property(_omniglove_sdk_soname OmnigloveSDK::api IMPORTED_SONAME) +install(FILES "${OMNIGLOVE_SDK_LIBRARY}" + DESTINATION plugins/omniglove + RENAME "${_omniglove_sdk_soname}" + COMPONENT omniglove +) + +install(FILES "${OMNIGLOVE_SDK_RELEASE_ROOT}/LICENSE" + DESTINATION plugins/omniglove + RENAME LICENSE.omniglove-sdk + COMPONENT omniglove +) +install(FILES "${OMNIGLOVE_SDK_RELEASE_ROOT}/THIRD_PARTY_NOTICES.md" + DESTINATION plugins/omniglove + RENAME THIRD_PARTY_NOTICES.omniglove-sdk.md + COMPONENT omniglove +) +install(FILES + "${OMNIGLOVE_SDK_RELEASE_ROOT}/licenses/fmt-LICENSE" + "${OMNIGLOVE_SDK_RELEASE_ROOT}/licenses/wjwwood-serial-LICENSE" + DESTINATION plugins/omniglove/licenses + COMPONENT omniglove +) + +install(FILES plugin.yaml README.md 60-omniglove-usb.rules + DESTINATION plugins/omniglove + COMPONENT omniglove +) +install(PROGRAMS install_udev_rules.sh + DESTINATION plugins/omniglove + COMPONENT omniglove +) diff --git a/src/plugins/omniglove/README.md b/src/plugins/omniglove/README.md new file mode 100644 index 0000000000..b23b26a674 --- /dev/null +++ b/src/plugins/omniglove/README.md @@ -0,0 +1,12 @@ + + +# AgiLink OmniGlove + +The Linux plugin publishes OmniGlove finger tracking as OpenXR hands for +Isaac Capture. + +Installation, wrist calibration, running, licensing and troubleshooting: +[AgiLink OmniGlove documentation](https://nvidia.github.io/IsaacCapture/main/device/hand_tracking/omniglove.html). diff --git a/src/plugins/omniglove/app/CMakeLists.txt b/src/plugins/omniglove/app/CMakeLists.txt new file mode 100644 index 0000000000..43a740fb1f --- /dev/null +++ b/src/plugins/omniglove/app/CMakeLists.txt @@ -0,0 +1,15 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(omniglove_plugin main.cpp) + +target_link_libraries(omniglove_plugin PRIVATE isaac::omniglove_plugin) + +set_target_properties(omniglove_plugin PROPERTIES + INSTALL_RPATH "$ORIGIN" +) + +install(TARGETS omniglove_plugin + RUNTIME DESTINATION plugins/omniglove + COMPONENT omniglove +) diff --git a/src/plugins/omniglove/app/main.cpp b/src/plugins/omniglove/app/main.cpp new file mode 100644 index 0000000000..f6ff0e25f4 --- /dev/null +++ b/src/plugins/omniglove/app/main.cpp @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace +{ + +static_assert(ATOMIC_BOOL_LOCK_FREE == 2, "lock-free atomic bool is required for signal safety"); + +std::atomic stop_requested{ false }; + +void signal_handler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) + { + stop_requested.store(true, std::memory_order_relaxed); + } +} + +} // namespace + +int main(int argc, char** argv) +try +{ + std::string plugin_root_id = "omniglove"; + for (int index = 1; index < argc; ++index) + { + const std::string argument = argv[index]; + constexpr const char* prefix = "--plugin-root-id="; + if (argument.rfind(prefix, 0) == 0) + { + plugin_root_id = argument.substr(std::char_traits::length(prefix)); + } + } + + std::signal(SIGINT, signal_handler); + std::signal(SIGTERM, signal_handler); + + plugins::omniglove::OmniglovePlugin plugin(plugin_root_id); + while (!stop_requested.load(std::memory_order_relaxed)) + { + plugin.update(); + constexpr auto kFramePeriod = plugins::omniglove::OmniglovePlugin::kSourceInterval; + std::this_thread::sleep_for(kFramePeriod); + } + return 0; +} +catch (const std::exception& error) +{ + isaaccapture::Logger::get("isaaccapture.plugins.omniglove.main")->error("{}: {}", argv[0], error.what()); + return 1; +} +catch (...) +{ + isaaccapture::Logger::get("isaaccapture.plugins.omniglove.main")->error("{}: unknown error", argv[0]); + return 1; +} diff --git a/src/plugins/omniglove/core/CMakeLists.txt b/src/plugins/omniglove/core/CMakeLists.txt new file mode 100644 index 0000000000..b9bef1b202 --- /dev/null +++ b/src/plugins/omniglove/core/CMakeLists.txt @@ -0,0 +1,30 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +add_library(omniglove_plugin_core STATIC + omniglove_hand_conversion.cpp + omniglove_plugin.cpp + inc/omniglove_plugin/omniglove_hand_conversion.hpp + inc/omniglove_plugin/omniglove_plugin.hpp +) + +target_include_directories(omniglove_plugin_core + INTERFACE + $ +) + +target_link_libraries(omniglove_plugin_core + PRIVATE + OmnigloveSDK::api + PUBLIC + Teleop::plugin_utils + deviceio::deviceio_session + deviceio::deviceio_trackers + oxr::oxr_core + oxr::oxr_utils + OpenXR::openxr_loader + Teleop::openxr_extensions + log_bridge::log_bridge_core +) + +add_library(isaac::omniglove_plugin ALIAS omniglove_plugin_core) diff --git a/src/plugins/omniglove/core/inc/omniglove_plugin/omniglove_hand_conversion.hpp b/src/plugins/omniglove/core/inc/omniglove_plugin/omniglove_hand_conversion.hpp new file mode 100644 index 0000000000..5c3d867af0 --- /dev/null +++ b/src/plugins/omniglove/core/inc/omniglove_plugin/omniglove_hand_conversion.hpp @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include + +struct omniglove_post_data; + +namespace plugins::omniglove +{ + +// SDK STREAM_DATA_V1.md defines these POST_DATA bits; its C header has no names. +inline constexpr uint32_t kPostIkConverged = 1U << 0U; +inline constexpr uint32_t kPostBoneModelValid = 1U << 2U; + +enum class ConversionStatus +{ + NotMapped, + IkNotConverged, + BoneModelInvalid, + InvalidPose, +}; + +using HandJoints = std::array; +using ConversionResult = std::variant; + +ConversionResult convert_post_data(const omniglove_post_data& post, bool is_left); + +const char* conversion_status_name(ConversionStatus status) noexcept; + +} // namespace plugins::omniglove diff --git a/src/plugins/omniglove/core/inc/omniglove_plugin/omniglove_plugin.hpp b/src/plugins/omniglove/core/inc/omniglove_plugin/omniglove_plugin.hpp new file mode 100644 index 0000000000..a50c0f9cff --- /dev/null +++ b/src/plugins/omniglove/core/inc/omniglove_plugin/omniglove_plugin.hpp @@ -0,0 +1,107 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "omniglove_hand_conversion.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +struct omniglove_session; +struct omniglove_stream_sample; + +namespace plugins::omniglove +{ + +struct SdkSessionDeleter +{ + void operator()(omniglove_session* session) const noexcept; +}; +using SdkSession = std::unique_ptr; + +class OmniglovePlugin +{ +public: + static constexpr auto kSourceInterval = std::chrono::milliseconds(8); + + explicit OmniglovePlugin(const std::string& plugin_root_id); + ~OmniglovePlugin(); + + void update(); + +private: + struct HandFrame + { + std::array joints{}; + int64_t sample_monotonic_ns = 0; + uint64_t connection_generation = 0; + uint64_t source_sequence = 0; + }; + + struct CallbackState + { + std::optional frame; + int64_t last_received_ns = 0; + }; + + struct StreamContext + { + OmniglovePlugin* plugin; + bool is_left; + }; + + struct HandState + { + std::optional stream; + std::unique_ptr injector; + int64_t last_pushed_ns = 0; + core::PluginDeviceState state = core::PluginDeviceState_DISCONNECTED; + core::PluginDeviceReason reason = core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + }; + + static constexpr std::size_t hand_index(bool is_left) + { + return is_left ? 0 : 1; + } + + HandState& hand_state(bool is_left); + static void stream_callback(const omniglove_stream_sample* sample, void* user_data) noexcept; + + void start_device_streams(); + void on_sample(const omniglove_stream_sample& sample, bool is_left); + void pump_hand(HandState& hand, bool is_left, const CallbackState& input); + + std::shared_ptr m_logger = + isaaccapture::Logger::get("isaaccapture.plugins.omniglove.OmniglovePlugin"); + + std::unique_ptr m_session; + std::unique_ptr m_deviceio_session; + std::unique_ptr m_wrist_source; + std::optional m_time_converter; + std::optional m_status_publisher; + + std::mutex m_frame_mutex; + std::array m_hands; + std::array m_inputs; + std::exception_ptr m_callback_error; + // Destroy the SDK first: its callbacks borrow the contexts, frames and mutex above. + SdkSession m_sdk_session; +}; + +} // namespace plugins::omniglove diff --git a/src/plugins/omniglove/core/omniglove_hand_conversion.cpp b/src/plugins/omniglove/core/omniglove_hand_conversion.cpp new file mode 100644 index 0000000000..6d1e403ee4 --- /dev/null +++ b/src/plugins/omniglove/core/omniglove_hand_conversion.cpp @@ -0,0 +1,156 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/omniglove_plugin/omniglove_hand_conversion.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace plugins::omniglove +{ + +namespace +{ + +constexpr float kJointRadiusM = 0.01F; +constexpr float kSqrtHalf = 0.7071067811865476F; +constexpr XrPosef kLeftThumbRoll{ { 0.0F, 0.0F, -kSqrtHalf, kSqrtHalf }, {} }; +constexpr XrPosef kRightThumbRoll{ { 0.0F, 0.0F, kSqrtHalf, kSqrtHalf }, {} }; +constexpr XrSpaceLocationFlags kValidFlags = + XR_SPACE_LOCATION_POSITION_VALID_BIT | XR_SPACE_LOCATION_ORIENTATION_VALID_BIT; + +struct JointSource +{ + XrHandJointEXT joint; + const omniglove_pose6_t* pose; +}; + +auto joint_sources(const omniglove_post_data_t& post) +{ + return std::array{ JointSource{ XR_HAND_JOINT_WRIST_EXT, &post.wrist.landmark }, + JointSource{ XR_HAND_JOINT_THUMB_METACARPAL_EXT, &post.thumb.landmarks.cmc }, + JointSource{ XR_HAND_JOINT_THUMB_PROXIMAL_EXT, &post.thumb.landmarks.mcp }, + JointSource{ XR_HAND_JOINT_THUMB_DISTAL_EXT, &post.thumb.landmarks.ip }, + JointSource{ XR_HAND_JOINT_THUMB_TIP_EXT, &post.thumb.landmarks.tip }, + JointSource{ XR_HAND_JOINT_INDEX_PROXIMAL_EXT, &post.index.landmarks.mcp }, + JointSource{ XR_HAND_JOINT_INDEX_INTERMEDIATE_EXT, &post.index.landmarks.pip }, + JointSource{ XR_HAND_JOINT_INDEX_DISTAL_EXT, &post.index.landmarks.dip }, + JointSource{ XR_HAND_JOINT_INDEX_TIP_EXT, &post.index.landmarks.tip }, + JointSource{ XR_HAND_JOINT_MIDDLE_PROXIMAL_EXT, &post.middle.landmarks.mcp }, + JointSource{ XR_HAND_JOINT_MIDDLE_INTERMEDIATE_EXT, &post.middle.landmarks.pip }, + JointSource{ XR_HAND_JOINT_MIDDLE_DISTAL_EXT, &post.middle.landmarks.dip }, + JointSource{ XR_HAND_JOINT_MIDDLE_TIP_EXT, &post.middle.landmarks.tip }, + JointSource{ XR_HAND_JOINT_RING_PROXIMAL_EXT, &post.ring.landmarks.mcp }, + JointSource{ XR_HAND_JOINT_RING_INTERMEDIATE_EXT, &post.ring.landmarks.pip }, + JointSource{ XR_HAND_JOINT_RING_DISTAL_EXT, &post.ring.landmarks.dip }, + JointSource{ XR_HAND_JOINT_RING_TIP_EXT, &post.ring.landmarks.tip }, + JointSource{ XR_HAND_JOINT_LITTLE_PROXIMAL_EXT, &post.pinky.landmarks.mcp }, + JointSource{ XR_HAND_JOINT_LITTLE_INTERMEDIATE_EXT, &post.pinky.landmarks.pip }, + JointSource{ XR_HAND_JOINT_LITTLE_DISTAL_EXT, &post.pinky.landmarks.dip }, + JointSource{ XR_HAND_JOINT_LITTLE_TIP_EXT, &post.pinky.landmarks.tip } }; +} + +bool convert_pose(const omniglove_pose6_t& source, bool is_left, XrPosef& destination) +{ + for (float value : source.position_m) + { + if (!std::isfinite(value)) + { + return false; + } + } + for (float value : source.quaternion_xyzw) + { + if (!std::isfinite(value)) + { + return false; + } + } + + const float norm = std::sqrt( + source.quaternion_xyzw[0] * source.quaternion_xyzw[0] + source.quaternion_xyzw[1] * source.quaternion_xyzw[1] + + source.quaternion_xyzw[2] * source.quaternion_xyzw[2] + source.quaternion_xyzw[3] * source.quaternion_xyzw[3]); + if (norm < 1.0e-6F) + { + return false; + } + + destination.position = { source.position_m[0], source.position_m[1], source.position_m[2] }; + destination.orientation = { source.quaternion_xyzw[0] / norm, source.quaternion_xyzw[1] / norm, + source.quaternion_xyzw[2] / norm, source.quaternion_xyzw[3] / norm }; + + destination = plugin_utils::remap_to_openxr_basis(destination, is_left); + return true; +} + +} // namespace + +ConversionResult convert_post_data(const omniglove_post_data_t& post, bool is_left) +{ + HandJoints joints{}; + for (auto& joint : joints) + { + joint = {}; + joint.pose.orientation.w = 1.0F; + joint.radius = kJointRadiusM; + } + + if (post.mapped == 0U) + { + return ConversionStatus::NotMapped; + } + if ((post.flags & kPostIkConverged) == 0U) + { + return ConversionStatus::IkNotConverged; + } + if ((post.flags & kPostBoneModelValid) == 0U) + { + return ConversionStatus::BoneModelInvalid; + } + + for (const JointSource& source : joint_sources(post)) + { + XrHandJointLocationEXT& destination = joints[static_cast(source.joint)]; + if (!convert_pose(*source.pose, is_left, destination.pose)) + { + return ConversionStatus::InvalidPose; + } + if (source.joint != XR_HAND_JOINT_WRIST_EXT) + { + // SDK finger frames point +Z toward the tip; OpenXR points -Z. + // The SDK wrist already has proximal +Z and must not be flipped. + constexpr XrPosef kYHalfTurn{ { 0.0F, 1.0F, 0.0F, 0.0F }, { 0.0F, 0.0F, 0.0F } }; + destination.pose = oxr_utils::multiply_poses(destination.pose, kYHalfTurn); + } + if (source.joint >= XR_HAND_JOINT_THUMB_METACARPAL_EXT && source.joint <= XR_HAND_JOINT_THUMB_TIP_EXT) + { + // SDK thumb hinges use local Y; this local Z roll aligns them with OpenXR +X. + destination.pose = oxr_utils::multiply_poses(destination.pose, is_left ? kLeftThumbRoll : kRightThumbRoll); + } + destination.locationFlags = kValidFlags; + } + return joints; +} + +const char* conversion_status_name(ConversionStatus status) noexcept +{ + switch (status) + { + case ConversionStatus::NotMapped: + return "not mapped"; + case ConversionStatus::IkNotConverged: + return "IK not converged"; + case ConversionStatus::BoneModelInvalid: + return "bone model invalid"; + case ConversionStatus::InvalidPose: + return "invalid pose"; + } + return "unknown"; +} + +} // namespace plugins::omniglove diff --git a/src/plugins/omniglove/core/omniglove_plugin.cpp b/src/plugins/omniglove/core/omniglove_plugin.cpp new file mode 100644 index 0000000000..1807559f81 --- /dev/null +++ b/src/plugins/omniglove/core/omniglove_plugin.cpp @@ -0,0 +1,329 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/omniglove_plugin/omniglove_plugin.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace plugins::omniglove +{ + +namespace +{ + +constexpr auto kSourceTimeout = std::chrono::milliseconds(200); +constexpr int64_t kStaleThresholdNs = std::chrono::duration_cast(kSourceTimeout).count(); +constexpr uint32_t kCallbackQueueCapacity = 64; +constexpr XrSpaceLocationFlags kTrackedFlags = + XR_SPACE_LOCATION_POSITION_TRACKED_BIT | XR_SPACE_LOCATION_ORIENTATION_TRACKED_BIT; + +const char* hand_name(bool is_left) +{ + return is_left ? "left" : "right"; +} + +SdkSession create_sdk_session(const std::shared_ptr& logger) +{ + // Accept any SDK ABI version when its header and runtime agree. A new ABI + // number is not itself an incompatibility; required API changes fail build. + const uint32_t runtime_abi = omniglove_abi_version(); + if (runtime_abi != OMNIGLOVE_ABI_VERSION) + { + throw std::runtime_error("Omniglove header/runtime ABI mismatch: header=" + + std::to_string(OMNIGLOVE_ABI_VERSION) + " runtime=" + std::to_string(runtime_abi)); + } + logger->info( + "Omniglove ABI {}.{}.{}", (runtime_abi >> 16U) & 0xffU, (runtime_abi >> 8U) & 0xffU, runtime_abi & 0xffU); + + omniglove_session_t* session = nullptr; + omniglove_init_report_t* report = nullptr; + const omniglove_error_t error = omniglove_initialize(nullptr, &session, &report); + SdkSession owned_session(session); + const std::unique_ptr owned_report( + report, &omniglove_init_report_destroy); + + if (report != nullptr) + { + const size_t size = omniglove_init_report_message(report, nullptr, 0); + if (size > 1) + { + std::vector message(size); + omniglove_init_report_message(report, message.data(), message.size()); + logger->info("SDK discovery: {}", message.data()); + } + } + + if (error != OMNIGLOVE_ERROR_NONE || session == nullptr) + { + throw std::runtime_error(std::string("omniglove_initialize failed: ") + omniglove_error_name(error)); + } + return owned_session; +} + +} // namespace + +void SdkSessionDeleter::operator()(omniglove_session_t* session) const noexcept +{ + omniglove_session_destroy(session); +} + +OmniglovePlugin::OmniglovePlugin(const std::string& plugin_root_id) +{ + m_logger->info("initializing with root {}", plugin_root_id); + + plugin_utils::WristSourceConfig wrist_config; + wrist_config.mode = + plugin_utils::wrist_source_mode_from_env("OMNIGLOVE_WRIST_SOURCE", plugin_utils::WristSourceMode::HandTracking); + if (wrist_config.mode != plugin_utils::WristSourceMode::HandTracking) + { + wrist_config.left_aim_to_wrist = plugin_utils::pose_from_env("OMNIGLOVE_AIM_TO_WRIST_LEFT"); + wrist_config.right_aim_to_wrist = plugin_utils::pose_from_env("OMNIGLOVE_AIM_TO_WRIST_RIGHT"); + } + m_logger->info("wrist source {}", plugin_utils::to_string(wrist_config.mode)); + m_sdk_session = create_sdk_session(m_logger); + const auto wrist_requirements = plugin_utils::WristPoseSource::collect_requirements(wrist_config.mode); + + auto extensions = core::DeviceIOSession::get_required_extensions(wrist_requirements.trackers); + extensions.push_back(XR_NVX1_DEVICE_INTERFACE_BASE_EXTENSION_NAME); + extensions.insert(extensions.end(), wrist_requirements.extensions.begin(), wrist_requirements.extensions.end()); + + const auto status_extensions = plugin_utils::PluginDeviceStatusPublisher::get_required_extensions(); + extensions.insert(extensions.end(), status_extensions.begin(), status_extensions.end()); + m_session = std::make_unique("Omniglove", extensions); + const auto handles = m_session->get_handles(); + m_deviceio_session = core::DeviceIOSession::run(wrist_requirements.trackers, handles); + m_wrist_source = std::make_unique( + wrist_config, handles, m_deviceio_session.get(), wrist_requirements.controller_tracker); + m_time_converter.emplace(handles); + m_status_publisher.emplace(handles, plugin_root_id); + + start_device_streams(); + m_logger->info( + "initialized with {} glove stream(s)", + std::count_if(m_hands.begin(), m_hands.end(), [](const auto& hand) { return hand.stream.has_value(); })); +} + +OmniglovePlugin::~OmniglovePlugin() = default; + +void OmniglovePlugin::start_device_streams() +{ + const size_t count = omniglove_session_device_count(m_sdk_session.get()); + + for (size_t index = 0; index < count; ++index) + { + omniglove_device_t* device = omniglove_session_device_at(m_sdk_session.get(), index); + if (device == nullptr) + { + throw std::runtime_error("SDK returned a null glove for valid index " + std::to_string(index)); + } + const omniglove_hand_t hand = omniglove_device_hand(device); + if (hand == OMNIGLOVE_HAND_UNKNOWN) + { + m_logger->warn("ignoring glove {} because its hand side is unknown", index); + continue; + } + const bool is_left = hand == OMNIGLOVE_HAND_LEFT; + auto& hand_state = this->hand_state(is_left); + if (hand_state.stream) + { + m_logger->warn("ignoring a second {} glove", hand_name(is_left)); + continue; + } + + // Own the callback context before starting the stream, including failure cleanup. + hand_state.stream.emplace(StreamContext{ this, is_left }); + + omniglove_stream_config_t config{}; + config.struct_size = sizeof(config); + config.kind = OMNIGLOVE_STREAM_POST_DATA; + config.stream_mode = OMNIGLOVE_STREAM_MODE_DEFAULT; + config.output_mode = OMNIGLOVE_STREAM_OUTPUT_DIRECT; + config.source_interval_ms = static_cast(kSourceInterval.count()); + config.source_timeout_ms = static_cast(kSourceTimeout.count()); + config.callback = &OmniglovePlugin::stream_callback; + config.callback_user_data = &*hand_state.stream; + config.callback_queue_capacity = kCallbackQueueCapacity; + + const auto configure_error = omniglove_device_stream_configure(device, &config); + if (configure_error != OMNIGLOVE_STREAM_ERROR_NONE) + { + throw std::runtime_error(std::string("could not configure ") + hand_name(is_left) + + " POST_DATA stream: " + std::to_string(static_cast(configure_error))); + } + const auto start_error = omniglove_device_stream_start(device, OMNIGLOVE_STREAM_POST_DATA); + if (start_error != OMNIGLOVE_STREAM_ERROR_NONE) + { + throw std::runtime_error(std::string("could not start ") + hand_name(is_left) + + " POST_DATA stream: " + std::to_string(static_cast(start_error))); + } + m_logger->info("started {} glove at {} Hz", hand_name(is_left), omniglove_device_nominal_source_rate_hz(device)); + } + + if (std::none_of(m_hands.begin(), m_hands.end(), [](const auto& hand) { return hand.stream.has_value(); })) + { + throw std::runtime_error("no left or right Omniglove POST_DATA stream could be started"); + } +} + +void OmniglovePlugin::stream_callback(const omniglove_stream_sample_t* sample, void* user_data) noexcept +{ + if (sample == nullptr) + { + return; + } + auto* context = static_cast(user_data); + try + { + context->plugin->on_sample(*sample, context->is_left); + } + catch (...) + { + // Exceptions must reach update(), never cross the SDK's C callback boundary. + std::lock_guard lock(context->plugin->m_frame_mutex); + if (!context->plugin->m_callback_error) + { + context->plugin->m_callback_error = std::current_exception(); + } + } +} + +void OmniglovePlugin::on_sample(const omniglove_stream_sample_t& sample, bool is_left) +{ + // The SDK data union may only be read through the member selected by kind. + const bool post_data_present = sample.kind == OMNIGLOVE_STREAM_POST_DATA && sample.data.postdata != nullptr; + if (!post_data_present || sample.layout_version != OMNIGLOVE_STREAM_POST_LAYOUT_VERSION || + sample.sample_monotonic_ns > std::numeric_limits::max()) + { + throw std::runtime_error(std::string(hand_name(is_left)) + " SDK sample contract mismatch: kind=" + + std::to_string(sample.kind) + " layout=" + std::to_string(sample.layout_version) + + " post_data_present=" + std::to_string(post_data_present) + + " timestamp=" + std::to_string(sample.sample_monotonic_ns)); + } + + { + std::lock_guard lock(m_frame_mutex); + m_inputs[hand_index(is_left)].last_received_ns = core::os_monotonic_now_ns(); + } + auto result = convert_post_data(*sample.data.postdata, is_left); + const auto* joints = std::get_if(&result); + if (joints == nullptr) + { + m_logger->debug("{} POST_DATA rejected: {}", hand_name(is_left), + conversion_status_name(std::get(result))); + return; + } + HandFrame converted; + converted.joints = *joints; + converted.sample_monotonic_ns = static_cast(sample.sample_monotonic_ns); + converted.connection_generation = sample.connection_generation; + converted.source_sequence = sample.source_sequence; + + std::lock_guard lock(m_frame_mutex); + m_inputs[hand_index(is_left)].frame = converted; +} + +OmniglovePlugin::HandState& OmniglovePlugin::hand_state(bool is_left) +{ + return m_hands[hand_index(is_left)]; +} + +void OmniglovePlugin::pump_hand(HandState& hand, bool is_left, const CallbackState& input) +{ + const auto& frame = input.frame; + auto& injector = hand.injector; + const int64_t now_ns = core::os_monotonic_now_ns(); + const bool fresh = + frame && frame->sample_monotonic_ns <= now_ns && now_ns - frame->sample_monotonic_ns < kStaleThresholdNs; + if (!fresh) + { + if (injector) + { + m_logger->info("{} hand inactive: SDK frame stale or invalid", hand_name(is_left)); + } + injector.reset(); + const bool receiving = input.last_received_ns > 0 && now_ns - input.last_received_ns < kStaleThresholdNs; + hand.state = receiving ? core::PluginDeviceState_DEGRADED : core::PluginDeviceState_DISCONNECTED; + hand.reason = receiving ? core::PluginDeviceReason_NO_CURRENT_DATA : core::PluginDeviceReason_NO_HARDWARE_SIGNAL; + return; + } + if (frame->sample_monotonic_ns == hand.last_pushed_ns) + { + return; + } + + const XrTime time = m_time_converter->convert_monotonic_ns_to_xrtime(frame->sample_monotonic_ns); + const plugin_utils::WristSample wrist = m_wrist_source->query(is_left, time); + if (!wrist.valid) + { + if (injector) + { + m_logger->info("{} hand inactive: wrist pose unavailable", hand_name(is_left)); + } + injector.reset(); + hand.state = core::PluginDeviceState_DEGRADED; + hand.reason = core::PluginDeviceReason_NO_CURRENT_DATA; + return; + } + + if (!injector) + { + const auto handles = m_session->get_handles(); + injector = std::make_unique( + handles.instance, handles.session, is_left ? XR_HAND_LEFT_EXT : XR_HAND_RIGHT_EXT, handles.space); + m_logger->info("{} hand active: connection generation {}, source sequence {}", hand_name(is_left), + frame->connection_generation, frame->source_sequence); + } + + auto joints = frame->joints; + for (auto& joint : joints) + { + if ((joint.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT) == 0) + { + continue; + } + joint.pose = oxr_utils::multiply_poses(wrist.pose, joint.pose); + if (wrist.tracked) + { + joint.locationFlags |= kTrackedFlags; + } + } + injector->push(joints.data(), time); + hand.last_pushed_ns = frame->sample_monotonic_ns; + hand.state = core::PluginDeviceState_CONNECTED; + hand.reason = core::PluginDeviceReason_NONE; +} + +void OmniglovePlugin::update() +{ + std::array inputs; + std::exception_ptr callback_error; + { + std::lock_guard lock(m_frame_mutex); + inputs = m_inputs; + callback_error = m_callback_error; + } + if (callback_error) + { + std::rethrow_exception(callback_error); + } + m_deviceio_session->update(); + for (bool is_left : { true, false }) + { + pump_hand(hand_state(is_left), is_left, inputs[hand_index(is_left)]); + } + const auto& left = hand_state(true); + const auto& right = hand_state(false); + m_status_publisher->publish_if_changed( + { { "/hand/left", left.state, left.reason, {} }, { "/hand/right", right.state, right.reason, {} } }, + core::os_monotonic_now_ns()); +} + +} // namespace plugins::omniglove diff --git a/src/plugins/omniglove/install_udev_rules.sh b/src/plugins/omniglove/install_udev_rules.sh new file mode 100755 index 0000000000..85ba60b598 --- /dev/null +++ b/src/plugins/omniglove/install_udev_rules.sh @@ -0,0 +1,35 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +set -euo pipefail + +script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +rule_source="$script_dir/60-omniglove-usb.rules" +rule_destination="/etc/udev/rules.d/60-omniglove-usb.rules" + +if [[ -f /.dockerenv ]] || grep -Eq '(docker|containerd|kubepods)' /proc/1/cgroup 2>/dev/null; then + echo "error: install udev rules on the host, not inside a container" >&2 + exit 1 +fi +if [[ ! -f "$rule_source" ]]; then + echo "error: missing $rule_source" >&2 + exit 1 +fi +if ! command -v udevadm >/dev/null 2>&1; then + echo "error: udevadm is required" >&2 + exit 1 +fi + +if [[ -f "$rule_destination" ]] && sudo cmp -s "$rule_source" "$rule_destination"; then + echo "OmniGlove udev rule is already installed" + exit 0 +fi + +sudo install -m 0644 "$rule_source" "$rule_destination" +sudo udevadm control --reload-rules +sudo udevadm trigger --subsystem-match=usb --action=change +sudo udevadm settle + +echo "Installed $rule_destination" +echo "Unplug and reconnect the gloves, then run applications as a normal user." diff --git a/src/plugins/omniglove/plugin.yaml b/src/plugins/omniglove/plugin.yaml new file mode 100644 index 0000000000..7b0cc581e5 --- /dev/null +++ b/src/plugins/omniglove/plugin.yaml @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +name: omniglove_plugin +description: "AgiLink OmniGlove hand tracking" +command: "./omniglove_plugin" +version: "1.0.0" +devices: + - path: "/hand/left" + type: "hand_tracker" + description: "Left OmniGlove" + - path: "/hand/right" + type: "hand_tracker" + description: "Right OmniGlove" diff --git a/src/plugins/omniglove/sdk.cmake b/src/plugins/omniglove/sdk.cmake new file mode 100644 index 0000000000..e15a5c1588 --- /dev/null +++ b/src/plugins/omniglove/sdk.cmake @@ -0,0 +1,89 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +if(NOT CMAKE_SYSTEM_NAME STREQUAL "Linux") + message(FATAL_ERROR "The OmniGlove SDK supports Linux only") +endif() +if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(x86_64|AMD64)$") + set(_omniglove_platform linux-x86_64) +elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^(aarch64|arm64)$") + set(_omniglove_platform linux-aarch64) +else() + message(FATAL_ERROR "Unsupported OmniGlove SDK architecture: ${CMAKE_SYSTEM_PROCESSOR}") +endif() + +include(FetchContent) +FetchContent_Declare(omniglove_sdk + GIT_REPOSITORY https://github.com/AgibotTech/agilink_omniglove_sdk.git + GIT_TAG a2f899cd45c51ae01c9bbca9b893e4afba41c0d7 # v1.0.0 + # This deliberately absent subdirectory prevents running the vendor's CMake. + SOURCE_SUBDIR isaac-prebuilt-sdk +) +FetchContent_MakeAvailable(omniglove_sdk) +file(REAL_PATH "${omniglove_sdk_SOURCE_DIR}" _omniglove_sdk_root) + +if(NOT EXISTS "${_omniglove_sdk_root}/SHA256SUMS" OR NOT EXISTS "${_omniglove_sdk_root}/VERSION") + message(FATAL_ERROR "Not a complete OmniGlove SDK release: ${_omniglove_sdk_root}") +endif() +# The pinned commit selects the managed release; its checksums detect corruption. +file(STRINGS "${_omniglove_sdk_root}/SHA256SUMS" _omniglove_checksums) +if(NOT _omniglove_checksums) + message(FATAL_ERROR "Empty OmniGlove SDK checksum manifest") +endif() +set(_omniglove_verified_files) +foreach(_omniglove_entry IN LISTS _omniglove_checksums) + if(NOT _omniglove_entry MATCHES "^([0-9a-fA-F]+) (.+)$") + message(FATAL_ERROR "Malformed OmniGlove SDK checksum entry: ${_omniglove_entry}") + endif() + set(_omniglove_expected "${CMAKE_MATCH_1}") + string(LENGTH "${_omniglove_expected}" _omniglove_digest_length) + if(NOT _omniglove_digest_length EQUAL 64) + message(FATAL_ERROR "Invalid OmniGlove SDK SHA256 digest: ${_omniglove_entry}") + endif() + file(REAL_PATH "${_omniglove_sdk_root}/${CMAKE_MATCH_2}" _omniglove_file) + cmake_path(IS_PREFIX _omniglove_sdk_root "${_omniglove_file}" NORMALIZE _omniglove_inside_root) + if(NOT _omniglove_inside_root) + message(FATAL_ERROR "OmniGlove SDK checksum path escapes release root: ${_omniglove_file}") + endif() + if(NOT EXISTS "${_omniglove_file}") + message(FATAL_ERROR "Missing OmniGlove SDK release file: ${_omniglove_file}") + endif() + file(SHA256 "${_omniglove_file}" _omniglove_actual) + if(NOT _omniglove_actual STREQUAL _omniglove_expected) + message(FATAL_ERROR "OmniGlove SDK checksum mismatch: ${_omniglove_file}") + endif() + list(APPEND _omniglove_verified_files "${_omniglove_file}") +endforeach() + +function(omniglove_require_verified_file path) + if(NOT EXISTS "${path}") + message(FATAL_ERROR "Missing OmniGlove SDK release file: ${path}") + endif() + file(REAL_PATH "${path}" _omniglove_required_file) + if(NOT _omniglove_required_file IN_LIST _omniglove_verified_files) + message(FATAL_ERROR "OmniGlove SDK file is not covered by SHA256SUMS: ${path}") + endif() +endfunction() + +foreach(_omniglove_required IN ITEMS + VERSION LICENSE THIRD_PARTY_NOTICES.md + licenses/fmt-LICENSE licenses/wjwwood-serial-LICENSE + "${_omniglove_platform}/cmake/OmnigloveSDKConfig.cmake" + "${_omniglove_platform}/cmake/OmnigloveSDKConfigVersion.cmake" + "${_omniglove_platform}/include/omniglove/omniglove.h" +) + omniglove_require_verified_file("${_omniglove_sdk_root}/${_omniglove_required}") +endforeach() + +set(OmnigloveSDK_DIR "${_omniglove_sdk_root}/${_omniglove_platform}/cmake") + +# Tests outside this plugin directory also consume the imported SDK target. +find_package(OmnigloveSDK CONFIG REQUIRED GLOBAL + PATHS "${_omniglove_sdk_root}/${_omniglove_platform}/cmake" + NO_DEFAULT_PATH +) +get_target_property(OMNIGLOVE_SDK_LIBRARY OmnigloveSDK::api IMPORTED_LOCATION) +set(OMNIGLOVE_SDK_RELEASE_ROOT "${_omniglove_sdk_root}") +omniglove_require_verified_file("${OMNIGLOVE_SDK_LIBRARY}") +find_library(_omniglove_system_libusb NAMES usb-1.0 libusb-1.0.so.0 REQUIRED) +message(STATUS "OmniGlove SDK ${OmnigloveSDK_VERSION}; system libusb: ${_omniglove_system_libusb}") diff --git a/src/plugins/plugin_utils/CMakeLists.txt b/src/plugins/plugin_utils/CMakeLists.txt index a1d0f4c452..d45e25bde4 100644 --- a/src/plugins/plugin_utils/CMakeLists.txt +++ b/src/plugins/plugin_utils/CMakeLists.txt @@ -8,6 +8,9 @@ add_library(teleop_plugin_utils STATIC plugin_device_status_publisher.cpp inc/plugin_utils/plugin_device_status_publisher.hpp wrist_pose_source.cpp + wrist_source_config.cpp + inc/plugin_utils/wrist_source_config.hpp + inc/plugin_utils/hand_pose_basis.hpp inc/plugin_utils/wrist_pose_source.hpp ) diff --git a/src/plugins/plugin_utils/inc/plugin_utils/hand_pose_basis.hpp b/src/plugins/plugin_utils/inc/plugin_utils/hand_pose_basis.hpp new file mode 100644 index 0000000000..6a1811fba2 --- /dev/null +++ b/src/plugins/plugin_utils/inc/plugin_utils/hand_pose_basis.hpp @@ -0,0 +1,25 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Wuji Technology. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace plugin_utils +{ + +// SDK right-hand +Y is palmar; conjugate by a Z half-turn for OpenXR dorsal +Y. +constexpr XrPosef remap_to_openxr_basis(XrPosef pose, bool is_left) +{ + if (!is_left) + { + pose.position.x = -pose.position.x; + pose.position.y = -pose.position.y; + pose.orientation.x = -pose.orientation.x; + pose.orientation.y = -pose.orientation.y; + } + return pose; +} + +} // namespace plugin_utils diff --git a/src/plugins/plugin_utils/inc/plugin_utils/wrist_pose_source.hpp b/src/plugins/plugin_utils/inc/plugin_utils/wrist_pose_source.hpp index 24645e13b9..48bb3546a5 100644 --- a/src/plugins/plugin_utils/inc/plugin_utils/wrist_pose_source.hpp +++ b/src/plugins/plugin_utils/inc/plugin_utils/wrist_pose_source.hpp @@ -5,6 +5,8 @@ // XR_MNDX_xdev_space preferred, controller aim pose as the fallback. #pragma once +#include "wrist_source_config.hpp" + #include #include #include @@ -18,27 +20,6 @@ namespace plugin_utils { -/** @brief Which device provides the wrist pose. */ -enum class WristSourceMode -{ - Auto, //!< Optical hand tracking preferred, controller aim pose as fallback. - HandTracking, //!< XR_MNDX_xdev_space optical hand tracking only. - Controller, //!< Controller aim pose (plus the configured rigid offset) only. -}; - -/** @brief Source selection plus per-device tuning. */ -struct WristSourceConfig -{ - WristSourceMode mode = WristSourceMode::Auto; - // Rigid transform from the controller aim pose to the wrist joint, per - // hand. Depends on the controller model and how it is mounted on the - // glove, so each plugin supplies its own calibrated pair (see - // kLeft/kRightAimToWrist in wuji_glove_plugin.cpp and - // kLeft/kRightHandOffset in manus_hand_tracking_plugin.cpp). - XrPosef left_aim_to_wrist{ { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, 0.0f } }; - XrPosef right_aim_to_wrist{ { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, 0.0f } }; -}; - /** @brief One wrist query result, in the session base space. */ struct WristSample { diff --git a/src/plugins/plugin_utils/inc/plugin_utils/wrist_source_config.hpp b/src/plugins/plugin_utils/inc/plugin_utils/wrist_source_config.hpp new file mode 100644 index 0000000000..eadcd8324a --- /dev/null +++ b/src/plugins/plugin_utils/inc/plugin_utils/wrist_source_config.hpp @@ -0,0 +1,45 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Wuji Technology. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +namespace plugin_utils +{ + +/** @brief Which device provides the wrist pose. */ +enum class WristSourceMode +{ + Auto, //!< Optical hand tracking preferred, controller aim pose as fallback. + HandTracking, //!< XR_MNDX_xdev_space optical hand tracking only. + Controller, //!< Controller aim pose (plus the configured rigid offset) only. +}; + +/** @brief Source selection plus per-device tuning. */ +struct WristSourceConfig +{ + WristSourceMode mode = WristSourceMode::Auto; + // Rigid transform from the controller aim pose to the wrist joint, per + // hand. Depends on the controller model and how it is mounted on the + // glove, so each plugin supplies its own calibrated pair (see + // kLeft/kRightAimToWrist in wuji_glove_plugin.cpp and + // kLeft/kRightHandOffset in manus_hand_tracking_plugin.cpp). + XrPosef left_aim_to_wrist{ { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, 0.0f } }; + XrPosef right_aim_to_wrist{ { 0.0f, 0.0f, 0.0f, 1.0f }, { 0.0f, 0.0f, 0.0f } }; +}; + +/** @brief Read a wrist mode; an unset variable uses fallback, an unknown value throws. */ +WristSourceMode wrist_source_mode_from_env(const char* name, WristSourceMode fallback); + +/** @brief Read and normalize px,py,pz,qx,qy,qz,qw; missing or invalid values throw. */ +XrPosef pose_from_env(const char* name); + +/** @brief Use fallback only for an unset variable; invalid values throw. */ +XrPosef pose_from_env(const char* name, const XrPosef& fallback); + +/** @brief The canonical environment-variable spelling of a wrist mode. */ +const char* to_string(WristSourceMode mode); + +} // namespace plugin_utils diff --git a/src/plugins/plugin_utils/wrist_source_config.cpp b/src/plugins/plugin_utils/wrist_source_config.cpp new file mode 100644 index 0000000000..793dc58fa7 --- /dev/null +++ b/src/plugins/plugin_utils/wrist_source_config.cpp @@ -0,0 +1,88 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/plugin_utils/wrist_source_config.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace plugin_utils +{ + +const char* to_string(WristSourceMode mode) +{ + switch (mode) + { + case WristSourceMode::Auto: + return "auto"; + case WristSourceMode::HandTracking: + return "hand_tracking"; + case WristSourceMode::Controller: + return "controller"; + } + throw std::invalid_argument("invalid wrist source mode"); +} + +WristSourceMode wrist_source_mode_from_env(const char* name, WristSourceMode fallback) +{ + const char* value = std::getenv(name); + if (value == nullptr || *value == '\0') + { + return fallback; + } + for (const auto mode : { WristSourceMode::HandTracking, WristSourceMode::Controller, WristSourceMode::Auto }) + { + if (std::strcmp(value, to_string(mode)) == 0) + { + return mode; + } + } + throw std::runtime_error(std::string(name) + " must be hand_tracking, controller or auto; got '" + value + "'"); +} + +XrPosef pose_from_env(const char* name) +{ + const char* value = std::getenv(name); + if (value == nullptr || *value == '\0') + { + throw std::runtime_error(std::string(name) + " is required"); + } + XrPosef pose{}; + char trailing; + if (std::sscanf(value, "%f,%f,%f,%f,%f,%f,%f %c", &pose.position.x, &pose.position.y, &pose.position.z, + &pose.orientation.x, &pose.orientation.y, &pose.orientation.z, &pose.orientation.w, &trailing) != 7) + { + throw std::runtime_error(std::string(name) + " must be px,py,pz,qx,qy,qz,qw"); + } + const float values[] = { pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, + pose.orientation.y, pose.orientation.z, pose.orientation.w }; + if (!std::all_of(std::begin(values), std::end(values), [](float component) { return std::isfinite(component); })) + { + throw std::runtime_error(std::string(name) + " contains a non-finite value"); + } + const double norm = std::hypot(std::hypot(static_cast(pose.orientation.x), pose.orientation.y), + std::hypot(static_cast(pose.orientation.z), pose.orientation.w)); + if (norm < 1.0e-6) + { + throw std::runtime_error(std::string(name) + " contains a zero quaternion"); + } + pose.orientation.x = static_cast(pose.orientation.x / norm); + pose.orientation.y = static_cast(pose.orientation.y / norm); + pose.orientation.z = static_cast(pose.orientation.z / norm); + pose.orientation.w = static_cast(pose.orientation.w / norm); + return pose; +} + +XrPosef pose_from_env(const char* name, const XrPosef& fallback) +{ + const char* value = std::getenv(name); + return value == nullptr || *value == '\0' ? fallback : pose_from_env(name); +} + +} // namespace plugin_utils diff --git a/src/plugins/wuji_glove/wuji_glove_plugin.cpp b/src/plugins/wuji_glove/wuji_glove_plugin.cpp index 4154af8f28..da87050e09 100644 --- a/src/plugins/wuji_glove/wuji_glove_plugin.cpp +++ b/src/plugins/wuji_glove/wuji_glove_plugin.cpp @@ -5,6 +5,7 @@ #include #include +#include #include #include @@ -77,35 +78,11 @@ constexpr auto kStaleThreshold = std::chrono::milliseconds(200); constexpr auto kDiscoveryInterval = std::chrono::seconds(1); constexpr auto kShutdownPollInterval = std::chrono::milliseconds(100); -// Re-express a wrist-relative joint pose from the Wuji skeleton basis in the -// OpenXR hand-joint basis. -// -// The SDK skeleton poses are FK link frames of wuji-sdk's default hand URDFs: -// the bone runs along -Z (matching XR_EXT_hand_tracking's "+Z points away from -// the fingertip"), and the palmar side is +Y on the right hand / -Y on the -// left (the left URDF is Y-mirrored). OpenXR puts the dorsal side at +Y for -// both hands, so the left hand already matches and the right hand differs by -// a 180° rotation about Z — whose conjugation simply negates the x and y -// components of both the position and the quaternion. -constexpr XrPosef remap_to_openxr_basis(const XrPosef& pose, bool is_left) -{ - if (is_left) - { - return pose; - } - XrPosef out = pose; - out.position.x = -pose.position.x; - out.position.y = -pose.position.y; - out.orientation.x = -pose.orientation.x; - out.orientation.y = -pose.orientation.y; - return out; -} - constexpr bool remap_to_openxr_basis_is_correct() { constexpr XrPosef input{ { 1.0f, 2.0f, 3.0f, 4.0f }, { 5.0f, 6.0f, 7.0f } }; - constexpr XrPosef left = remap_to_openxr_basis(input, true); - constexpr XrPosef right = remap_to_openxr_basis(input, false); + constexpr XrPosef left = plugin_utils::remap_to_openxr_basis(input, true); + constexpr XrPosef right = plugin_utils::remap_to_openxr_basis(input, false); return left.orientation.x == 1.0f && left.orientation.y == 2.0f && left.orientation.z == 3.0f && left.orientation.w == 4.0f && left.position.x == 5.0f && left.position.y == 6.0f && left.position.z == 7.0f && right.orientation.x == -1.0f && right.orientation.y == -2.0f && @@ -146,76 +123,13 @@ bool convert_skeleton(const WujiHandSkeleton* frame, const XrPosef sdk_pose{ XrQuaternionf{ src.pose.orientation.x, src.pose.orientation.y, src.pose.orientation.z, src.pose.orientation.w }, XrVector3f{ src.pose.position[0], src.pose.position[1], src.pose.position[2] } }; - dst.pose = remap_to_openxr_basis(sdk_pose, is_left); + dst.pose = plugin_utils::remap_to_openxr_basis(sdk_pose, is_left); dst.radius = kDefaultJointRadius; dst.locationFlags = kPoseValidFlags; } return true; } -// Optional aim-to-wrist offset override. -// WUJI_GLOVE_AIM_TO_WRIST_{LEFT,RIGHT}="px,py,pz,qx,qy,qz,qw" (meters + quat). -XrPosef pose_from_env(const char* name, const XrPosef& fallback) -{ - const char* value = std::getenv(name); - if (value == nullptr || *value == '\0') - { - return fallback; - } - auto logger = isaaccapture::Logger::get("isaaccapture.plugins.wuji_glove.main"); - XrPosef pose{}; - if (std::sscanf(value, "%f,%f,%f,%f,%f,%f,%f", &pose.position.x, &pose.position.y, &pose.position.z, - &pose.orientation.x, &pose.orientation.y, &pose.orientation.z, &pose.orientation.w) != 7) - { - logger->warn("could not parse {} ('{}', want px,py,pz,qx,qy,qz,qw); using the built-in offset", name, value); - return fallback; - } - // sscanf("%f") happily accepts "nan" and "inf"; a non-finite offset would - // poison every injected joint pose, and NaN also slips past the norm check - // below (every comparison against NaN is false). - if (!std::isfinite(pose.position.x) || !std::isfinite(pose.position.y) || !std::isfinite(pose.position.z) || - !std::isfinite(pose.orientation.x) || !std::isfinite(pose.orientation.y) || - !std::isfinite(pose.orientation.z) || !std::isfinite(pose.orientation.w)) - { - logger->warn("{} ('{}') has non-finite values; using the built-in offset", name, value); - return fallback; - } - const float norm = std::sqrt(pose.orientation.x * pose.orientation.x + pose.orientation.y * pose.orientation.y + - pose.orientation.z * pose.orientation.z + pose.orientation.w * pose.orientation.w); - if (norm < 1e-6f) - { - logger->warn("{} has a zero quaternion; using the built-in offset", name); - return fallback; - } - pose.orientation.x /= norm; - pose.orientation.y /= norm; - pose.orientation.z /= norm; - pose.orientation.w /= norm; - return pose; -} - -// Wrist-source selection: WUJI_GLOVE_WRIST_SOURCE = auto (default) | -// hand_tracking | controller. -plugin_utils::WristSourceMode wrist_source_mode_from_env() -{ - const char* value = std::getenv("WUJI_GLOVE_WRIST_SOURCE"); - if (value == nullptr || *value == '\0' || std::strcmp(value, "auto") == 0) - { - return plugin_utils::WristSourceMode::Auto; - } - if (std::strcmp(value, "hand_tracking") == 0) - { - return plugin_utils::WristSourceMode::HandTracking; - } - if (std::strcmp(value, "controller") == 0) - { - return plugin_utils::WristSourceMode::Controller; - } - isaaccapture::Logger::get("isaaccapture.plugins.wuji_glove.main") - ->warn("unknown WUJI_GLOVE_WRIST_SOURCE '{}', using 'auto'", value); - return plugin_utils::WristSourceMode::Auto; -} - // Resolve the glove's hand side explicitly via the device's "hand_side" GET // (a NUL-terminated "left"/"right" string) — never inferred from frame contents. std::optional query_is_left(WujiDevice* dev) @@ -255,9 +169,10 @@ WujiGlovePlugin::WujiGlovePlugin(const std::string& plugin_root_id) noexcept(fal // OpenXR session exists for the push-device (injection) extension, the // wrist-source queries, and the XrTime base. plugin_utils::WristSourceConfig wrist_config; - wrist_config.mode = wrist_source_mode_from_env(); - wrist_config.left_aim_to_wrist = pose_from_env("WUJI_GLOVE_AIM_TO_WRIST_LEFT", kLeftAimToWrist); - wrist_config.right_aim_to_wrist = pose_from_env("WUJI_GLOVE_AIM_TO_WRIST_RIGHT", kRightAimToWrist); + wrist_config.mode = + plugin_utils::wrist_source_mode_from_env("WUJI_GLOVE_WRIST_SOURCE", plugin_utils::WristSourceMode::Auto); + wrist_config.left_aim_to_wrist = plugin_utils::pose_from_env("WUJI_GLOVE_AIM_TO_WRIST_LEFT", kLeftAimToWrist); + wrist_config.right_aim_to_wrist = plugin_utils::pose_from_env("WUJI_GLOVE_AIM_TO_WRIST_RIGHT", kRightAimToWrist); auto wrist_requirements = plugin_utils::WristPoseSource::collect_requirements(wrist_config.mode); std::vector> trackers = wrist_requirements.trackers; diff --git a/src/python/isaaccapture/cts/hand/checker/README.md b/src/python/isaaccapture/cts/hand/checker/README.md index 3d6cc25ea8..926cbf87a0 100644 --- a/src/python/isaaccapture/cts/hand/checker/README.md +++ b/src/python/isaaccapture/cts/hand/checker/README.md @@ -60,6 +60,17 @@ Labels are optional. Without them the report carries a note, the window checks a `consistency.position_orientation_same_frame` (which reads the roll windows) report that they could not be answered, and the rest are judged as usual. +## Package without the viewer + +```bash +.venv/bin/python -m hand_cts.package /path/to/take-hand.mcap --output-dir /path/to/output +``` + +This uses the viewer's bundle builder, reruns all checks, verifies the original +recording and companion files, and prints the archive's verdict and SHA-256. +It does not require the optional panel dependencies. Existing archives are never +overwritten. + ## Read the report ```text diff --git a/src/python/isaaccapture/cts/hand/checker/src/hand_cts/package.py b/src/python/isaaccapture/cts/hand/checker/src/hand_cts/package.py new file mode 100755 index 0000000000..c0ff88596c --- /dev/null +++ b/src/python/isaaccapture/cts/hand/checker/src/hand_cts/package.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +"""Check and package a hand take with the official CTS viewer's bundle builder.""" + +from __future__ import annotations + +import argparse +import hashlib +import io +import json +from pathlib import Path +import zipfile + +from cts_common.panel.bundle import companions + +from hand_cts.labels import StepTimeline +from hand_cts.mcap_source import McapFrameSource +from hand_cts.panel import bundle +from hand_cts.panel.track import HandTee +from hand_cts.report import run + + +def main(argv: list[str] | None = None) -> None: + """Build and verify one complete take without overwriting an existing archive.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("recording", type=Path) + parser.add_argument("--output-dir", type=Path) + args = parser.parse_args(argv) + recording = args.recording.resolve() + sidecars, missing = companions(recording) + required = [recording, *sidecars] + if not recording.is_file(): + missing.insert(0, str(recording)) + if missing: + parser.error("missing take files: " + ", ".join(missing)) + + before = {path: hashlib.sha256(path.read_bytes()).hexdigest() for path in required} + timeline = StepTimeline.beside(recording) + source = HandTee(McapFrameSource(recording), timeline) + report = run(source, timeline=timeline) + name, data = bundle.build(report, source.tracks(), recording, timeline) + with zipfile.ZipFile(io.BytesIO(data)) as archive: + root = name.removesuffix(".zip") + manifest = json.loads(archive.read(f"{root}/report.json")) + for item in manifest["inputs"]["files"]: + path = recording.parent / item["name"] + if item["sha256"] != before[path]: + raise RuntimeError(f"take changed during packaging: {path}") + if any( + hashlib.sha256(path.read_bytes()).hexdigest() != digest + for path, digest in before.items() + ): + raise RuntimeError( + "take changed during packaging; finish recording before packaging" + ) + + output_dir = args.output_dir or recording.parent + output_dir.mkdir(parents=True, exist_ok=True) + output = output_dir / name + with output.open("xb") as destination: + destination.write(data) + print( + json.dumps( + { + "archive": str(output), + "verdict": report.verdict, + "bytes": len(data), + "sha256": hashlib.sha256(data).hexdigest(), + "checker": manifest["tool"], + }, + indent=2, + ) + ) + + +if __name__ == "__main__": + main() diff --git a/src/python/isaaccapture/cts/hand/checker/tests/test_package.py b/src/python/isaaccapture/cts/hand/checker/tests/test_package.py new file mode 100644 index 0000000000..fba7725354 --- /dev/null +++ b/src/python/isaaccapture/cts/hand/checker/tests/test_package.py @@ -0,0 +1,75 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +"""Headless packaging preserves the take and rejects incomplete or changing inputs.""" + +from __future__ import annotations + +import hashlib +import json +import zipfile +from pathlib import Path + +import pytest +import synth + +from hand_cts import package + + +@pytest.fixture +def take(tmp_path: Path) -> Path: + recording = synth.write_recording(tmp_path / "take-hand.mcap", synth.both_hands(60)) + recording.with_suffix(".labels.json").write_text('{"steps": []}') + recording.with_suffix(".json").write_text('{"device": "example"}') + recording.with_suffix(".log").write_text("capture log\n") + return recording + + +def test_headless_archive_preserves_all_inputs(take, tmp_path, capsys): + paths = [take, *(take.with_suffix(s) for s in (".labels.json", ".json", ".log"))] + before = {path.name: path.read_bytes() for path in paths} + package.main([str(take), "--output-dir", str(tmp_path / "output")]) + result = json.loads(capsys.readouterr().out) + archive = Path(result["archive"]) + assert hashlib.sha256(archive.read_bytes()).hexdigest() == result["sha256"] + with zipfile.ZipFile(archive) as bundle: + assert bundle.testzip() is None + root = archive.stem + report = json.loads(bundle.read(f"{root}/report.json")) + assert report["verdict"] == result["verdict"] + for item in report["inputs"]["files"]: + original = before[item["name"]] + assert bundle.read(f"{root}/{item['name']}") == original + assert item["sha256"] == hashlib.sha256(original).hexdigest() + assert {path.name: path.read_bytes() for path in paths} == before + + +def test_headless_archive_never_overwrites(take, capsys): + package.main([str(take)]) + output = Path(json.loads(capsys.readouterr().out)["archive"]) + original = output.read_bytes() + with pytest.raises(FileExistsError): + package.main([str(take)]) + assert output.read_bytes() == original + + +def test_headless_packaging_rejects_incomplete_takes(take): + take.with_suffix(".labels.json").unlink() + with pytest.raises(SystemExit) as exited: + package.main([str(take)]) + assert exited.value.code == 2 + assert not list(take.parent.glob("*.zip")) + + +def test_headless_packaging_rejects_a_concurrent_input_change(take, monkeypatch): + original = package.bundle.build + + def changing_input(*args, **kwargs): + archive = original(*args, **kwargs) + take.with_suffix(".log").write_text("modified during packaging\n") + return archive + + monkeypatch.setattr(package.bundle, "build", changing_input) + with pytest.raises(RuntimeError, match="take changed during packaging"): + package.main([str(take)]) + assert not list(take.parent.glob("*.zip")) diff --git a/tests/cpp/plugins/CMakeLists.txt b/tests/cpp/plugins/CMakeLists.txt index fd7e2d2e68..f5ef077786 100644 --- a/tests/cpp/plugins/CMakeLists.txt +++ b/tests/cpp/plugins/CMakeLists.txt @@ -21,3 +21,7 @@ endif() if(TARGET manus_calibration_core) add_subdirectory(manus_calibration) endif() + +if(BUILD_PLUGIN_OMNIGLOVE) + add_subdirectory(omniglove) +endif() diff --git a/tests/cpp/plugins/omniglove/CMakeLists.txt b/tests/cpp/plugins/omniglove/CMakeLists.txt new file mode 100644 index 0000000000..f9fbf6a12e --- /dev/null +++ b/tests/cpp/plugins/omniglove/CMakeLists.txt @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +# SPDX-License-Identifier: Apache-2.0 + +add_executable(omniglove_plugin_tests test_hand_conversion.cpp) +target_link_libraries(omniglove_plugin_tests + PRIVATE Catch2::Catch2WithMain isaac::omniglove_plugin OmnigloveSDK::api +) +catch_discover_tests(omniglove_plugin_tests TEST_PREFIX "omniglove_" ADD_TAGS_AS_LABELS) diff --git a/tests/cpp/plugins/omniglove/test_hand_conversion.cpp b/tests/cpp/plugins/omniglove/test_hand_conversion.cpp new file mode 100644 index 0000000000..bf8a9f87c8 --- /dev/null +++ b/tests/cpp/plugins/omniglove/test_hand_conversion.cpp @@ -0,0 +1,285 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ + +using plugins::omniglove::ConversionStatus; +using plugins::omniglove::HandJoints; + +void fill_pose(omniglove_pose6_t& pose, float x, float y, float z, const XrQuaternionf& orientation) +{ + pose.position_m[0] = x; + pose.position_m[1] = y; + pose.position_m[2] = z; + pose.quaternion_xyzw[0] = orientation.x; + pose.quaternion_xyzw[1] = orientation.y; + pose.quaternion_xyzw[2] = orientation.z; + pose.quaternion_xyzw[3] = orientation.w; +} + +void fill_hand(omniglove_post_data_t& post, const XrQuaternionf& orientation = { 0.0F, 0.0F, 0.0F, 2.0F }) +{ + post.mapped = 1; + post.flags = plugins::omniglove::kPostIkConverged | plugins::omniglove::kPostBoneModelValid; + float x = 0.01F; + auto fill = [&x, &orientation](omniglove_pose6_t& pose) + { + fill_pose(pose, x, x + 0.01F, x + 0.02F, orientation); + x += 0.01F; + }; + fill(post.wrist.landmark); + fill(post.thumb.landmarks.cmc); + fill(post.thumb.landmarks.mcp); + fill(post.thumb.landmarks.ip); + fill(post.thumb.landmarks.tip); + const std::array fingers = { &post.index, &post.middle, &post.ring, &post.pinky }; + for (auto* finger : fingers) + { + fill(finger->landmarks.mcp); + fill(finger->landmarks.pip); + fill(finger->landmarks.dip); + fill(finger->landmarks.tip); + } +} + +bool near(float left, float right) +{ + return std::abs(left - right) < 1.0e-6F; +} + +std::array rotate(const XrQuaternionf& q, const std::array& v) +{ + const std::array t = { 2.0F * (q.y * v[2] - q.z * v[1]), 2.0F * (q.z * v[0] - q.x * v[2]), + 2.0F * (q.x * v[1] - q.y * v[0]) }; + return { v[0] + q.w * t[0] + q.y * t[2] - q.z * t[1], v[1] + q.w * t[1] + q.z * t[0] - q.x * t[2], + v[2] + q.w * t[2] + q.x * t[1] - q.y * t[0] }; +} + +std::array subtract(const XrVector3f& a, const XrVector3f& b) +{ + return { a.x - b.x, a.y - b.y, a.z - b.z }; +} + +float dot(const std::array& a, const std::array& b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]; +} + +std::array normalized(const std::array& v) +{ + const float length = std::sqrt(dot(v, v)); + return { v[0] / length, v[1] / length, v[2] / length }; +} + +std::array cross(const std::array& a, const std::array& 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] }; +} + +bool openxr_axes(bool is_left, const XrQuaternionf& orientation) +{ + omniglove_post_data_t post{}; + fill_hand(post, orientation); + const auto result = plugins::omniglove::convert_post_data(post, is_left); + const auto* converted = std::get_if(&result); + if (converted == nullptr) + { + return false; + } + const auto& joints = *converted; + const float norm = std::sqrt(orientation.x * orientation.x + orientation.y * orientation.y + + orientation.z * orientation.z + orientation.w * orientation.w); + const XrQuaternionf unit = { orientation.x / norm, orientation.y / norm, orientation.z / norm, orientation.w / norm }; + for (std::size_t index = 0; index < joints.size(); ++index) + { + const auto& joint = joints[index]; + if (joint.locationFlags == 0) + { + continue; + } + for (const std::array& axis : + { std::array{ 0.0F, 1.0F, 0.0F }, std::array{ 0.0F, 0.0F, 1.0F } }) + { + auto sdk_axis = axis; + if (!is_left) + { + sdk_axis[0] = -sdk_axis[0]; + sdk_axis[1] = -sdk_axis[1]; + } + auto expected = rotate(unit, sdk_axis); + if (!is_left) + { + expected[0] = -expected[0]; + expected[1] = -expected[1]; + } + auto xr_axis = axis; + if (index >= XR_HAND_JOINT_THUMB_METACARPAL_EXT && index <= XR_HAND_JOINT_THUMB_TIP_EXT && axis[1] != 0.0F) + { + xr_axis = { is_left ? -1.0F : 1.0F, 0.0F, 0.0F }; + } + if (index != XR_HAND_JOINT_WRIST_EXT && axis[2] != 0.0F) + { + xr_axis[2] = -xr_axis[2]; + } + const auto actual = rotate(joint.pose.orientation, xr_axis); + for (std::size_t component = 0; component < actual.size(); ++component) + { + if (!near(actual[component], expected[component])) + { + return false; + } + } + } + } + return true; +} + +} // namespace + +TEST_CASE("OmniGlove conversion preserves its OpenXR hand contract", "[omniglove][conversion]") +{ + omniglove_post_data_t post{}; + fill_hand(post); + auto result = plugins::omniglove::convert_post_data(post, true); + REQUIRE(std::holds_alternative(result)); + auto joints = std::get(result); + constexpr XrSpaceLocationFlags valid = XR_SPACE_LOCATION_POSITION_VALID_BIT | XR_SPACE_LOCATION_ORIENTATION_VALID_BIT; + int valid_count = 0; + for (const auto& joint : joints) + { + valid_count += (joint.locationFlags & valid) == valid; + } + CHECK(valid_count == 21); + CHECK(joints[XR_HAND_JOINT_PALM_EXT].locationFlags == 0); + CHECK(joints[XR_HAND_JOINT_INDEX_METACARPAL_EXT].locationFlags == 0); + CHECK(near(joints[XR_HAND_JOINT_WRIST_EXT].pose.orientation.w, 1.0F)); + + for (bool is_left : { true, false }) + { + CHECK(openxr_axes(is_left, { 0.0F, 0.0F, 0.0F, 2.0F })); + CHECK(openxr_axes(is_left, { 1.0F, 2.0F, 3.0F, 4.0F })); + } + const XrPosef left_wrist = joints[XR_HAND_JOINT_WRIST_EXT].pose; + result = plugins::omniglove::convert_post_data(post, false); + REQUIRE(std::holds_alternative(result)); + joints = std::get(result); + CHECK(near(joints[XR_HAND_JOINT_WRIST_EXT].pose.position.x, -left_wrist.position.x)); + CHECK(near(joints[XR_HAND_JOINT_WRIST_EXT].pose.position.y, -left_wrist.position.y)); + CHECK(near(joints[XR_HAND_JOINT_WRIST_EXT].pose.position.z, left_wrist.position.z)); + + post.flags = plugins::omniglove::kPostBoneModelValid; + CHECK(std::get(plugins::omniglove::convert_post_data(post, true)) == + ConversionStatus::IkNotConverged); + post.flags = plugins::omniglove::kPostIkConverged; + CHECK(std::get(plugins::omniglove::convert_post_data(post, true)) == + ConversionStatus::BoneModelInvalid); + fill_hand(post); + post.mapped = 0; + CHECK(std::get(plugins::omniglove::convert_post_data(post, true)) == ConversionStatus::NotMapped); + fill_hand(post); + post.index.landmarks.mcp.position_m[0] = std::numeric_limits::quiet_NaN(); + CHECK(std::get(plugins::omniglove::convert_post_data(post, true)) == ConversionStatus::InvalidPose); +} + +TEST_CASE("OmniGlove bent thumb hinges align with local X", "[omniglove][conversion]") +{ + const float inverse_norm = 1.0F / std::sqrt(30.0F); + const std::array source_transforms = { XrPosef{ { 0.0F, 0.0F, 0.0F, 1.0F }, {} }, + XrPosef{ { inverse_norm, 2.0F * inverse_norm, + 3.0F * inverse_norm, 4.0F * inverse_norm }, + { 0.11F, -0.07F, 0.09F } } }; + constexpr std::array thumb_joints = { XR_HAND_JOINT_THUMB_METACARPAL_EXT, XR_HAND_JOINT_THUMB_PROXIMAL_EXT, + XR_HAND_JOINT_THUMB_DISTAL_EXT, XR_HAND_JOINT_THUMB_TIP_EXT }; + for (bool is_left : { true, false }) + { + for (const XrPosef& transform : source_transforms) + { + for (float ip_bend : { 0.4F, 1.1F }) + { + CAPTURE(is_left, ip_bend, transform.orientation.x); + constexpr float mcp_bend = 0.45F; + const float distal_angle = mcp_bend + ip_bend; + const XrQuaternionf mcp_orientation{ 0.0F, std::sin(mcp_bend / 2.0F), 0.0F, std::cos(mcp_bend / 2.0F) }; + const XrQuaternionf ip_orientation{ 0.0F, std::sin(distal_angle / 2.0F), 0.0F, + std::cos(distal_angle / 2.0F) }; + const XrVector3f mcp{ 0.02F, 0.01F, 0.025F }; + const XrVector3f ip{ mcp.x + 0.03F * std::sin(mcp_bend), mcp.y, mcp.z + 0.03F * std::cos(mcp_bend) }; + const XrVector3f tip{ ip.x + 0.02F * std::sin(distal_angle), ip.y, ip.z + 0.02F * std::cos(distal_angle) }; + // Thumb flexion is in the SDK XZ plane, with its hinge along Y. + const std::array local_poses = { + XrPosef{ { 0.0F, 0.0F, 0.0F, 1.0F }, { 0.02F, 0.01F, 0.0F } }, XrPosef{ mcp_orientation, mcp }, + XrPosef{ ip_orientation, ip }, XrPosef{ ip_orientation, tip } + }; + omniglove_post_data_t post{}; + fill_hand(post); + const std::array landmarks = { &post.thumb.landmarks.cmc, &post.thumb.landmarks.mcp, + &post.thumb.landmarks.ip, &post.thumb.landmarks.tip }; + std::array expected_positions; + for (std::size_t i = 0; i < local_poses.size(); ++i) + { + const XrPosef pose = oxr_utils::multiply_poses(transform, local_poses[i]); + fill_pose(*landmarks[i], pose.position.x, pose.position.y, pose.position.z, pose.orientation); + expected_positions[i] = pose.position; + if (!is_left) + { + expected_positions[i].x = -expected_positions[i].x; + expected_positions[i].y = -expected_positions[i].y; + } + } + std::array, 4> bone_directions; + for (std::size_t i = 0; i < bone_directions.size() - 1; ++i) + { + bone_directions[i] = normalized(subtract(expected_positions[i + 1], expected_positions[i])); + } + bone_directions.back() = bone_directions[2]; + const auto ip_hinge = normalized(cross(bone_directions[2], bone_directions[1])); + const auto mcp_hinge = normalized(cross(bone_directions[1], bone_directions[0])); + const auto result = plugins::omniglove::convert_post_data(post, is_left); + REQUIRE(std::holds_alternative(result)); + const auto& joints = std::get(result); + for (std::size_t i = 0; i < thumb_joints.size(); ++i) + { + CAPTURE(i); + const auto& pose = joints[thumb_joints[i]].pose; + CHECK(near(pose.position.x, expected_positions[i].x)); + CHECK(near(pose.position.y, expected_positions[i].y)); + CHECK(near(pose.position.z, expected_positions[i].z)); + const auto x_axis = rotate(pose.orientation, { 1.0F, 0.0F, 0.0F }); + CHECK(dot(x_axis, ip_hinge) > 0.99999F); + CHECK(dot(x_axis, mcp_hinge) > 0.99999F); + CHECK(dot(rotate(pose.orientation, { 0.0F, 0.0F, -1.0F }), bone_directions[i]) > 0.99999F); + } + } + } + } +} + +TEST_CASE("OmniGlove returns no joints when the last landmark is invalid", "[omniglove][conversion]") +{ + omniglove_post_data_t post{}; + fill_hand(post); + post.pinky.landmarks.tip.quaternion_xyzw[3] = std::numeric_limits::infinity(); + auto result = plugins::omniglove::convert_post_data(post, true); + REQUIRE(std::holds_alternative(result)); + CHECK(std::get(result) == ConversionStatus::InvalidPose); + + fill_hand(post); + for (auto& component : post.pinky.landmarks.tip.quaternion_xyzw) + { + component = 0.0F; + } + result = plugins::omniglove::convert_post_data(post, false); + REQUIRE(std::holds_alternative(result)); + CHECK(std::get(result) == ConversionStatus::InvalidPose); +} diff --git a/tests/cpp/plugins/plugin_utils/CMakeLists.txt b/tests/cpp/plugins/plugin_utils/CMakeLists.txt index 9534f3849b..1edff4676e 100644 --- a/tests/cpp/plugins/plugin_utils/CMakeLists.txt +++ b/tests/cpp/plugins/plugin_utils/CMakeLists.txt @@ -1,14 +1,15 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -add_executable(plugin_device_status_publisher_tests +add_executable(plugin_utils_tests test_plugin_device_status_publisher.cpp + test_wrist_source_config.cpp ) -target_link_libraries(plugin_device_status_publisher_tests +target_link_libraries(plugin_utils_tests PRIVATE Catch2::Catch2WithMain Teleop::plugin_utils ) -catch_discover_tests(plugin_device_status_publisher_tests) +catch_discover_tests(plugin_utils_tests ADD_TAGS_AS_LABELS) diff --git a/tests/cpp/plugins/plugin_utils/test_wrist_source_config.cpp b/tests/cpp/plugins/plugin_utils/test_wrist_source_config.cpp new file mode 100644 index 0000000000..7dc0ebb275 --- /dev/null +++ b/tests/cpp/plugins/plugin_utils/test_wrist_source_config.cpp @@ -0,0 +1,128 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 Agibot Co., Ltd. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace +{ + +class Environment +{ +public: + explicit Environment(const char* name) : m_name(name) + { + if (const char* value = std::getenv(name)) + { + m_previous = value; + } + unsetenv(name); + } + ~Environment() + { + if (m_previous) + { + setenv(m_name, m_previous->c_str(), 1); + } + else + { + unsetenv(m_name); + } + } + void set(const char* value) + { + setenv(m_name, value, 1); + } + +private: + const char* m_name; + std::optional m_previous; +}; + +constexpr const char* kVariable = "ISAAC_TEST_WRIST_CONFIG"; + +} // namespace + +TEST_CASE("Wrist source mode accepts only named modes", "[plugin_utils][wrist_config]") +{ + Environment env(kVariable); + using plugin_utils::WristSourceMode; + CHECK(plugin_utils::wrist_source_mode_from_env(kVariable, WristSourceMode::Auto) == WristSourceMode::Auto); + env.set("hand_tracking"); + CHECK(plugin_utils::wrist_source_mode_from_env(kVariable, WristSourceMode::Auto) == WristSourceMode::HandTracking); + env.set("controller"); + CHECK(plugin_utils::wrist_source_mode_from_env(kVariable, WristSourceMode::Auto) == WristSourceMode::Controller); + env.set("auto"); + CHECK(plugin_utils::wrist_source_mode_from_env(kVariable, WristSourceMode::HandTracking) == WristSourceMode::Auto); + for (const auto mode : { WristSourceMode::HandTracking, WristSourceMode::Controller, WristSourceMode::Auto }) + { + env.set(plugin_utils::to_string(mode)); + CHECK(plugin_utils::wrist_source_mode_from_env(kVariable, WristSourceMode::Auto) == mode); + } + env.set("controler"); + CHECK_THROWS_AS(plugin_utils::wrist_source_mode_from_env(kVariable, WristSourceMode::Auto), std::runtime_error); +} + +TEST_CASE("Required wrist offsets reject missing and malformed input", "[plugin_utils][wrist_config]") +{ + Environment env(kVariable); + CHECK_THROWS_AS(plugin_utils::pose_from_env(kVariable), std::runtime_error); + for (const char* value : + { "", "0,0,0,0,0,0", "0,0,0,0,0,0,0", "nan,0,0,0,0,0,1", "0,0,0,0,0,inf,1", "0,0,0,0,0,0,1,junk" }) + { + env.set(value); + CHECK_THROWS_AS(plugin_utils::pose_from_env(kVariable), std::runtime_error); + } +} + +TEST_CASE("Wrist offsets normalize finite quaternions and preserve metres", "[plugin_utils][wrist_config]") +{ + Environment env(kVariable); + for (const char* value : { "0.1,-0.2,0.3,0,0,0,2", "0.1,-0.2,0.3,0,0,0,3e38" }) + { + env.set(value); + const auto pose = plugin_utils::pose_from_env(kVariable); + CHECK(std::abs(pose.position.x - 0.1F) < 1.0e-6F); + CHECK(std::abs(pose.position.y + 0.2F) < 1.0e-6F); + CHECK(std::abs(pose.position.z - 0.3F) < 1.0e-6F); + CHECK(std::abs(pose.orientation.w - 1.0F) < 1.0e-6F); + } +} + +TEST_CASE("Optional wrist offsets fall back only when unset", "[plugin_utils][wrist_config]") +{ + Environment env(kVariable); + const XrPosef fallback{ { 0, 0, 0, 1 }, { 1, 2, 3 } }; + CHECK(plugin_utils::pose_from_env(kVariable, fallback).position.y == 2); + env.set("invalid"); + CHECK_THROWS_AS(plugin_utils::pose_from_env(kVariable, fallback), std::runtime_error); +} + +TEST_CASE("Glove basis conversion preserves left poses and is reversible on the right", "[plugin_utils][hand_basis]") +{ + constexpr XrPosef pose{ { 0.1F, 0.2F, 0.3F, 0.9F }, { 1.0F, 2.0F, 3.0F } }; + for (bool is_left : { true, false }) + { + const auto converted = plugin_utils::remap_to_openxr_basis(pose, is_left); + const float sign = is_left ? 1.0F : -1.0F; + CHECK(converted.position.x == sign * pose.position.x); + CHECK(converted.position.y == sign * pose.position.y); + CHECK(converted.position.z == pose.position.z); + CHECK(converted.orientation.x == sign * pose.orientation.x); + CHECK(converted.orientation.y == sign * pose.orientation.y); + CHECK(converted.orientation.z == pose.orientation.z); + CHECK(converted.orientation.w == pose.orientation.w); + const auto restored = plugin_utils::remap_to_openxr_basis(converted, is_left); + CHECK(restored.position.x == pose.position.x); + CHECK(restored.position.y == pose.position.y); + CHECK(restored.orientation.x == pose.orientation.x); + CHECK(restored.orientation.y == pose.orientation.y); + } +}