From 1784cc1a7bfd7d2f583fd4cb94b523764ad3de33 Mon Sep 17 00:00:00 2001 From: Michael Haselton Date: Tue, 22 Sep 2026 15:58:24 -0400 Subject: [PATCH 01/12] feat(soma): add body pose transport pipeline Add versioned SOMA V0 body joint-rotation and evaluated joint-pose schemas. Integrate generated trackers, Python bindings, schema goldens, documentation, and validation coverage. Add a typed SOMA body DeviceIO source that normalizes both body representations into evaluated joint poses. Use upstream FK for joint rotations and direct mapping for evaluated poses. Add a standalone body demo publisher and allow the DeviceIO viewer to select native SOMA or FullBodyPose while preserving each schema's joint order and topology. Signed-off-by: Michael Haselton --- docs/source/device/trackers.rst | 66 ++++ docs/source/references/generated_trackers.rst | 6 + docs/source/references/retargeting/index.rst | 5 + examples/CMakeLists.txt | 1 + examples/deviceio_live_view/README.md | 70 +++- examples/deviceio_live_view/pyproject.toml | 1 + .../deviceio_live_view/body_pipeline.py | 69 ++++ .../deviceio_live_view/deviceio_pipeline.py | 45 +++ .../deviceio_live_view/deviceio_viser.py | 148 +++---- .../deviceio_live_view/full_body_pose.py | 64 ++++ .../deviceio_live_view/live_deviceio.py | 55 ++- .../deviceio_live_view/soma_body.py | 69 ++++ examples/soma_body_publisher/CMakeLists.txt | 9 + examples/soma_body_publisher/README.md | 49 +++ .../cpp/soma_body_pusher/CMakeLists.txt | 15 + .../cpp/soma_body_pusher/soma_body_pusher.cpp | 150 ++++++++ examples/soma_body_publisher/pyproject.toml | 24 ++ .../soma_body_publisher/__init__.py | 2 + .../soma_body_publisher/__main__.py | 8 + .../soma_body_publisher/publisher.py | 187 +++++++++ src/core/deviceio_trackers/trackers.toml | 16 + src/core/schema/fbs/soma_body_common_v0.fbs | 89 +++++ .../schema/fbs/soma_body_joint_poses_v0.fbs | 35 ++ src/core/schema/fbs/soma_body_v0.fbs | 40 ++ src/core/schema/fbs/soma_common_v0.fbs | 17 + .../schema/golden/soma_body_common_v0.bfbs | Bin 0 -> 4824 bytes .../golden/soma_body_joint_poses_v0.bfbs | Bin 0 -> 6696 bytes src/core/schema/golden/soma_body_v0.bfbs | Bin 0 -> 6696 bytes src/core/schema/golden/soma_common_v0.bfbs | Bin 0 -> 716 bytes src/core/schema/python/CMakeLists.txt | 3 + src/core/schema/python/schema_module.cpp | 8 + src/core/schema/python/schema_serialized.h | 18 +- .../soma_body_joint_poses_v0_bindings.h | 98 +++++ .../schema/python/soma_body_v0_bindings.h | 169 ++++++++ .../schema/python/soma_common_v0_bindings.h | 23 ++ .../deviceio_source_nodes/__init__.py | 11 + .../deviceio_tensor_types.py | 30 ++ .../deviceio_source_nodes/soma_body_source.py | 106 +++++ .../tensor_types/__init__.py | 6 + .../tensor_types/indices.py | 4 + .../tensor_types/standard_types.py | 28 ++ .../retargeting_engine/utilities/__init__.py | 2 +- .../utilities/soma_body_evaluator.py | 109 ++++++ src/python/isaaccapture/schema/__init__.py | 24 ++ tests/AGENTS.md | 3 + tests/cpp/core/schema/CMakeLists.txt | 2 + .../schema/test_soma_body_joint_poses_v0.cpp | 52 +++ tests/cpp/core/schema/test_soma_body_v0.cpp | 124 ++++++ .../test_soma_body_evaluator.py | 85 +++++ .../test_soma_body_source.py | 189 +++++++++ .../schema/test_soma_body_joint_poses_v0.py | 51 +++ tests/python/core/schema/test_soma_body_v0.py | 213 +++++++++++ tests/python/examples/CMakeLists.txt | 2 + .../deviceio_live_view/CMakeLists.txt | 21 + .../examples/deviceio_live_view/conftest.py | 25 ++ .../deviceio_live_view/pyproject.toml | 13 + .../deviceio_live_view/test_soma_viewer.py | 361 ++++++++++++++++++ .../soma_body_publisher/CMakeLists.txt | 22 ++ .../examples/soma_body_publisher/conftest.py | 26 ++ .../soma_body_publisher/pyproject.toml | 14 + .../soma_body_publisher/test_publisher.py | 199 ++++++++++ 61 files changed, 3166 insertions(+), 115 deletions(-) create mode 100644 examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py create mode 100644 examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py create mode 100644 examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/full_body_pose.py create mode 100644 examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py create mode 100644 examples/soma_body_publisher/CMakeLists.txt create mode 100644 examples/soma_body_publisher/README.md create mode 100644 examples/soma_body_publisher/cpp/soma_body_pusher/CMakeLists.txt create mode 100644 examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp create mode 100644 examples/soma_body_publisher/pyproject.toml create mode 100644 examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/__init__.py create mode 100644 examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/__main__.py create mode 100644 examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py create mode 100644 src/core/schema/fbs/soma_body_common_v0.fbs create mode 100644 src/core/schema/fbs/soma_body_joint_poses_v0.fbs create mode 100644 src/core/schema/fbs/soma_body_v0.fbs create mode 100644 src/core/schema/fbs/soma_common_v0.fbs create mode 100644 src/core/schema/golden/soma_body_common_v0.bfbs create mode 100644 src/core/schema/golden/soma_body_joint_poses_v0.bfbs create mode 100644 src/core/schema/golden/soma_body_v0.bfbs create mode 100644 src/core/schema/golden/soma_common_v0.bfbs create mode 100644 src/core/schema/python/soma_body_joint_poses_v0_bindings.h create mode 100644 src/core/schema/python/soma_body_v0_bindings.h create mode 100644 src/core/schema/python/soma_common_v0_bindings.h create mode 100644 src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py create mode 100644 src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py create mode 100644 tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp create mode 100644 tests/cpp/core/schema/test_soma_body_v0.cpp create mode 100644 tests/python/core/retargeting_engine/test_soma_body_evaluator.py create mode 100644 tests/python/core/retargeting_engine/test_soma_body_source.py create mode 100644 tests/python/core/schema/test_soma_body_joint_poses_v0.py create mode 100644 tests/python/core/schema/test_soma_body_v0.py create mode 100644 tests/python/examples/deviceio_live_view/CMakeLists.txt create mode 100644 tests/python/examples/deviceio_live_view/conftest.py create mode 100644 tests/python/examples/deviceio_live_view/pyproject.toml create mode 100644 tests/python/examples/deviceio_live_view/test_soma_viewer.py create mode 100644 tests/python/examples/soma_body_publisher/CMakeLists.txt create mode 100644 tests/python/examples/soma_body_publisher/conftest.py create mode 100644 tests/python/examples/soma_body_publisher/pyproject.toml create mode 100644 tests/python/examples/soma_body_publisher/test_publisher.py diff --git a/docs/source/device/trackers.rst b/docs/source/device/trackers.rst index b5568bb8e7..372db901d8 100644 --- a/docs/source/device/trackers.rst +++ b/docs/source/device/trackers.rst @@ -24,6 +24,8 @@ reading it from OpenXR tensor collections via the - :code-file:`Generic3AxisPedalTracker ` -- foot pedal axis values (generated) - :code-file:`JointStateTracker ` -- named joint-space device state (leader arms, exoskeletons, gloves, ...) (generated) - :code-file:`Se3Tracker ` -- generic SE3 (6-DoF) pose sources (tracker pucks, mocap rigid bodies, logical trackers) (generated) +- :code-file:`SomaBodyJointRotationsV0Tracker ` -- SOMA v0 body joint rotations with 77 controls (generated) +- :code-file:`SomaBodyJointPosesV0Tracker ` -- evaluated SOMA v0 body poses for 77 joints (generated) All trackers follow the same lifecycle: @@ -269,6 +271,70 @@ reads the PICO ``XR_BD_body_tracking`` extension directly. ``FullBodyTrackerPico`` remains available as a deprecated alias for ``FullBodyTracker`` so existing scripts run unchanged. +SOMA body V0 trackers +~~~~~~~~~~~~~~~~~~~~~ + +Reads the SOMA body joint-rotation transport schema v0 from an OpenXR tensor collection. ``V0`` +versions the Isaac Teleop schema; it is not an upstream SOMA-X release number. The schema is defined +and verified against SOMA-X v0.3.1 and its v0.3 pose interface. A vendor adapter converts its native +skeleton to :code:`SomaBodyJointRotationsV0`, serializes the checked-in schema, and publishes it +through ``SchemaPusher``. The tracker transports and records the fixed payload; it does not perform +vendor-specific skeleton or reference-pose conversion. + +The payload contains the 77 user-facing joint rotations for SOMA's 78-joint public skeleton. It omits +the virtual Root, which is always identity and is added internally by ``SOMALayer``. Hips is the +global rotation; the remaining rotations use SOMA-X v0.3.1 with ``reference_pose=None`` and +``absolute_pose=False``. Producers must convert historical, custom, or vendor-native references +before publishing. If the native skeleton has a separate Root, the producer composes it into the +global Hips rotation and translation. Rotations are unit XYZW quaternions; ``q`` and ``-q`` represent +the same rotation, and the transport requires no quaternion-sign convention. ``global_translation`` +is applied to Hips and is measured in meters. The reference frame is right-handed with +Y up and +Z +forward. Each joint rotation and the global translation have independent validity. + +A SOMA-X consumer can convert the quaternion array with +`soma.geometry.transforms.quaternion_xyzw_to_matrix `__ +and pass the resulting matrices to ``SOMALayer.pose(..., pose2rot=False)``. + +When a producer already has evaluated global positions and orientations, it can +publish ``SomaBodyJointPosesV0`` through ``SomaBodyJointPosesV0Tracker`` instead. +That profile carries a global pose and independent validity for each of the same +77 public joints. Positions use meters and orientations use unit XYZW +quaternions in the same reference-frame convention. It requires no downstream +FK. + +The integration defines the reference frame and keeps it stable for the collection. To align this +pose with another tracker, such as HMD full-body tracking, the producer transforms its native +tracking frame into the same physical reference frame before publishing. + +For a retargeting graph, ``SomaBodySource(name="body", collection_id="vendor.soma", layer=layer)`` +registers the rotation tracker by default. Pass ``representation="joint-poses"`` +to select the evaluated-pose tracker; that profile does not require ``layer``. +The source evaluates rotations once per graph step against a caller-supplied +SOMA identity or directly maps evaluated poses, then emits ``SomaBodyInput`` +with global poses for all 77 joints. +An inactive tracker produces an absent output. The raw tracker and transport do +not require SOMA-X or PyTorch. The live-view example supplies those evaluation +dependencies, renders the SOMA public hierarchy, and offers launch-time +body-schema selection. It does not map SOMA into ``FullBodyPose``. + +- Schemas: :code-file:`src/core/schema/fbs/soma_body_common_v0.fbs`, :code-file:`src/core/schema/fbs/soma_body_v0.fbs`, :code-file:`src/core/schema/fbs/soma_body_joint_poses_v0.fbs` +- Manifest: :code-file:`src/core/deviceio_trackers/trackers.toml` (``soma_body_joint_rotations_v0`` and ``soma_body_joint_poses_v0``) +- C++ header: ``#include `` +- Python import: ``from isaaccapture.deviceio_trackers import SomaBodyJointRotationsV0Tracker`` +- Evaluated-pose C++ header: ``#include `` +- Evaluated-pose Python import: ``from isaaccapture.deviceio_trackers import SomaBodyJointPosesV0Tracker`` +- Graph source: :code-file:`src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py` +- Live-view example: :code-file:`examples/deviceio_live_view/README.md` +- Demo publisher: :code-file:`examples/soma_body_publisher/README.md` +- Record channels: ``soma_body_joint_rotations_v0``, ``soma_body_joint_rotations_v0_tracked`` | MCAP schema: ``core.SomaBodyJointRotationsV0Record`` +- Evaluated-pose record channels: ``soma_body_joint_poses_v0``, ``soma_body_joint_poses_v0_tracked`` | MCAP schema: ``core.SomaBodyJointPosesV0Record`` +- Tests: + + - :code-file:`tests/cpp/core/schema/test_soma_body_v0.cpp` + - :code-file:`tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp` + - :code-file:`tests/python/core/schema/test_soma_body_v0.py` + - :code-file:`tests/python/core/retargeting_engine/test_soma_body_source.py` + FrameMetadataTrackerOak ~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/docs/source/references/generated_trackers.rst b/docs/source/references/generated_trackers.rst index 6f916b9ec0..8b23e61f4e 100644 --- a/docs/source/references/generated_trackers.rst +++ b/docs/source/references/generated_trackers.rst @@ -89,6 +89,12 @@ Generated today * - ``se3_tracker`` - ``Se3Tracker`` - Overrides ``class``; the default would give ``Se3TrackerTracker`` + * - ``soma_body_joint_rotations_v0`` + - ``SomaBodyJointRotationsV0Tracker`` + - SOMA v0 body joint rotations with 77 controls for the 78-joint skeleton; defined against SOMA-X v0.3.1 + * - ``soma_body_joint_poses_v0`` + - ``SomaBodyJointPosesV0Tracker`` + - Evaluated SOMA v0 positions and orientations for the same 77 public joints * - ``oglo_tactile`` - ``OgloTactileTracker`` - MCAP channels are ``oglo``/``oglo_tracked``, so ``channel`` is overridden diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 63649e7f85..08848dc5e6 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -17,6 +17,11 @@ Source Nodes * ``JointStateSource`` -- provides name-keyed joint positions from a generic joint-space device (leader arm, exoskeleton, ...). See :doc:`joint_space`. * ``FullBodySource`` -- provides full-body pose (e.g. Pico tracking). +* ``SomaBodySource`` -- polls either a typed ``SomaBodyJointRotationsV0`` or + ``SomaBodyJointPosesV0`` payload from a configured publisher collection. It + runs upstream FK for rotations or directly maps evaluated poses, then emits + an optional ``SomaBodyInput`` containing the 77 global SOMA joint poses. + See :doc:`/device/trackers` and :code-file:`examples/deviceio_live_view/README.md`. Available Retargeters --------------------- diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 57cadc7c90..af5f8720b0 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -17,6 +17,7 @@ if(BUILD_EXAMPLES) add_subdirectory(native_openxr) add_subdirectory(mcap_record_replay) add_subdirectory(deviceio_live_view) + add_subdirectory(soma_body_publisher) add_subdirectory(haptic_feedback) add_subdirectory(sharpa_avatar) add_subdirectory(vehicle_teleop) diff --git a/examples/deviceio_live_view/README.md b/examples/deviceio_live_view/README.md index 5fce01d879..e324fc0eb7 100644 --- a/examples/deviceio_live_view/README.md +++ b/examples/deviceio_live_view/README.md @@ -12,10 +12,78 @@ CloudXR runtime and WSS proxy itself, so there is nothing to launch separately. ```bash uv pip install -e ./examples/deviceio_live_view -python -m isaaccapture_examples.deviceio_live_view --accept-eula +uv run --no-sync python -m isaaccapture_examples.deviceio_live_view --accept-eula ``` Open the URL it prints (default ). It binds every interface, so another machine on the network can reach it at `http://:8080`; pass `--host 127.0.0.1` to keep it local, `--port` to move it. Ctrl+C stops it. + +## SOMA body POC + +The unreleased SOMA POC uses upstream FK and renders all 77 transported joints +in SOMA's native public order. It uses a neutral identity, not subject +calibration. Separate hand tracking is unchanged. + +```text +SomaBodyJointRotationsV0 -> tracker -> upstream FK --+ + +-> SomaBodyInput -> viewer +SomaBodyJointPosesV0 ----> tracker -> direct map ----+ +``` + +Use SOMA-X 0.3.1 and its neutral identity assets. For an uninstalled source +build, set `PYTHONPATH` to the built IsaacCapture package and this example's +`python/` directory. + +```bash +uv pip install -e ./examples/deviceio_live_view + +uv run --no-sync python -m isaaccapture_examples.deviceio_live_view --accept-eula \ + --body-schema soma \ + --soma-data-root /path/to/SOMA-X/assets +``` + +Use the independent [SOMA body publisher](../soma_body_publisher/README.md) to +send the bundled motion, or connect another publisher using the checked-in SOMA +body schema. + +Select the body input at launch with `--body-schema full-body-pose` (the default) +or `--body-schema soma`. SOMA requires `--soma-data-root` for downstream FK. +For compatibility with earlier commands, supplying assets without `--body-schema` +also selects SOMA; an explicit schema always takes precedence. Switch sources by +restarting the viewer with the other option. Hands, head, and controllers remain +unchanged. + +The SOMA viewer reads the `soma_demo` collection by default. Use +`--soma-collection-id ` to match another publisher's collection. +Joint rotations are the default SOMA profile. Pass +`--soma-body-representation joint-poses` when the publisher sends evaluated +joint poses instead. The publisher and viewer options must match. + +`SomaBodySource` follows the normal DeviceIO source contract: the session discovers +the generated tracker selected for `SomaBodyJointRotationsV0` or +`SomaBodyJointPosesV0`. The raw tracker remains the transport and recording +boundary. The source evaluates rotations once per graph step against its +caller-supplied SOMA identity; evaluated poses map directly. Both modes emit +the same global positions, orientations, and validity as `SomaBodyInput`. The +viewer uses the joint names and hierarchy reported by the pinned SOMA layer. It +does not convert SOMA into `FullBodyPose`. + +SOMA-X and PyTorch are source-evaluation dependencies, not tracker or transport +dependencies. + +Missing data hides the body. For joint rotations, invalid translation invalidates +the complete skeleton. An invalid joint control also invalidates its descendants +because their global poses depend on that ancestor. Evaluated joint poses carry +independent per-joint validity. Other valid branches remain visible. +Independent hands, subject calibration, a `FullBodyPose` compatibility +retargeter, and recording/replay examples are outside this live-view POC. + +The focused tests cover upstream FK agreement, body-source selection, native +SOMA topology, and the shared renderer. Real-asset checks are optional: + +```bash +python -m pytest tests/python/examples/deviceio_live_view \ + --soma-assets /path/to/SOMA-X/assets +``` diff --git a/examples/deviceio_live_view/pyproject.toml b/examples/deviceio_live_view/pyproject.toml index 74fba527df..243b533ec3 100644 --- a/examples/deviceio_live_view/pyproject.toml +++ b/examples/deviceio_live_view/pyproject.toml @@ -17,6 +17,7 @@ requires-python = ">=3.11,<3.14" dependencies = [ "isaaccapture[cloudxr]", "numpy>=1.23.0", + "py-soma-x==0.3.1", "viser>=0.2.0", ] diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py new file mode 100644 index 0000000000..f317c8dda2 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py @@ -0,0 +1,69 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Body-schema selection contracts for the DeviceIO live viewer.""" + +from dataclasses import dataclass +from enum import Enum +from pathlib import Path + +from isaaccapture.retargeting_engine.interface.retargeter_core_types import ( + OutputSelector, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import SomaBodyRepresentation + + +class BodySchema(str, Enum): + FULL_BODY_POSE = "full-body-pose" + SOMA = "soma" + + +@dataclass(frozen=True) +class BodyViewLayout: + joint_names: tuple[str, ...] + bones: tuple[tuple[int, int], ...] + positions_index: int + valid_index: int + + +@dataclass(frozen=True) +class BodyViewPipeline: + output: OutputSelector + layout: BodyViewLayout + + +def resolve_body_schema( + body_schema: BodySchema | str | None, soma_data_root: Path | None +) -> BodySchema: + selected = ( + BodySchema(body_schema) + if body_schema is not None + else ( + BodySchema.SOMA if soma_data_root is not None else BodySchema.FULL_BODY_POSE + ) + ) + if selected is BodySchema.SOMA and soma_data_root is None: + raise ValueError("--body-schema soma requires --soma-data-root") + return selected + + +def create_body_view_pipeline( + soma_data_root: Path | None = None, + *, + body_schema: BodySchema | str | None = None, + soma_collection_id: str = "soma_demo", + soma_body_representation: SomaBodyRepresentation + | str = SomaBodyRepresentation.JOINT_ROTATIONS, +) -> BodyViewPipeline: + selected = resolve_body_schema(body_schema, soma_data_root) + if selected is BodySchema.FULL_BODY_POSE: + from .full_body_pose import create_full_body_pose_pipeline + + return create_full_body_pose_pipeline() + + from .soma_body import create_soma_body_pipeline + + assert soma_data_root is not None + return create_soma_body_pipeline( + soma_data_root, soma_collection_id, soma_body_representation + ) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py new file mode 100644 index 0000000000..96e5d2f326 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py @@ -0,0 +1,45 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Compose the human DeviceIO sources used by the live viewer.""" + +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + ControllersSource, + HandsSource, + HeadSource, +) +from isaaccapture.retargeting_engine.interface import OutputCombiner + +from .body_pipeline import BodyViewPipeline, create_body_view_pipeline + +HANDS_CHANNEL = "hands" + + +def build_all_human_pipeline( + soma_data_root=None, + *, + body_schema=None, + soma_collection_id="soma_demo", + soma_body_representation="joint-rotations", + body: BodyViewPipeline | None = None, +): + """Wire every human-related DeviceIO source into one pipeline.""" + hands = HandsSource(name=HANDS_CHANNEL) + head = HeadSource(name="head") + controllers = ControllersSource(name="controllers") + selected_body = body or create_body_view_pipeline( + soma_data_root, + body_schema=body_schema, + soma_collection_id=soma_collection_id, + soma_body_representation=soma_body_representation, + ) + return OutputCombiner( + { + "hand_left": hands.output(HandsSource.LEFT), + "hand_right": hands.output(HandsSource.RIGHT), + "head": head.output("head"), + "controller_left": controllers.output(ControllersSource.LEFT), + "controller_right": controllers.output(ControllersSource.RIGHT), + "body": selected_body.output, + } + ) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_viser.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_viser.py index c5679f42d8..6829eb31a3 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_viser.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_viser.py @@ -1,11 +1,9 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""DeviceIO-to-viser helpers for the live human-tracking viewer. +"""DeviceIO-to-viser rendering helpers for the live human-tracking viewer. -Pipeline builder: ``build_all_human_pipeline``. - -Viz classes: ``HandViz``, ``ControllerViz``, ``FullBodyViz``, ``HeadViz``, +Viz classes: ``HandViz``, ``ControllerViz``, ``BodyViz``, ``HeadViz``, ``HumanDeviceIOViz``. Rendering helpers: ``HAND_BONES``, ``BODY_BONES``, ``controller_state``. @@ -14,23 +12,17 @@ import numpy as np import viser -from isaaccapture.retargeting_engine.deviceio_source_nodes import ( - ControllersSource, - FullBodySource, - HandsSource, - HeadSource, -) -from isaaccapture.retargeting_engine.interface import OutputCombiner from isaaccapture.retargeting_engine.tensor_types import HandInputIndex from isaaccapture.retargeting_engine.tensor_types.indices import ( - BodyJointIndex, ControllerInputIndex, - FullBodyInputIndex, HeadInputIndex, ) -HANDS_CHANNEL = "hands" -BODY_JOINT_NAMES = [joint.name for joint in BodyJointIndex] +from .body_pipeline import BodyViewLayout +from .full_body_pose import FULL_BODY_POSE_BONES, FULL_BODY_POSE_LAYOUT + +BODY_JOINT_NAMES = list(FULL_BODY_POSE_LAYOUT.joint_names) +BODY_BONES = FULL_BODY_POSE_BONES # --------------------------------------------------------------------------- # Color palette shared across all viz scripts @@ -110,61 +102,6 @@ def setup_scene(server) -> GroundGrid: return GroundGrid(server, grid) -def build_all_human_pipeline(): - """Wire every human-related DeviceIO source into one pipeline.""" - hands = HandsSource(name=HANDS_CHANNEL) - head = HeadSource(name="head") - controllers = ControllersSource(name="controllers") - full_body = FullBodySource(name="full_body") - return OutputCombiner( - { - "hand_left": hands.output(HandsSource.LEFT), - "hand_right": hands.output(HandsSource.RIGHT), - "head": head.output("head"), - "controller_left": controllers.output(ControllersSource.LEFT), - "controller_right": controllers.output(ControllersSource.RIGHT), - "full_body": full_body.output(FullBodySource.FULL_BODY), - } - ) - - -# PICO body-joint connectivity (parent → child) for skeleton rendering. -# Indices follow BodyJointIndex: 0=PELVIS, 1/2=LEFT/RIGHT_HIP, 3/6/9=SPINE1/2/3, -# 4/5=LEFT/RIGHT_KNEE, 7/8=LEFT/RIGHT_ANKLE, 10/11=LEFT/RIGHT_FOOT, 12=NECK, -# 13/14=LEFT/RIGHT_COLLAR, 15=HEAD, 16/17=LEFT/RIGHT_SHOULDER, -# 18/19=LEFT/RIGHT_ELBOW, 20/21=LEFT/RIGHT_WRIST, 22/23=LEFT/RIGHT_HAND — 24 total. -BODY_BONES: tuple[tuple[int, int], ...] = ( - # Trunk and spine - (0, 1), - (0, 2), - (0, 3), - (3, 6), - (6, 9), - (9, 12), - (12, 15), - # Left leg - (1, 4), - (4, 7), - (7, 10), - # Right leg - (2, 5), - (5, 8), - (8, 11), - # Left arm - (12, 13), - (13, 16), - (16, 18), - (18, 20), - (20, 22), - # Right arm - (12, 14), - (14, 17), - (17, 19), - (19, 21), - (21, 23), -) - - # OpenXR hand-joint connectivity (parent → child) for skeleton rendering. # Indices follow XR_HAND_JOINT_*_EXT: 0=PALM, 1=WRIST, thumb has 4 joints # (no intermediate), the other 4 fingers have 5 joints each — 26 total. @@ -209,10 +146,14 @@ def _bone_segments(positions: np.ndarray) -> np.ndarray: ).astype(np.float32) -def _valid_bone_segments(positions: np.ndarray, valid: np.ndarray) -> np.ndarray: +def _valid_bone_segments( + positions: np.ndarray, + valid: np.ndarray, + bones: tuple[tuple[int, int], ...] = BODY_BONES, +) -> np.ndarray: """Return (N, 2, 3) segment array for body bones whose both endpoints are valid.""" segments: list[np.ndarray] = [] - for a, b in BODY_BONES: + for a, b in bones: if valid[a] and valid[b]: segments.append(np.stack([positions[a], positions[b]], axis=0)) if not segments: @@ -428,22 +369,23 @@ def update(self, state: dict) -> None: self.ray.colors = np.zeros((0, 2, 3), dtype=np.float32) -class FullBodyViz: - """Viser handles for full-body skeleton (joint cloud + skeleton segments).""" +class BodyViz: + """Viser handles for a body skeleton (joint cloud + skeleton segments).""" - def __init__(self, server: viser.ViserServer): + def __init__(self, server: viser.ViserServer, layout: BodyViewLayout): + self.layout = layout self.color = np.array(TRACKED_COLOR, dtype=np.float32) - zero_pts = np.zeros((len(BODY_JOINT_NAMES), 3), dtype=np.float32) + zero_pts = np.zeros((len(layout.joint_names), 3), dtype=np.float32) zero_segs = np.zeros((0, 2, 3), dtype=np.float32) self.points = server.scene.add_point_cloud( - name="/full_body/joints", + name="/body/joints", points=zero_pts, - colors=np.tile(self.color, (len(BODY_JOINT_NAMES), 1)), + colors=np.tile(self.color, (len(layout.joint_names), 1)), point_size=0.01, ) self.bones = server.scene.add_line_segments( - name="/full_body/bones", + name="/body/bones", points=zero_segs, colors=np.zeros((0, 2, 3), dtype=np.float32), line_width=2.0, @@ -451,9 +393,11 @@ def __init__(self, server: viser.ViserServer): def update(self, positions: np.ndarray | None, valid: np.ndarray | None) -> None: if positions is None or valid is None: - zero_pts = np.zeros((len(BODY_JOINT_NAMES), 3), dtype=np.float32) + zero_pts = np.zeros((len(self.layout.joint_names), 3), dtype=np.float32) self.points.points = zero_pts - self.points.colors = np.tile(INVALID_COLOR, (len(BODY_JOINT_NAMES), 1)) + self.points.colors = np.tile( + INVALID_COLOR, (len(self.layout.joint_names), 1) + ) self.bones.points = np.zeros((0, 2, 3), dtype=np.float32) self.bones.colors = np.zeros((0, 2, 3), dtype=np.float32) return @@ -466,7 +410,7 @@ def update(self, positions: np.ndarray | None, valid: np.ndarray | None) -> None point_colors[~valid_bool] = INVALID_COLOR self.points.colors = point_colors - segs = _valid_bone_segments(positions, valid_bool) + segs = _valid_bone_segments(positions, valid_bool, self.layout.bones) self.bones.points = segs self.bones.colors = np.tile(self.color, (segs.shape[0], 2, 1)) @@ -515,14 +459,20 @@ class HumanDeviceIOViz: Inactive or absent trackers are hidden instead of drawn in the invalid color. """ - def __init__(self, server: viser.ViserServer, ground: GroundGrid | None = None): + def __init__( + self, + server: viser.ViserServer, + ground: GroundGrid | None = None, + body_layout: BodyViewLayout = FULL_BODY_POSE_LAYOUT, + ): self._ground = ground + self._body_layout = body_layout self.hand_left = HandViz(server, "hand_left", LEFT_COLOR) self.hand_right = HandViz(server, "hand_right", RIGHT_COLOR) self.head = HeadViz(server) self.controller_left = ControllerViz(server, "controller_left", LEFT_COLOR) self.controller_right = ControllerViz(server, "controller_right", RIGHT_COLOR) - self.full_body = FullBodyViz(server) + self.body = BodyViz(server, body_layout) def _update_hand_if_active(self, viz: HandViz, hand) -> bool: if hand.is_none: @@ -551,34 +501,32 @@ def _update_controller_if_active(self, viz: ControllerViz, controller) -> bool: viz.update(state) return True - def _update_full_body_if_active(self, full_body) -> tuple[bool, int]: - if full_body.is_none: - self.full_body.points.visible = False - self.full_body.bones.visible = False + def _update_body_if_active(self, body) -> tuple[bool, int]: + if body.is_none: + self.body.points.visible = False + self.body.bones.visible = False return False, 0 positions = np.asarray( - full_body[FullBodyInputIndex.JOINT_POSITIONS], dtype=np.float32 + body[self._body_layout.positions_index], dtype=np.float32 ) - valid = np.asarray(full_body[FullBodyInputIndex.JOINT_VALID], dtype=np.uint8) + valid = np.asarray(body[self._body_layout.valid_index], dtype=np.uint8) n_valid = int(np.count_nonzero(valid)) if n_valid == 0: - self.full_body.points.visible = False - self.full_body.bones.visible = False + self.body.points.visible = False + self.body.bones.visible = False return False, 0 - self.full_body.points.visible = True - self.full_body.bones.visible = True - self.full_body.update(positions, valid) + self.body.points.visible = True + self.body.bones.visible = True + self.body.update(positions, valid) if self._ground is not None: self._ground.follow(positions, valid) return True, n_valid def update(self, result) -> dict[str, bool | int]: """Update every tracker; hide inactive ones. Returns active flags.""" - full_body_active, full_body_joints = self._update_full_body_if_active( - result["full_body"] - ) + body_active, body_joints = self._update_body_if_active(result["body"]) return { "hand_left": self._update_hand_if_active( self.hand_left, result["hand_left"] @@ -593,6 +541,6 @@ def update(self, result) -> dict[str, bool | int]: "controller_right": self._update_controller_if_active( self.controller_right, result["controller_right"] ), - "full_body_active": full_body_active, - "full_body_joints": full_body_joints, + "body_active": body_active, + "body_joints": body_joints, } diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/full_body_pose.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/full_body_pose.py new file mode 100644 index 0000000000..624b175491 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/full_body_pose.py @@ -0,0 +1,64 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""FullBodyPose source and skeleton layout for the DeviceIO live viewer.""" + +from isaaccapture.retargeting_engine.deviceio_source_nodes import FullBodySource +from isaaccapture.retargeting_engine.tensor_types import ( + BodyJointIndex, + FullBodyInputIndex, +) + +from .body_pipeline import BodyViewLayout, BodyViewPipeline + + +# PICO body-joint connectivity (parent → child) for skeleton rendering. +# Indices follow BodyJointIndex: 0=PELVIS, 1/2=LEFT/RIGHT_HIP, 3/6/9=SPINE1/2/3, +# 4/5=LEFT/RIGHT_KNEE, 7/8=LEFT/RIGHT_ANKLE, 10/11=LEFT/RIGHT_FOOT, 12=NECK, +# 13/14=LEFT/RIGHT_COLLAR, 15=HEAD, 16/17=LEFT/RIGHT_SHOULDER, +# 18/19=LEFT/RIGHT_ELBOW, 20/21=LEFT/RIGHT_WRIST, 22/23=LEFT/RIGHT_HAND (24 total). +FULL_BODY_POSE_BONES: tuple[tuple[int, int], ...] = ( + # Trunk and spine + (0, 1), + (0, 2), + (0, 3), + (3, 6), + (6, 9), + (9, 12), + (12, 15), + # Left leg + (1, 4), + (4, 7), + (7, 10), + # Right leg + (2, 5), + (5, 8), + (8, 11), + # Left arm + (12, 13), + (13, 16), + (16, 18), + (18, 20), + (20, 22), + # Right arm + (12, 14), + (14, 17), + (17, 19), + (19, 21), + (21, 23), +) + +FULL_BODY_POSE_LAYOUT = BodyViewLayout( + joint_names=tuple(joint.name for joint in BodyJointIndex), + bones=FULL_BODY_POSE_BONES, + positions_index=int(FullBodyInputIndex.JOINT_POSITIONS), + valid_index=int(FullBodyInputIndex.JOINT_VALID), +) + + +def create_full_body_pose_pipeline() -> BodyViewPipeline: + source = FullBodySource("body") + return BodyViewPipeline( + output=source.output(FullBodySource.FULL_BODY), + layout=FULL_BODY_POSE_LAYOUT, + ) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py index c50022d352..bc0f7d3b2d 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py @@ -20,18 +20,20 @@ import argparse import time +from pathlib import Path import viser from isaaccapture.cloudxr import CloudXRLauncher -from isaaccapture.teleop_session_manager import TeleopSession, TeleopSessionConfig - -from .deviceio_viser import ( - BODY_JOINT_NAMES, - HumanDeviceIOViz, - build_all_human_pipeline, - setup_scene, +from isaaccapture.teleop_session_manager import ( + TeleopSession, + TeleopSessionConfig, ) +from isaaccapture.retargeting_engine.deviceio_source_nodes import SomaBodyRepresentation + +from .body_pipeline import BodySchema, create_body_view_pipeline, resolve_body_schema +from .deviceio_pipeline import build_all_human_pipeline +from .deviceio_viser import HumanDeviceIOViz, setup_scene def main(argv: list[str]) -> int: @@ -42,15 +44,43 @@ def main(argv: list[str]) -> int: help="Viser HTTP bind address (default: 0.0.0.0, all interfaces; pass 127.0.0.1 to keep it local)", ) parser.add_argument("--port", type=int, default=8080, help="Viser HTTP port") + parser.add_argument( + "--body-schema", + choices=[schema.value for schema in BodySchema], + help="Input body schema (default: full-body-pose, or soma when assets are supplied)", + ) + parser.add_argument("--soma-data-root", type=Path, help="SOMA POC assets directory") + parser.add_argument( + "--soma-collection-id", + default="soma_demo", + help="SOMA publisher's tensor collection ID", + ) + parser.add_argument( + "--soma-body-representation", + choices=[representation.value for representation in SomaBodyRepresentation], + default=SomaBodyRepresentation.JOINT_ROTATIONS.value, + help="SOMA FlatBuffer profile consumed by the viewer", + ) CloudXRLauncher.add_launcher_arguments(parser) args = parser.parse_args(argv[1:]) + try: + body_schema = resolve_body_schema(args.body_schema, args.soma_data_root) + body = create_body_view_pipeline( + args.soma_data_root, + body_schema=body_schema, + soma_collection_id=args.soma_collection_id, + soma_body_representation=args.soma_body_representation, + ) + pipeline = build_all_human_pipeline(body=body) + except ValueError as error: + parser.error(str(error)) + server = viser.ViserServer(host=args.host, port=args.port) ground = setup_scene(server) - config = TeleopSessionConfig( app_name="LiveDeviceIOExample", - pipeline=build_all_human_pipeline(), + pipeline=pipeline, ) with CloudXRLauncher.launch_context(args) as launcher: @@ -59,18 +89,19 @@ def main(argv: list[str]) -> int: print("[live] waiting for headset connection… (Ctrl+C to stop)") with TeleopSession(config) as session: - viz = HumanDeviceIOViz(server, ground) + viz = HumanDeviceIOViz(server, ground, body.layout) print( f"[live] viser listening on {args.host}:{args.port} " f"(http://localhost:{args.port})" ) + print(f"[live] body schema: {body_schema.value}") try: while True: result = session.step() active = viz.update(result) if session.frame_count % 60 == 0: - body_joints = active["full_body_joints"] + body_joints = active["body_joints"] print( f"[live] frame={session.frame_count} " f"hands(L/R)={'Y' if active['hand_left'] else '-'}/" @@ -78,7 +109,7 @@ def main(argv: list[str]) -> int: f"head={'Y' if active['head'] else '-'} " f"ctrl(L/R)={'Y' if active['controller_left'] else '-'}/" f"{'Y' if active['controller_right'] else '-'} " - f"body={body_joints:02d}/{len(BODY_JOINT_NAMES)}" + f"body={body_joints:02d}/{len(body.layout.joint_names)}" ) time.sleep(1 / 60) except KeyboardInterrupt: diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py new file mode 100644 index 0000000000..c5787d9adb --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py @@ -0,0 +1,69 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""SOMA body source and layout for the DeviceIO live viewer.""" + +from pathlib import Path + +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + SomaBodyRepresentation, + SomaBodySource, +) +from isaaccapture.retargeting_engine.tensor_types import SomaBodyInputIndex +from isaaccapture.schema import SomaBodyJointV0 + +from .body_pipeline import BodyViewLayout, BodyViewPipeline + + +def create_layer(data_root: Path): + import soma + import torch + + if soma.__version__ != "0.3.1": + raise RuntimeError(f"The POC requires SOMA-X 0.3.1, got {soma.__version__}") + layer = soma.SOMALayer( + data_root=str(data_root), + identity_model_type="soma", + device="cpu", + lod="low", + output_unit=soma.Unit.METERS, + enable_procedural_transforms=False, + correctives_model_path=None, + ) + expected = [ + name.replace("_", "") + for name, joint in sorted( + SomaBodyJointV0.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ] + if [name.upper() for name in layer.public_joint_names] != ["ROOT", *expected]: + raise ValueError("SOMA layer joint order differs from the V0 FBS") + with torch.no_grad(): + layer.prepare_identity( + torch.zeros((1, layer.identity_model.num_identity_coeffs)) + ) + return layer + + +def create_soma_body_pipeline( + data_root: Path, + collection_id: str, + representation: SomaBodyRepresentation + | str = SomaBodyRepresentation.JOINT_ROTATIONS, +) -> BodyViewPipeline: + source = SomaBodySource( + "body", + collection_id, + create_layer(data_root), + representation=representation, + ) + return BodyViewPipeline( + output=source.output(SomaBodySource.BODY), + layout=BodyViewLayout( + joint_names=source.joint_names, + bones=source.bones, + positions_index=int(SomaBodyInputIndex.JOINT_POSITIONS), + valid_index=int(SomaBodyInputIndex.JOINT_VALID), + ), + ) diff --git a/examples/soma_body_publisher/CMakeLists.txt b/examples/soma_body_publisher/CMakeLists.txt new file mode 100644 index 0000000000..ff899a019b --- /dev/null +++ b/examples/soma_body_publisher/CMakeLists.txt @@ -0,0 +1,9 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +add_subdirectory(cpp/soma_body_pusher) + +include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) +install_python_example(DESTINATION examples/soma_body_publisher) diff --git a/examples/soma_body_publisher/README.md b/examples/soma_body_publisher/README.md new file mode 100644 index 0000000000..50d44babd5 --- /dev/null +++ b/examples/soma_body_publisher/README.md @@ -0,0 +1,49 @@ + + +# SOMA Body Publisher + +Publishes the motion bundled with SOMA-X through the Isaac Teleop SOMA body +FlatBuffer and `SchemaPusher`. This is a deterministic input generator for +testing SOMA consumers. It is not a vendor plugin or vendor network protocol. + +The Python process converts the bundled motion into the pinned SOMA-X v0.3.1 +pose contract and writes framed FlatBuffers to the native `soma_body_pusher`. +It can publish joint rotations for downstream FK or pre-evaluated joint poses. +The native process validates the selected payload and publishes the `soma_demo` +tensor collection. + +```text +SOMA-X example_animation.npy + -> Python SOMA body encoder + -> soma_body_pusher + -> SchemaPusher collection soma_demo +``` + +Install the example and build the native pusher: + +```bash +uv pip install -e ./examples/soma_body_publisher +cmake --build ./build --target soma_body_pusher +``` + +Start a SOMA consumer such as `deviceio_live_view`, connect its client, then run: + +```bash +source ~/.cloudxr/run/cloudxr.env +uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ + --data-root /path/to/SOMA-X/assets \ + --pusher ./build/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher \ + --loop +``` + +Joint rotations are the default. To evaluate the animation before transport, +add `--body-representation joint-poses`. Configure the consumer with the same +representation. The two modes publish `soma_body_joint_rotations_v0` and +`soma_body_joint_poses_v0`, respectively. + +The assets directory must contain `example_animation.npy` and the neutral SOMA +identity assets. Add `--validate-only` to process every bundled frame through +the FlatBuffer verifier without starting OpenXR or publishing data. diff --git a/examples/soma_body_publisher/cpp/soma_body_pusher/CMakeLists.txt b/examples/soma_body_publisher/cpp/soma_body_pusher/CMakeLists.txt new file mode 100644 index 0000000000..e4cd8a57de --- /dev/null +++ b/examples/soma_body_publisher/cpp/soma_body_pusher/CMakeLists.txt @@ -0,0 +1,15 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +find_package(Threads REQUIRED) + +add_executable(soma_body_pusher soma_body_pusher.cpp) +target_link_libraries(soma_body_pusher PRIVATE + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema + Threads::Threads +) +install(TARGETS soma_body_pusher + RUNTIME DESTINATION examples/soma_body_publisher/cpp +) diff --git a/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp b/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp new file mode 100644 index 0000000000..e20e2665d9 --- /dev/null +++ b/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp @@ -0,0 +1,150 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +enum class BodyRepresentation +{ + JointRotations, + JointPoses, +}; + +struct Arguments +{ + bool validate_only = false; + BodyRepresentation representation = BodyRepresentation::JointRotations; +}; + +Arguments parse_arguments(int argc, char** argv) +{ + Arguments arguments; + for (int index = 1; index < argc; ++index) + { + const std::string argument = argv[index]; + if (argument == "--validate-only") + { + arguments.validate_only = true; + } + else if (argument == "--body-representation" && index + 1 < argc) + { + const std::string value = argv[++index]; + if (value == "joint-rotations") + { + arguments.representation = BodyRepresentation::JointRotations; + } + else if (value == "joint-poses") + { + arguments.representation = BodyRepresentation::JointPoses; + } + else + { + throw std::invalid_argument("Unknown SOMA body representation: " + value); + } + } + else + { + throw std::invalid_argument("Unknown or incomplete argument: " + argument); + } + } + return arguments; +} + +uint64_t little_endian(const uint8_t* bytes, size_t size) +{ + uint64_t value = 0; + for (size_t i = 0; i < size; ++i) + { + value |= static_cast(bytes[i]) << (8 * i); + } + return value; +} + +} // namespace + +int main(int argc, char** argv) +{ + if (argc == 2 && std::string(argv[1]) == "--help") + { + std::cout << "Usage: soma_body_pusher [--body-representation joint-rotations|joint-poses] " + "[--validate-only]\n" + "Reads framed SOMA body FlatBuffers from stdin.\n"; + return 0; + } + auto logger = isaaccapture::Logger::get("isaaccapture.examples.soma_body_pusher"); + try + { + const auto arguments = parse_arguments(argc, argv); + const bool joint_poses = arguments.representation == BodyRepresentation::JointPoses; + const size_t max_flatbuffer_size = joint_poses ? 4096 : 2048; + const char* tensor_identifier = joint_poses ? "soma_body_joint_poses_v0" : "soma_body_joint_rotations_v0"; + std::unique_ptr session; + std::unique_ptr pusher; + size_t frames = 0; + std::array header{}; + while (std::cin.read(reinterpret_cast(header.data()), header.size())) + { + // Local pipe framing only: byte count + clip-relative device time, both little-endian. + const auto size = little_endian(header.data(), 4); + const auto raw_time = little_endian(header.data() + 4, 8); + if (size == 0 || size > max_flatbuffer_size || raw_time > INT64_MAX) + { + throw std::invalid_argument("Invalid SOMA body demo packet header"); + } + std::vector bytes(size); + if (!std::cin.read(reinterpret_cast(bytes.data()), bytes.size())) + { + throw std::invalid_argument("Truncated SOMA body demo payload"); + } + flatbuffers::Verifier verifier(bytes.data(), bytes.size()); + const bool valid = joint_poses ? verifier.VerifyBuffer(nullptr) : + verifier.VerifyBuffer(nullptr); + if (!valid) + { + throw std::invalid_argument("Invalid SOMA body FlatBuffer"); + } + if (!arguments.validate_only) + { + if (!pusher) + { + session = std::make_unique( + "SomaBodyDemoPublisher", core::SchemaPusher::get_required_extensions(), false); + pusher = std::make_unique( + session->get_handles(), core::SchemaPusherConfig{ .collection_id = "soma_demo", + .max_flatbuffer_size = max_flatbuffer_size, + .tensor_identifier = tensor_identifier, + .localized_name = "SOMA Body Demo" }); + } + pusher->push_buffer( + bytes.data(), bytes.size(), core::os_monotonic_now_ns(), static_cast(raw_time)); + } + ++frames; + } + if (std::cin.gcount() != 0) + { + throw std::invalid_argument("Truncated SOMA body demo packet header"); + } + logger->info("{} {} SOMA body demo frames", arguments.validate_only ? "Validated" : "Published", frames); + return 0; + } + catch (const std::exception& error) + { + logger->error("{}", error.what()); + return 1; + } +} diff --git a/examples/soma_body_publisher/pyproject.toml b/examples/soma_body_publisher/pyproject.toml new file mode 100644 index 0000000000..30a18ac0d9 --- /dev/null +++ b/examples/soma_body_publisher/pyproject.toml @@ -0,0 +1,24 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[project] +name = "isaaccapture-examples-soma-body-publisher" +version = "0.0.0" +description = "Isaac Teleop SOMA body demo publisher" +requires-python = ">=3.11,<3.14" +dependencies = [ + "isaaccapture", + "numpy>=1.23.0", + "py-soma-x==0.3.1", +] + +[tool.hatch.build.targets.wheel] +only-include = ["python/isaaccapture_examples/soma_body_publisher"] +sources = ["python"] + +[tool.uv.sources] +isaaccapture = { path = "../..", editable = true } diff --git a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/__init__.py b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/__init__.py new file mode 100644 index 0000000000..52a7a9daf0 --- /dev/null +++ b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/__init__.py @@ -0,0 +1,2 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 diff --git a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/__main__.py b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/__main__.py new file mode 100644 index 0000000000..72d96631d8 --- /dev/null +++ b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/__main__.py @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys + +from .publisher import main + +sys.exit(main(sys.argv)) diff --git a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py new file mode 100644 index 0000000000..3afaf1d918 --- /dev/null +++ b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py @@ -0,0 +1,187 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Publish the bundled SOMA motion through the SOMA body transport.""" + +import argparse +import logging +import math +import signal +import struct +import subprocess +import sys +import time +from pathlib import Path + +import numpy as np + +from isaaccapture.schema import ( + Point, + SomaBodyJointPoseArrayV0, + SomaBodyJointPosesV0, + SomaBodyJointRotationArrayV0, + SomaBodyJointV0, + SomaBodyJointRotationsV0, +) + +logger = logging.getLogger("isaaccapture.examples.soma_body_publisher") + + +def create_layer(data_root: Path): + """Create the neutral SOMA-X v0.3.1 model used by the bundled motion.""" + import soma + import torch + + if soma.__version__ != "0.3.1": + raise RuntimeError(f"The POC requires SOMA-X 0.3.1, got {soma.__version__}") + layer = soma.SOMALayer( + data_root=str(data_root), + identity_model_type="soma", + device="cpu", + lod="low", + output_unit=soma.Unit.METERS, + enable_procedural_transforms=False, + correctives_model_path=None, + ) + expected = [ + name.replace("_", "") + for name, joint in sorted( + SomaBodyJointV0.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ] + if [name.upper() for name in layer.public_joint_names] != ["ROOT", *expected]: + raise ValueError("SOMA layer joint order differs from the V0 FBS") + with torch.no_grad(): + layer.prepare_identity( + torch.zeros((1, layer.identity_model.num_identity_coeffs)) + ) + return layer + + +def demo_controls(path: Path, layer): + """Normalize the bundled motion as in v0.3.1 tools/demo_soma_vis.py.""" + import torch + from soma.geometry.rig_utils import joint_local_to_world, joint_world_to_local + from soma.geometry.transforms import matrix_to_quaternion_xyzw + + motion = np.load(path, allow_pickle=False) + if ( + motion.ndim != 4 + or motion.shape[1] not in (78, 94) + or motion.shape[2:] != (4, 4) + or not len(motion) + ): + raise ValueError("Expected bundled SOMA motion shaped (T, 78 or 94, 4, 4)") + local = torch.as_tensor(motion[..., :3, :3], dtype=torch.float32) + if local.shape[1] == 94: + twists = (*range(40, 44), *range(72, 76), *range(81, 85), *range(90, 94)) + local = local[:, [i for i in range(94) if i not in twists]] + orient = layer.t_pose_world[layer.public_transform_joint_indices, :3, :3] + world = joint_local_to_world(local, layer.public_joint_parent_ids) + relative = joint_world_to_local( + world @ orient.transpose(-2, -1), layer.public_joint_parent_ids + )[:, 1:] + return matrix_to_quaternion_xyzw(relative).numpy(), motion[:, 1, :3, 3], relative + + +def soma_joint_rotations(quaternions, translation): + joints = SomaBodyJointRotationArrayV0() + joints.rotations[:] = quaternions + joints.is_valid[:] = 1 + return SomaBodyJointRotationsV0(joints, Point(*translation), True) + + +def evaluate_demo_controls(layer, rotations, translations): + """Evaluate a batch of SOMA controls into global public joint poses.""" + import torch + from soma.geometry.transforms import matrix_to_quaternion_xyzw + + with torch.no_grad(): + output = layer.pose( + rotations, + transl=torch.as_tensor(translations, dtype=torch.float32), + pose2rot=False, + fk_only=True, + apply_correctives=False, + ) + orientations = matrix_to_quaternion_xyzw(output["transforms"][:, 1:, :3, :3]) + return output["joints"].numpy(), orientations.numpy() + + +def soma_joint_poses(positions, orientations): + joints = SomaBodyJointPoseArrayV0() + joints.positions[:] = positions + joints.orientations[:] = orientations + joints.is_valid[:] = 1 + return SomaBodyJointPosesV0(joints) + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--data-root", required=True, type=Path) + parser.add_argument( + "--pusher", required=True, type=Path, help="Built soma_body_pusher executable" + ) + parser.add_argument("--rate", type=float, default=30.0) + parser.add_argument("--loop", action="store_true") + parser.add_argument( + "--body-representation", + choices=("joint-rotations", "joint-poses"), + default="joint-rotations", + help="SOMA FlatBuffer profile to publish (default: joint-rotations)", + ) + parser.add_argument( + "--validate-only", + action="store_true", + help="Validate FBS packets without OpenXR", + ) + args = parser.parse_args(argv[1:]) + if not math.isfinite(args.rate) or args.rate <= 0: + parser.error("rate must be finite and positive") + layer = create_layer(args.data_root) + q, t, rotations = demo_controls(args.data_root / "example_animation.npy", layer) + if args.body_representation == "joint-poses": + positions, orientations = evaluate_demo_controls(layer, rotations, t) + frame_values = positions, orientations + else: + frame_values = q, t + command = [ + str(args.pusher.resolve()), + "--body-representation", + args.body_representation, + ] + (["--validate-only"] if args.validate_only else []) + with subprocess.Popen(command, stdin=subprocess.PIPE) as process: + interrupted = False + try: + start = time.monotonic() + frame = 0 + while True: + for first, second in zip(*frame_values): + remaining = start + frame / args.rate - time.monotonic() + if remaining > 0 and not args.validate_only: + time.sleep(remaining) + if args.body_representation == "joint-poses": + payload = soma_joint_poses(first, second).to_bytes() + else: + payload = soma_joint_rotations(first, second).to_bytes() + process.stdin.write( + struct.pack("o726owz*fB^v_F_aehiDj> zNs+>Zixep+T%=IpB87_-6e&`;aOon2iWDwfxbzGL3>Yw&|KHoaeO5Hm%lujE-TQX; zz1!95E+rxZg9D{5NlAxvNYIzz&oXe!%FjDHTo*`NhUtLS z;Imx`QkCMV;mXiRzF00{E*m>LY4}iTDL~HZia|+bEPo;_SIQ$nB`D_bc|ma}9fURk zzZ(m;MsWlEr2x6&x|`pS<&G(a^NYqB92r5v9LL4yZTxi8_)?fJB7YSh+W4A^$E~&f z7lIS`yZVdIpP!x86~kHaQw-;4m?(WS zj*DmAY17T(qx5A(^?y7Hmzs*_Qa1fZ=}YxjpZ|c3XH!=U*IRT?!v12eGMX=*JY$!E z6u`Ur^;=B6Y=mXt}x89Lpk! zO`qQfmb=rMV_9SYTpBI&;JM(8{ka^Uf>Y%xfCCWql4n&e)%08|qUIPYn;a`Yfq&-?8wlVjIRaEc4t}T(9L8+i=V?`=_tZ+m_pE z!!gfE#l2;@416v}f8O!C6+e=;d|lqO@odHwk+GoCKZb+8uEy!H+?-{W#M=E zai9Cz>^df70da1mnQQx`)OWr+&i=h#qF+(KN?mP$C}e>irYP2d@M-Uzdk?4(C)V8 zOnwE$ai7wX@HNlRjA!zjQ5?T#X`$lc=YZ%~kko&Ezw^1m*=5DV&l~<&kaaZAj*I6tTBzdVc}3y! zlZxjx+V?6RkJB>L|GBKFI6R^otEo7$VbGs*zay$UzOHzLkFBnl4$JW3yo%owN8{Xv z-{newF*tsb3uF?0mnZw7WtzltQ6%twrz3tt6P*WKi_Fa#`_tnb4}($}7wc*qyhboK zr(&5~2~TbTCY3O6dAJ(*CMbH3#FqWi?lj{t!36$AIYb=KV}2JTO)n(*lBd(0Wp0k9 KcKOi~ul*0&VX|ic literal 0 HcmV?d00001 diff --git a/src/core/schema/golden/soma_body_joint_poses_v0.bfbs b/src/core/schema/golden/soma_body_joint_poses_v0.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..65dbe073308241e480972f85c5acb7ccb33c491a GIT binary patch literal 6696 zcmai2J#<@16~4-^*%eruHDJIjS}!`_ujelM(;&MWNdtF295N{fSi#o2}=amy%Lo^v?3!w92fxlfW+$p4Ur5G0zOSR zwhMFtTb~y>=oLBqg2?_CVH1Y8fyc0QbV1}CbXI_y0PPL~P=`MvPyWzotb@;M$`0;p z;PU=7@Dw)hqOC$_A7f=)JKaOvKfrwMUjEo&XSg&TI(s zL5ZM8?gI%R0@RV<8c+s?fiUpATVw;c4a9+$pF=KyH6Q~-fcj@e)`1&96gYtM zkAN~T1RTAJd;wKp7&v|f>j10*DIg3y`;5r%f%kzvU?0wJ0*k;U1_JCLs5Kw~bOF0q zz=uE%hyeAo@B_F3M1eg-Tmy>00Pq3}_ZX-G!@w~XV;fijQa}%|kK}9sw}Chl1?-}E z2;_h$fHIKT%2Mg#43=OFD{!?kw@`g=2;H*x=SG9|im+vZ9grIOtSLdLlD$5WzdD)D z=CA{*hmA%QeMq&bK{Q6{ii4EOboxpvm(NWm^T})i_nV4$&7+wAVf4GXur!J{Hae3; ztQdFxUA4Rs#i4)RT;r3I2$=IY{j}9zY^gt!N@o$j(ob3an$pM8TKk#g6`qv=-F{{s}t~jhWZ%tC8*+hOSoxS#fEd$Ab@BACJ{0RD7 z4zR61!HuJDRq;kFFV%tf4)nGZ?_JBg-+}iw^o|to=a%=h1Me;94gM{N?`M{GsCamk z>-eVA*()YBMa8>hc?0NkdEtIjIVF#2k&hMcr z4K=S>DCpi++@^hHxb$yN@ytS@uhxM_|9bur#5sg}%JU99`jt_1B|Y?^t~{Ma2ou)$z&cWPWrS2Yp@5Ghlh^iW8iB zMqSEko^Pvpb`>X>XC^gyJ(EaI@e zipRQf5=Xy_kNZ5x&52_|HlgS0?JJhNsJPtc&0H4Ff#QAH@*Z{Iv2gnS6U@i=9LjzN z9t$U@czC5U(u+P9V=F&dI1dyLuVhB16z}AI3XX^!#lx$eku}9Txu5*wL5>xV=QCxy zJa5UcPvQX|API__X;N~ zisQd;1jmBBQ1kFThVoc({PzLxSddjU4_=jhIZzybp5R!J9C&V@@=T-jqR-_aIDe8K z@V$rgIDL*$QcB* zzSI@xjOB1}L+PLOwQ+8v-^I%QZ1VCo7DyTWE>7B^47Jc@QH1gJ=ZtxC=l3Y@JYX#{ zHYc>F=ee9p&g3vySM%UAf{}Hl%h+b{=9Yw0Ve^%TGkEhj|7e%;tQ8LqhVkdc;PH4K z^H~rv-jd0WJYD82b3U5frKe^%c0>8d@n`r)UvB~f0MD^ce%F|)ESHww8=5X*s3<+y zlTZtV@b!$(S=!&ka|n&uezePas?N=qmaB#NMO?39UJ0G?_4lkkb3%REC>E*(G>GTq z7cfT07&#X+M*Uv=V><%yjQCWaG1dpZmn~gg7?88(yQ221>c!Sge`ufk@7MgalV4;G zWX1D4#PwobP_Kqy*U(a*Ipo>g+Mm&-rNTY)k*EuMnH1`K>lcBov+$EX@=K`5Yuyv* z@#bLfokICTxPTwf9yfu@xE+t$mcJ+TcR}U5|8bD(O>PxnPFwp*H{XD7^yM7`R_!7Hb;-=y<9=nO+A1Pe}wXBN7BJNIxih(0&o z-!y)g^B{|?jq`|(daQJ>l~5daQGh=3S;>}b!1cyc%-<)plLE+NJ=ise3cg=pP94Xi rCLb7wR}(yo&Y5|1OwXJ2858~Gxx_W=2RJX!ELZE)u(j^zfU+ literal 0 HcmV?d00001 diff --git a/src/core/schema/golden/soma_body_v0.bfbs b/src/core/schema/golden/soma_body_v0.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..ab60b6b5bdd875ef27f12687877068321562400c GIT binary patch literal 6696 zcmai2JycxB6}~GWiP)Qf&qr_N??M*@w5)1Se?Mll+VCBf?=$!S6#SzKE zDzKKV9EubaDkxN_a6zGhLWK$!6fP)Ks8FGTf&fCprrqpnB}hyl%n z?GJ%tz|!*~nDPvZG~aNM)V3B<^32=YPc zM2*NTbU2E#51{UMfn;*7I$bJWsg`dPC#uua)tTb;-qRmknL`^h$=Cxsop?54x}onx z9=Ly8VM9&p?0&2g4B#!G z0CWQDN8kr=8SsD&M0^J*1E+xP!x(p<2J``Yhu{Y=4`hG@u!-a>16P3*69qg%aSPyB zdH{x8CaSZOr^hh_gBXGH)v1}kC7pc!LAuY0PWo z-+9aHR~-5m^ffdxf`GS_e%9(&4%8pdWb=ss%P9Vg)n8Kj7+PyTmp+Hzap-#}89$$$ ztSJt2nM)7p@WofWAxA>lj(QhA^A_@g@MT5$hjbX}R~-6hI6?TbrSzG1O64H^Abd%D z6~%uxP?sg8$e2jNQ``X1^S{~+LGO>sEif-%Vq$kja)VaJc-zul%F)gyLc)zy1%guRAi^O-L_EJP#$~;JM(W9rj#|ID&MH!&%?Yx&32R_4^A;rZ0}mb4TST;37jiiuZHNn{UNK zhI;-H<>zOXx6+D-43!nn3=p{6t$4@~c^-6`A2UEGDW_2HVocy!#hA_e=}T^1ZK!_D zKtY!_9k*d!87}=x{44~?TJY#!PVsv2Oj&QiqhAY(mozNQMbtI(lYWusp=5qe z15PrE+mN3^=Hl?6%g>(b_YF&?&2Q+}a2aRn>nJ}xmbco1$2cpB_qyfnwBRw$WySlc z<@KP>`5B#e+*>hLX!D@Qy!^!Kvv^-|qH}d10%K@!H$KP00hrd)bjt-fYTyTlN(1CB?hnl=qgT{vG-G1I62I%6n5PipTd- zC%ve5x#2pGamVc;BoupfLXhq9?SVL#EaAd9LWo@tZ>>RcY8^C$ft zzk61dKHDh1(hvO#)MZ=gvyD$^L_LZZpXf%GlpeX6^nmZ)BjV8aP{(-j z2_MRu;;+DciqfYYC%p&gb7HKc-tBjciE_C?_iQFT zUO;`^+}3bZd4GjH&(w+F8o|hI#ba#a_;O3bsW@(#+6*J$d1zz&^xw(aL0mW($6pXf z!0iTs`$4C%7qEGQZk8E#;UbaZK^sm_ipdl zB$~_820KuV#V~5zZ`sER_9GN#8PYcARc&f|a;{dIzJ}kM7*mP0h5B1opIql>xl}9R z5i#w*31fte(*u}m>i6Rx$7m6t?;8Nm#-{$G{XWiV=JFNb zoAWp@J6pP8F1*$QKVq~W%)?IDKY}#QJ0j-LwFP3Qyj zT)JMGs+6ucHx4whYxCR>eU1-v$$Y!gj%DLjuvyza^zHy*zIbnG?kwH9=1{%_dmOK$ zW*nfd}MBB9C<`I5$aIfOn zvEI#e{FIY+KRScow86a4A6F)mfBvvko1C4&E@rREO9|1h~xTiQBv)3t|zd)xCIx+O>bnG4)+PMLp9_YCC?jWYH{bTln zEHc-}5FPb6szE8S6rMbQF~@DmcZg*28h3_p4be^pAdl|~Cf-r1U=MAG<8Ffwj3e0l kcn%#k{pgq;H|R4a`pYwkW5#^5U!GTP?oh+doqe(XAB}tg=Kufz literal 0 HcmV?d00001 diff --git a/src/core/schema/golden/soma_common_v0.bfbs b/src/core/schema/golden/soma_common_v0.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..96cd221eb69e28fba6a72b0a56eddc6586dfdf21 GIT binary patch literal 716 zcmZvZy-or_6oqGj2vHKZpr9aD78Zu3u(UA5SlB6vl?hn|lTBb(Va1=?2ch&4d=Lu@ z3rk-@$L|abBJpnS+`qGT?mg2m#L8%5)67Ebqxtm8>5Vi)nLG5q ze0ne9e+vpU%M#Z--b-bM{;O50zTjW0f22aGmHvH~) z1n=YuOyx1pE0{+xVoh_G;Jr#5;CcMp3jTrm3|qm@<`g#MPyY@=Xh&o*%RRvlq54mx literal 0 HcmV?d00001 diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index 8c1f7eebe1..2a0319ded9 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -19,6 +19,9 @@ pybind11_add_module(schema_py schema_array_views.h schema_serialized.h se3_tracker_bindings.h + soma_common_v0_bindings.h + soma_body_joint_poses_v0_bindings.h + soma_body_v0_bindings.h schema_module.cpp steering_wheel_bindings.h ) diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index 9caf5cc65b..05078ba533 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -21,6 +21,9 @@ #include "plugin_device_status_bindings.h" #include "pose_bindings.h" #include "se3_tracker_bindings.h" +#include "soma_body_joint_poses_v0_bindings.h" +#include "soma_body_v0_bindings.h" +#include "soma_common_v0_bindings.h" #include "steering_wheel_bindings.h" #include "timestamp_bindings.h" @@ -81,4 +84,9 @@ PYBIND11_MODULE(_schema, m) // Bind full body types (BodyJointPose, BodyJoints structs, FullBodyPose table). core::bind_full_body(m); + + // Bind shared SOMA primitives, then the versioned body pose contracts. + core::bind_soma_common_v0(m); + core::bind_soma_body_v0(m); + core::bind_soma_body_joint_poses_v0(m); } diff --git a/src/core/schema/python/schema_serialized.h b/src/core/schema/python/schema_serialized.h index a97912028d..3fcb8a5855 100644 --- a/src/core/schema/python/schema_serialized.h +++ b/src/core/schema/python/schema_serialized.h @@ -39,7 +39,23 @@ namespace core template py::class_> serialized_class(py::module& m, const char* name, const char* doc) { - return py::class_>(m, name, doc); + return py::class_>(m, name, doc) + .def( + "to_bytes", + [](const Serialized& self) + { + auto encoded = self; + // A nested view's owner may be a Record, not a buffer rooted at T. + if (encoded.buffer().empty()) + { + typename T::NativeTableType native; + self->UnPackTo(&native); + encoded = pack(native); + } + const auto bytes = encoded.buffer(); + return py::bytes(reinterpret_cast(bytes.data()), bytes.size()); + }, + "Copy this table into standalone FlatBuffer bytes, including when read from a record."); } /*! diff --git a/src/core/schema/python/soma_body_joint_poses_v0_bindings.h b/src/core/schema/python/soma_body_joint_poses_v0_bindings.h new file mode 100644 index 0000000000..22249d9b5e --- /dev/null +++ b/src/core/schema/python/soma_body_joint_poses_v0_bindings.h @@ -0,0 +1,98 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline const SomaBodyJointPoseV0& first_soma_body_joint_pose_v0(const py::object& self) +{ + return *(*self.cast().values())[0]; +} + +constexpr py::ssize_t SOMA_BODY_JOINT_POSE_STRIDE = static_cast(sizeof(SomaBodyJointPoseV0)); +constexpr py::ssize_t SOMA_BODY_JOINT_POSE_COUNT = static_cast(SomaBodyJointV0_NUM_JOINTS); + +static_assert(sizeof(SomaBodyJointPoseArrayV0) == + sizeof(SomaBodyJointPoseV0) * static_cast(SomaBodyJointV0_NUM_JOINTS), + "SomaBodyJointPoseArrayV0.values length must equal SomaBodyJointV0::NUM_JOINTS"); + +inline void bind_soma_body_joint_poses_v0(py::module& m) +{ + py::class_(m, "SomaBodyJointPoseV0") + .def(py::init<>()) + .def(py::init(), py::arg("pose"), py::arg("is_valid") = false) + .def_property_readonly("pose", &SomaBodyJointPoseV0::pose, py::return_value_policy::reference_internal) + .def_property_readonly("is_valid", &SomaBodyJointPoseV0::is_valid); + + py::class_(m, "SomaBodyJointPoseArrayV0") + .def(py::init<>()) + .def( + "values", + [](const SomaBodyJointPoseArrayV0& self, size_t index) -> const SomaBodyJointPoseV0* + { + if (index >= static_cast(SomaBodyJointV0_NUM_JOINTS)) + { + throw py::index_error("SomaBodyJointPoseArrayV0 index out of range"); + } + return (*self.values())[index]; + }, + py::arg("index"), py::return_value_policy::reference_internal) + .def_property_readonly( + "positions", + [](py::object self) + { + const auto* first = + reinterpret_cast(&first_soma_body_joint_pose_v0(self).pose().position()); + return strided_field_view(self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 3); + }, + "Joint positions as a writable (77, 3) float32 view in SomaBodyJointV0 order.") + .def_property_readonly( + "orientations", + [](py::object self) + { + const auto* first = + reinterpret_cast(&first_soma_body_joint_pose_v0(self).pose().orientation()); + return strided_field_view(self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 4); + }, + "Unit XYZW quaternions as a writable (77, 4) float32 view.") + .def_property_readonly( + "is_valid", + [offset = FBS_FIELD_OFFSET(SomaBodyJointPoseV0, is_valid)](py::object self) + { + const auto* first = fbs_field_address(first_soma_body_joint_pose_v0(self), offset); + return strided_field_view( + self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 0); + }, + "Per-joint validity as a writable (77,) uint8 view."); + + serialized_class(m, "SomaBodyJointPosesV0", "Encoded evaluated SOMA body poses v0.") + .def(py::init( + [](const SomaBodyJointPoseArrayV0& joint_poses) + { + SomaBodyJointPosesV0T native; + native.joint_poses = std::make_shared(joint_poses); + return pack(native); + }), + py::arg("joint_poses") = SomaBodyJointPoseArrayV0()) + .def_property_readonly( + "joint_poses", field(&SomaBodyJointPosesV0::joint_poses), py::return_value_policy::reference_internal); + + bind_record( + m, "SomaBodyJointPosesV0Record", "SomaBodyJointPosesV0"); +} + +} // namespace core diff --git a/src/core/schema/python/soma_body_v0_bindings.h b/src/core/schema/python/soma_body_v0_bindings.h new file mode 100644 index 0000000000..bec4181688 --- /dev/null +++ b/src/core/schema/python/soma_body_v0_bindings.h @@ -0,0 +1,169 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline const SomaJointRotationV0& first_soma_body_v0_rotation(const py::object& self) +{ + return *(*self.cast().values())[0]; +} + +constexpr py::ssize_t SOMA_BODY_JOINT_STRIDE = static_cast(sizeof(SomaJointRotationV0)); +constexpr py::ssize_t SOMA_BODY_JOINT_COUNT = static_cast(SomaBodyJointV0_NUM_JOINTS); + +static_assert(sizeof(SomaBodyJointRotationArrayV0) == + sizeof(SomaJointRotationV0) * static_cast(SomaBodyJointV0_NUM_JOINTS), + "SomaBodyJointRotationArrayV0.values length must equal SomaBodyJointV0::NUM_JOINTS"); + +inline void bind_soma_body_v0(py::module& m) +{ + py::enum_(m, "SomaBodyJointV0") + .value("HIPS", SomaBodyJointV0_HIPS) + .value("SPINE1", SomaBodyJointV0_SPINE1) + .value("SPINE2", SomaBodyJointV0_SPINE2) + .value("CHEST", SomaBodyJointV0_CHEST) + .value("NECK1", SomaBodyJointV0_NECK1) + .value("NECK2", SomaBodyJointV0_NECK2) + .value("HEAD", SomaBodyJointV0_HEAD) + .value("HEAD_END", SomaBodyJointV0_HEAD_END) + .value("JAW", SomaBodyJointV0_JAW) + .value("LEFT_EYE", SomaBodyJointV0_LEFT_EYE) + .value("RIGHT_EYE", SomaBodyJointV0_RIGHT_EYE) + .value("LEFT_SHOULDER", SomaBodyJointV0_LEFT_SHOULDER) + .value("LEFT_ARM", SomaBodyJointV0_LEFT_ARM) + .value("LEFT_FORE_ARM", SomaBodyJointV0_LEFT_FORE_ARM) + .value("LEFT_HAND", SomaBodyJointV0_LEFT_HAND) + .value("LEFT_HAND_THUMB1", SomaBodyJointV0_LEFT_HAND_THUMB1) + .value("LEFT_HAND_THUMB2", SomaBodyJointV0_LEFT_HAND_THUMB2) + .value("LEFT_HAND_THUMB3", SomaBodyJointV0_LEFT_HAND_THUMB3) + .value("LEFT_HAND_THUMB_END", SomaBodyJointV0_LEFT_HAND_THUMB_END) + .value("LEFT_HAND_INDEX1", SomaBodyJointV0_LEFT_HAND_INDEX1) + .value("LEFT_HAND_INDEX2", SomaBodyJointV0_LEFT_HAND_INDEX2) + .value("LEFT_HAND_INDEX3", SomaBodyJointV0_LEFT_HAND_INDEX3) + .value("LEFT_HAND_INDEX4", SomaBodyJointV0_LEFT_HAND_INDEX4) + .value("LEFT_HAND_INDEX_END", SomaBodyJointV0_LEFT_HAND_INDEX_END) + .value("LEFT_HAND_MIDDLE1", SomaBodyJointV0_LEFT_HAND_MIDDLE1) + .value("LEFT_HAND_MIDDLE2", SomaBodyJointV0_LEFT_HAND_MIDDLE2) + .value("LEFT_HAND_MIDDLE3", SomaBodyJointV0_LEFT_HAND_MIDDLE3) + .value("LEFT_HAND_MIDDLE4", SomaBodyJointV0_LEFT_HAND_MIDDLE4) + .value("LEFT_HAND_MIDDLE_END", SomaBodyJointV0_LEFT_HAND_MIDDLE_END) + .value("LEFT_HAND_RING1", SomaBodyJointV0_LEFT_HAND_RING1) + .value("LEFT_HAND_RING2", SomaBodyJointV0_LEFT_HAND_RING2) + .value("LEFT_HAND_RING3", SomaBodyJointV0_LEFT_HAND_RING3) + .value("LEFT_HAND_RING4", SomaBodyJointV0_LEFT_HAND_RING4) + .value("LEFT_HAND_RING_END", SomaBodyJointV0_LEFT_HAND_RING_END) + .value("LEFT_HAND_PINKY1", SomaBodyJointV0_LEFT_HAND_PINKY1) + .value("LEFT_HAND_PINKY2", SomaBodyJointV0_LEFT_HAND_PINKY2) + .value("LEFT_HAND_PINKY3", SomaBodyJointV0_LEFT_HAND_PINKY3) + .value("LEFT_HAND_PINKY4", SomaBodyJointV0_LEFT_HAND_PINKY4) + .value("LEFT_HAND_PINKY_END", SomaBodyJointV0_LEFT_HAND_PINKY_END) + .value("RIGHT_SHOULDER", SomaBodyJointV0_RIGHT_SHOULDER) + .value("RIGHT_ARM", SomaBodyJointV0_RIGHT_ARM) + .value("RIGHT_FORE_ARM", SomaBodyJointV0_RIGHT_FORE_ARM) + .value("RIGHT_HAND", SomaBodyJointV0_RIGHT_HAND) + .value("RIGHT_HAND_THUMB1", SomaBodyJointV0_RIGHT_HAND_THUMB1) + .value("RIGHT_HAND_THUMB2", SomaBodyJointV0_RIGHT_HAND_THUMB2) + .value("RIGHT_HAND_THUMB3", SomaBodyJointV0_RIGHT_HAND_THUMB3) + .value("RIGHT_HAND_THUMB_END", SomaBodyJointV0_RIGHT_HAND_THUMB_END) + .value("RIGHT_HAND_INDEX1", SomaBodyJointV0_RIGHT_HAND_INDEX1) + .value("RIGHT_HAND_INDEX2", SomaBodyJointV0_RIGHT_HAND_INDEX2) + .value("RIGHT_HAND_INDEX3", SomaBodyJointV0_RIGHT_HAND_INDEX3) + .value("RIGHT_HAND_INDEX4", SomaBodyJointV0_RIGHT_HAND_INDEX4) + .value("RIGHT_HAND_INDEX_END", SomaBodyJointV0_RIGHT_HAND_INDEX_END) + .value("RIGHT_HAND_MIDDLE1", SomaBodyJointV0_RIGHT_HAND_MIDDLE1) + .value("RIGHT_HAND_MIDDLE2", SomaBodyJointV0_RIGHT_HAND_MIDDLE2) + .value("RIGHT_HAND_MIDDLE3", SomaBodyJointV0_RIGHT_HAND_MIDDLE3) + .value("RIGHT_HAND_MIDDLE4", SomaBodyJointV0_RIGHT_HAND_MIDDLE4) + .value("RIGHT_HAND_MIDDLE_END", SomaBodyJointV0_RIGHT_HAND_MIDDLE_END) + .value("RIGHT_HAND_RING1", SomaBodyJointV0_RIGHT_HAND_RING1) + .value("RIGHT_HAND_RING2", SomaBodyJointV0_RIGHT_HAND_RING2) + .value("RIGHT_HAND_RING3", SomaBodyJointV0_RIGHT_HAND_RING3) + .value("RIGHT_HAND_RING4", SomaBodyJointV0_RIGHT_HAND_RING4) + .value("RIGHT_HAND_RING_END", SomaBodyJointV0_RIGHT_HAND_RING_END) + .value("RIGHT_HAND_PINKY1", SomaBodyJointV0_RIGHT_HAND_PINKY1) + .value("RIGHT_HAND_PINKY2", SomaBodyJointV0_RIGHT_HAND_PINKY2) + .value("RIGHT_HAND_PINKY3", SomaBodyJointV0_RIGHT_HAND_PINKY3) + .value("RIGHT_HAND_PINKY4", SomaBodyJointV0_RIGHT_HAND_PINKY4) + .value("RIGHT_HAND_PINKY_END", SomaBodyJointV0_RIGHT_HAND_PINKY_END) + .value("LEFT_LEG", SomaBodyJointV0_LEFT_LEG) + .value("LEFT_SHIN", SomaBodyJointV0_LEFT_SHIN) + .value("LEFT_FOOT", SomaBodyJointV0_LEFT_FOOT) + .value("LEFT_TOE_BASE", SomaBodyJointV0_LEFT_TOE_BASE) + .value("LEFT_TOE_END", SomaBodyJointV0_LEFT_TOE_END) + .value("RIGHT_LEG", SomaBodyJointV0_RIGHT_LEG) + .value("RIGHT_SHIN", SomaBodyJointV0_RIGHT_SHIN) + .value("RIGHT_FOOT", SomaBodyJointV0_RIGHT_FOOT) + .value("RIGHT_TOE_BASE", SomaBodyJointV0_RIGHT_TOE_BASE) + .value("RIGHT_TOE_END", SomaBodyJointV0_RIGHT_TOE_END) + .value("NUM_JOINTS", SomaBodyJointV0_NUM_JOINTS); + + py::class_(m, "SomaBodyJointRotationArrayV0") + .def(py::init<>()) + .def( + "values", + [](const SomaBodyJointRotationArrayV0& self, size_t index) -> const SomaJointRotationV0* + { + if (index >= static_cast(SomaBodyJointV0_NUM_JOINTS)) + { + throw py::index_error("SomaBodyJointRotationArrayV0 index out of range"); + } + return (*self.values())[index]; + }, + py::arg("index"), py::return_value_policy::reference_internal) + .def_property_readonly( + "rotations", + [](py::object self) + { + const auto* first = reinterpret_cast(&first_soma_body_v0_rotation(self).rotation()); + return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 4); + }, + "Unit XYZW quaternions as a writable (77, 4) float32 view.") + .def_property_readonly( + "is_valid", + [offset = FBS_FIELD_OFFSET(SomaJointRotationV0, is_valid)](py::object self) + { + const auto* first = fbs_field_address(first_soma_body_v0_rotation(self), offset); + return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 0); + }, + "Per-joint validity as a writable (77,) uint8 view."); + + serialized_class(m, "SomaBodyJointRotationsV0", "Encoded SOMA body joint rotations v0.") + .def(py::init( + [](const SomaBodyJointRotationArrayV0& joint_rotations, const Point& global_translation, + bool global_translation_is_valid) + { + SomaBodyJointRotationsV0T native; + native.joint_rotations = std::make_shared(joint_rotations); + native.global_translation = std::make_shared(global_translation); + native.global_translation_is_valid = global_translation_is_valid; + return pack(native); + }), + py::arg("joint_rotations") = SomaBodyJointRotationArrayV0(), py::arg("global_translation") = Point(), + py::arg("global_translation_is_valid") = false) + .def_property_readonly("joint_rotations", field(&SomaBodyJointRotationsV0::joint_rotations), + py::return_value_policy::reference_internal) + .def_property_readonly("global_translation", field(&SomaBodyJointRotationsV0::global_translation), + py::return_value_policy::reference_internal) + .def_property_readonly( + "global_translation_is_valid", field(&SomaBodyJointRotationsV0::global_translation_is_valid)); + + bind_record( + m, "SomaBodyJointRotationsV0Record", "SomaBodyJointRotationsV0"); +} + +} // namespace core diff --git a/src/core/schema/python/soma_common_v0_bindings.h b/src/core/schema/python/soma_common_v0_bindings.h new file mode 100644 index 0000000000..49f1cc3d02 --- /dev/null +++ b/src/core/schema/python/soma_common_v0_bindings.h @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_soma_common_v0(py::module& m) +{ + py::class_(m, "SomaJointRotationV0") + .def(py::init<>()) + .def(py::init(), py::arg("rotation"), py::arg("is_valid") = false) + .def_property_readonly("rotation", &SomaJointRotationV0::rotation, py::return_value_policy::reference_internal) + .def_property_readonly("is_valid", &SomaJointRotationV0::is_valid); +} + +} // namespace core diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py index bf80861216..79c328d293 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py @@ -25,6 +25,7 @@ from .key_event_testing import FakeKeyEventSource from .joint_state_source import JointStateSource from .full_body_source import FullBodySource +from .soma_body_source import SomaBodyRepresentation, SomaBodySource from .message_channel_source import MessageChannelSource from .message_channel_sink import MessageChannelSink from .message_channel_config import ( @@ -41,6 +42,8 @@ KeyboardOutputTrackedType, JointStateOutputTrackedType, FullBodyPoseTrackedType, + SomaBodyJointRotationsV0TrackedType, + SomaBodyJointPosesV0TrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, @@ -48,6 +51,8 @@ DeviceIOKeyboardOutputTracked, DeviceIOJointStateOutputTracked, DeviceIOFullBodyPoseTracked, + DeviceIOSomaBodyJointRotationsV0Tracked, + DeviceIOSomaBodyJointPosesV0Tracked, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, MessageChannelStatusType, @@ -74,6 +79,8 @@ "FakeKeyEventSource", "JointStateSource", "FullBodySource", + "SomaBodySource", + "SomaBodyRepresentation", "MessageChannelSource", "MessageChannelSink", "MessageChannelConfig", @@ -87,6 +94,8 @@ "KeyboardOutputTrackedType", "JointStateOutputTrackedType", "FullBodyPoseTrackedType", + "SomaBodyJointRotationsV0TrackedType", + "SomaBodyJointPosesV0TrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", "MessageChannelStatusType", @@ -97,6 +106,8 @@ "DeviceIOKeyboardOutputTracked", "DeviceIOJointStateOutputTracked", "DeviceIOFullBodyPoseTracked", + "DeviceIOSomaBodyJointRotationsV0Tracked", + "DeviceIOSomaBodyJointPosesV0Tracked", "DeviceIOMessageChannelMessagesTracked", "MessageChannelMessagesTrackedGroup", "MessageChannelStatusGroup", diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py index 5cebd9a090..b56170e12b 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py @@ -22,6 +22,8 @@ KeyboardOutput, JointStateOutput, FullBodyPose, + SomaBodyJointPosesV0, + SomaBodyJointRotationsV0, MessageChannelMessagesTracked, ) @@ -115,6 +117,18 @@ class FullBodyPoseTrackedType(_PayloadTensorType): _payload_cls = FullBodyPose +class SomaBodyJointRotationsV0TrackedType(_PayloadTensorType): + """SomaBodyJointRotationsV0 payload from a generated SOMA body tracker.""" + + _payload_cls = SomaBodyJointRotationsV0 + + +class SomaBodyJointPosesV0TrackedType(_PayloadTensorType): + """SomaBodyJointPosesV0 payload from a generated SOMA body tracker.""" + + _payload_cls = SomaBodyJointPosesV0 + + class MessageChannelMessagesTrackedType(_RequiredPayloadTensorType): """MessageChannelMessagesTracked batch from DeviceIO MessageChannelTracker.""" @@ -215,6 +229,22 @@ def DeviceIOFullBodyPoseTracked() -> TensorGroupType: ) +def DeviceIOSomaBodyJointRotationsV0Tracked() -> TensorGroupType: + """SOMA V0 body controls, or None when the tracker is inactive.""" + return TensorGroupType( + "deviceio_soma_body_joint_rotations_v0", + [SomaBodyJointRotationsV0TrackedType("soma_body_tracked")], + ) + + +def DeviceIOSomaBodyJointPosesV0Tracked() -> TensorGroupType: + """Evaluated SOMA V0 body poses, or None when the tracker is inactive.""" + return TensorGroupType( + "deviceio_soma_body_joint_poses_v0", + [SomaBodyJointPosesV0TrackedType("soma_body_tracked")], + ) + + def DeviceIOMessageChannelMessagesTracked() -> TensorGroupType: """Message batch from DeviceIO MessageChannelTracker.""" return TensorGroupType( diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py new file mode 100644 index 0000000000..70b9575308 --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py @@ -0,0 +1,106 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""DeviceIO SOMA body source for rotation and evaluated-pose transports.""" + +from enum import Enum +from typing import TYPE_CHECKING, Any + +import numpy as np + +from isaaccapture.deviceio_trackers import ( + SomaBodyJointPosesV0Tracker, + SomaBodyJointRotationsV0Tracker, +) + +from .deviceio_tensor_types import ( + DeviceIOSomaBodyJointPosesV0Tracked, + DeviceIOSomaBodyJointRotationsV0Tracked, +) +from .interface import IDeviceIOSource +from ..interface.retargeter_core_types import RetargeterIO, RetargeterIOType +from ..interface.tensor_group import TensorGroup +from ..interface.tensor_group_type import OptionalType +from ..tensor_types import SomaBodyInput, SomaBodyInputIndex +from ..utilities.soma_body_evaluator import _SomaBodyEvaluator + +if TYPE_CHECKING: + from isaaccapture.deviceio import ITracker + + +class SomaBodyRepresentation(str, Enum): + JOINT_ROTATIONS = "joint-rotations" + JOINT_POSES = "joint-poses" + + +class SomaBodySource(IDeviceIOSource): + """Normalize either SOMA V0 wire profile into evaluated joint poses. + + The generated tracker remains the raw transport and recording boundary. The + rotation profile runs FK once per graph step. The evaluated-pose profile maps + directly into the same downstream ``SomaBodyInput``. + """ + + BODY = "soma_body" + + def __init__( + self, + name: str, + collection_id: str, + layer: Any | None = None, + *, + representation: SomaBodyRepresentation + | str = SomaBodyRepresentation.JOINT_ROTATIONS, + ) -> None: + if not collection_id: + raise ValueError("SOMA body collection_id must not be empty") + self.representation = SomaBodyRepresentation(representation) + if self.representation is SomaBodyRepresentation.JOINT_ROTATIONS: + if layer is None: + raise ValueError("joint-rotations requires a prepared SOMA layer") + self._tracker = SomaBodyJointRotationsV0Tracker(collection_id) + self._input_type = DeviceIOSomaBodyJointRotationsV0Tracked() + else: + self._tracker = SomaBodyJointPosesV0Tracker(collection_id) + self._input_type = DeviceIOSomaBodyJointPosesV0Tracked() + self._evaluator = _SomaBodyEvaluator(layer) if layer is not None else None + self.joint_names = self._evaluator.joint_names if self._evaluator else () + self.bones = self._evaluator.bones if self._evaluator else () + super().__init__(name) + + def get_tracker(self) -> "ITracker": + return self._tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + group = TensorGroup(self.input_spec()["deviceio_soma_body"]) + group[0] = self._tracker.get_data(deviceio_session) + return {"deviceio_soma_body": group} + + def input_spec(self) -> RetargeterIOType: + return {"deviceio_soma_body": self._input_type} + + def output_spec(self) -> RetargeterIOType: + return {self.BODY: OptionalType(SomaBodyInput())} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + data = inputs["deviceio_soma_body"][0] + group = outputs[self.BODY] + if data is None: + group.set_none() + return + + if self.representation is SomaBodyRepresentation.JOINT_ROTATIONS: + assert self._evaluator is not None + positions, orientations, valid = self._evaluator.evaluate(data) + elif data.joint_poses is None: + positions = np.zeros((77, 3), dtype=np.float32) + orientations = np.zeros((77, 4), dtype=np.float32) + orientations[:, 3] = 1.0 + valid = np.zeros(77, dtype=np.uint8) + else: + positions = np.asarray(data.joint_poses.positions, dtype=np.float32) + orientations = np.asarray(data.joint_poses.orientations, dtype=np.float32) + valid = np.asarray(data.joint_poses.is_valid, dtype=np.uint8) + group[SomaBodyInputIndex.JOINT_POSITIONS] = positions + group[SomaBodyInputIndex.JOINT_ORIENTATIONS] = orientations + group[SomaBodyInputIndex.JOINT_VALID] = valid diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py b/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py index 5406ef949c..11577bb286 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py @@ -12,10 +12,12 @@ HeadInput, ControllerInput, FullBodyInput, + SomaBodyInput, TransformMatrix, Generic3AxisPedalInput, NUM_HAND_JOINTS, NUM_BODY_JOINTS, + NUM_SOMA_BODY_JOINTS, RobotHandJoints, ) from .tactile_types import ( @@ -34,6 +36,7 @@ ControllerInputIndex, Generic3AxisPedalInputIndex, FullBodyInputIndex, + SomaBodyInputIndex, HandJointIndex, BodyJointIndex, FingerIndex, @@ -53,10 +56,12 @@ "HeadInput", "ControllerInput", "FullBodyInput", + "SomaBodyInput", "TransformMatrix", "Generic3AxisPedalInput", "NUM_HAND_JOINTS", "NUM_BODY_JOINTS", + "NUM_SOMA_BODY_JOINTS", "RobotHandJoints", # Tactile / haptic types "TactileVector", @@ -73,6 +78,7 @@ "ControllerInputIndex", "Generic3AxisPedalInputIndex", "FullBodyInputIndex", + "SomaBodyInputIndex", "HandJointIndex", "BodyJointIndex", "FingerIndex", diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py b/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py index 44c4e178fe..954f1d1f68 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py @@ -21,6 +21,7 @@ ControllerInput, Generic3AxisPedalInput, FullBodyInput, + SomaBodyInput, ) @@ -47,6 +48,9 @@ def _create_index_enum(name: str, group_type, prefix: str = "") -> IntEnum: FullBodyInputIndex: Any = _create_index_enum( "FullBodyInputIndex", FullBodyInput(), "body_" ) +SomaBodyInputIndex: Any = _create_index_enum( + "SomaBodyInputIndex", SomaBodyInput(), "soma_body_" +) class HandJointIndex(IntEnum): diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/standard_types.py b/src/python/isaaccapture/retargeting_engine/tensor_types/standard_types.py index da7338c0c2..e39ed4e9a4 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/standard_types.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/standard_types.py @@ -18,6 +18,7 @@ # Constants NUM_HAND_JOINTS = 26 # XR_HAND_JOINT_COUNT_EXT from OpenXR NUM_BODY_JOINTS = 24 # XR_BODY_JOINT_COUNT_BD from XR_BD_body_tracking +NUM_SOMA_BODY_JOINTS = 77 # Public SOMA joints, excluding virtual Root # ============================================================================ # Hand Tracking Types @@ -253,6 +254,33 @@ def FullBodyInput() -> TensorGroupType: ) +def SomaBodyInput() -> TensorGroupType: + """Evaluated global poses for the 77 transported SOMA body joints.""" + return TensorGroupType( + "soma_body", + [ + NDArrayType( + "soma_body_joint_positions", + shape=(NUM_SOMA_BODY_JOINTS, 3), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + NDArrayType( + "soma_body_joint_orientations", + shape=(NUM_SOMA_BODY_JOINTS, 4), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + NDArrayType( + "soma_body_joint_valid", + shape=(NUM_SOMA_BODY_JOINTS,), + dtype=DLDataType.UINT, + dtype_bits=8, + ), + ], + ) + + # ============================================================================ # Transform Types # ============================================================================ diff --git a/src/python/isaaccapture/retargeting_engine/utilities/__init__.py b/src/python/isaaccapture/retargeting_engine/utilities/__init__.py index 249f9546b3..b0403fe3cc 100644 --- a/src/python/isaaccapture/retargeting_engine/utilities/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/utilities/__init__.py @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 """Utility nodes for the retargeting engine - transform, coordinate conversion, etc.""" diff --git a/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py new file mode 100644 index 0000000000..edd07c2a32 --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py @@ -0,0 +1,109 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Internal evaluation of transported SOMA controls into global joint poses.""" + +from importlib import import_module +from typing import Any + +import numpy as np + + +def _numpy(value: Any) -> np.ndarray: + if hasattr(value, "detach"): + value = value.detach() + if hasattr(value, "cpu"): + value = value.cpu() + if hasattr(value, "numpy"): + value = value.numpy() + return np.asarray(value) + + +class _SomaBodyEvaluator: + """Run upstream SOMA FK and expose all public transported joints.""" + + def __init__(self, layer: Any) -> None: + joint_names = tuple(str(name) for name in layer.public_joint_names) + parent_ids = _numpy(layer.output_joint_parent_ids).astype(np.int64) + if len(joint_names) != 78 or joint_names[0].upper() != "ROOT": + raise ValueError("SOMA evaluator requires Root plus 77 public joints") + if parent_ids.shape != (78,): + raise ValueError("SOMA evaluator requires 78 public parent IDs") + if any( + parent < 0 or parent >= child + for child, parent in enumerate(parent_ids[1:], 1) + ): + raise ValueError("SOMA public joints must follow parent-before-child order") + + self.layer = layer + self.joint_names = joint_names[1:] + self.parent_ids = tuple(int(parent) for parent in parent_ids) + self.bones = tuple( + (int(parent) - 1, child - 1) + for child, parent in enumerate(parent_ids[1:], 1) + if parent != 0 + ) + + def _joint_validity( + self, control_valid: np.ndarray, translation_valid: bool + ) -> np.ndarray: + valid = np.zeros(78, dtype=np.uint8) + valid[0] = translation_valid + for child in range(1, 78): + valid[child] = valid[self.parent_ids[child]] and control_valid[child - 1] + return valid[1:] + + @staticmethod + def _pose_inputs( + data: Any, + ) -> tuple[np.ndarray, np.ndarray, np.ndarray, bool]: + # Nested FlatBuffer structs are optional on the wire even though the + # Python constructor supplies defaults. Keep vendor omissions safe. + joint_rotations = data.joint_rotations + if joint_rotations is None: + control_valid = np.zeros(77, dtype=bool) + rotations = np.zeros((77, 4), dtype=np.float32) + rotations[:, 3] = 1.0 + else: + control_valid = np.asarray(joint_rotations.is_valid, dtype=bool) + rotations = np.asarray(joint_rotations.rotations, dtype=np.float32).copy() + rotations[~control_valid] = (0.0, 0.0, 0.0, 1.0) + + translation = data.global_translation + translation_valid = ( + bool(data.global_translation_is_valid) and translation is not None + ) + translation_xyz = np.asarray( + [translation.x, translation.y, translation.z] + if translation_valid + else [0.0, 0.0, 0.0], + dtype=np.float32, + ) + return rotations, control_valid, translation_xyz, translation_valid + + def evaluate(self, data: Any) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + torch = import_module("torch") + transforms = import_module("soma.geometry.transforms") + + rotations, control_valid, translation_xyz, translation_valid = ( + self._pose_inputs(data) + ) + + with torch.no_grad(): + output = self.layer.pose( + transforms.quaternion_xyzw_to_matrix( + torch.from_numpy(rotations).unsqueeze(0) + ), + transl=torch.from_numpy(translation_xyz).unsqueeze(0), + pose2rot=False, + fk_only=True, + apply_correctives=False, + ) + orientations = transforms.matrix_to_quaternion_xyzw( + output["transforms"][0, 1:, :3, :3] + ) + return ( + _numpy(output["joints"][0]).astype(np.float32, copy=False), + _numpy(orientations).astype(np.float32, copy=False), + self._joint_validity(control_valid, translation_valid), + ) diff --git a/src/python/isaaccapture/schema/__init__.py b/src/python/isaaccapture/schema/__init__.py index 3c488108bf..07601e70f5 100644 --- a/src/python/isaaccapture/schema/__init__.py +++ b/src/python/isaaccapture/schema/__init__.py @@ -84,6 +84,18 @@ BodyJoints, FullBodyPose, FullBodyPoseRecord, + # Shared SOMA types. + SomaJointRotationV0, + # SOMA body joint-rotation v0 types. + SomaBodyJointV0, + SomaBodyJointRotationArrayV0, + SomaBodyJointRotationsV0, + SomaBodyJointRotationsV0Record, + # SOMA evaluated body joint-pose v0 types. + SomaBodyJointPoseV0, + SomaBodyJointPoseArrayV0, + SomaBodyJointPosesV0, + SomaBodyJointPosesV0Record, ) # Deprecated aliases, resolved lazily via __getattr__ so accessing them emits a @@ -189,4 +201,16 @@ def __getattr__(name: str): "BodyJoints", "FullBodyPose", "FullBodyPoseRecord", + # Shared SOMA types. + "SomaJointRotationV0", + # SOMA body joint-rotation v0 types. + "SomaBodyJointV0", + "SomaBodyJointRotationArrayV0", + "SomaBodyJointRotationsV0", + "SomaBodyJointRotationsV0Record", + # SOMA evaluated body joint-pose v0 types. + "SomaBodyJointPoseV0", + "SomaBodyJointPoseArrayV0", + "SomaBodyJointPosesV0", + "SomaBodyJointPosesV0Record", ] diff --git a/tests/AGENTS.md b/tests/AGENTS.md index c9c53461f3..69cd639b12 100644 --- a/tests/AGENTS.md +++ b/tests/AGENTS.md @@ -63,6 +63,9 @@ Shared C++ fixtures (not executables) live under `tests/cpp/viz/support/` as before (e.g. viz tests require `BUILD_VIZ`). 4. Do **not** colocate new pytest/Catch2 trees under `src/` or `examples/`. +- FlatBuffers generated `VT_*` constants are vtable offsets, not schema field + IDs. Compare them with `(field_id + 2) * 2`, as the existing schema tests do. + ## Out of scope here - Vehicle teleop worker tests belong in `examples/vehicle_teleop/tests/`, alongside the example they exercise. diff --git a/tests/cpp/core/schema/CMakeLists.txt b/tests/cpp/core/schema/CMakeLists.txt index 5189316600..3450c4d1fe 100644 --- a/tests/cpp/core/schema/CMakeLists.txt +++ b/tests/cpp/core/schema/CMakeLists.txt @@ -16,6 +16,8 @@ add_executable(schema_tests test_joint_se3_pose.cpp test_se3_tracker.cpp test_serialized.cpp + test_soma_body_v0.cpp + test_soma_body_joint_poses_v0.cpp test_schema_conform.cpp ) target_link_libraries(schema_tests PRIVATE diff --git a/tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp b/tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp new file mode 100644 index 0000000000..6860aaa523 --- /dev/null +++ b/tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include +#include +#include + +namespace +{ + +constexpr flatbuffers::voffset_t vt(flatbuffers::voffset_t field) +{ + return (field + 2) * 2; +} + +static_assert(core::SomaBodyJointPosesV0::VT_JOINT_POSES == vt(0)); +static_assert(core::SomaBodyJointPosesV0Record::VT_DATA == vt(0)); +static_assert(core::SomaBodyJointPosesV0Record::VT_TIMESTAMP == vt(1)); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaBodyJointPoseArrayV0) == 77 * sizeof(core::SomaBodyJointPoseV0)); + +TEST_CASE("SOMA evaluated body joint poses v0 round trip", "[soma_body_joint_poses_v0][flatbuffers]") +{ + core::SomaBodyJointPosesV0T pose; + pose.joint_poses = std::make_shared(); + pose.joint_poses->mutable_values()->Mutate( + core::SomaBodyJointV0_HEAD, + core::SomaBodyJointPoseV0( + core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)), true)); + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaBodyJointPosesV0::Pack(builder, &pose)); + flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); + REQUIRE(verifier.VerifyBuffer(nullptr)); + + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(decoded->joint_poses() != nullptr); + const auto* head = (*decoded->joint_poses()->values())[core::SomaBodyJointV0_HEAD]; + CHECK(head->pose().position().x() == 1.0f); + CHECK(head->pose().position().y() == 2.0f); + CHECK(head->pose().position().z() == 3.0f); + CHECK(head->pose().orientation().z() == 0.6f); + CHECK(head->pose().orientation().w() == 0.8f); + CHECK(head->is_valid()); +} + +} // namespace diff --git a/tests/cpp/core/schema/test_soma_body_v0.cpp b/tests/cpp/core/schema/test_soma_body_v0.cpp new file mode 100644 index 0000000000..5a76d9b4c2 --- /dev/null +++ b/tests/cpp/core/schema/test_soma_body_v0.cpp @@ -0,0 +1,124 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#define VT(field) (field + 2) * 2 +static_assert(core::SomaBodyJointRotationsV0::VT_JOINT_ROTATIONS == VT(0)); +static_assert(core::SomaBodyJointRotationsV0::VT_GLOBAL_TRANSLATION == VT(1)); +static_assert(core::SomaBodyJointRotationsV0::VT_GLOBAL_TRANSLATION_IS_VALID == VT(2)); +static_assert(core::SomaBodyJointRotationsV0Record::VT_DATA == VT(0)); +static_assert(core::SomaBodyJointRotationsV0Record::VT_TIMESTAMP == VT(1)); + +#define TYPE(field) decltype(std::declval().field()) +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); + +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaBodyJointRotationArrayV0) == 77 * sizeof(core::SomaJointRotationV0)); + +static_assert(core::SomaBodyJointV0_HIPS == 0); +static_assert(core::SomaBodyJointV0_LEFT_SHOULDER == 11); +static_assert(core::SomaBodyJointV0_RIGHT_SHOULDER == 39); +static_assert(core::SomaBodyJointV0_LEFT_LEG == 67); +static_assert(core::SomaBodyJointV0_RIGHT_TOE_END == 76); +static_assert(core::SomaBodyJointV0_NUM_JOINTS == 77); + +TEST_CASE("SOMA body v0 rotation array has the public layout", "[soma_body_v0][struct]") +{ + core::SomaBodyJointRotationArrayV0 rotations; + + REQUIRE(rotations.values()->size() == static_cast(core::SomaBodyJointV0_NUM_JOINTS)); + CHECK((*rotations.values())[core::SomaBodyJointV0_HIPS]->rotation().x() == 0.0f); + CHECK_FALSE((*rotations.values())[core::SomaBodyJointV0_RIGHT_TOE_END]->is_valid()); + + const core::SomaJointRotationV0 hips(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true); + rotations.mutable_values()->Mutate(core::SomaBodyJointV0_HIPS, hips); + + CHECK((*rotations.values())[core::SomaBodyJointV0_HIPS]->rotation().z() == Catch::Approx(0.6f)); + CHECK((*rotations.values())[core::SomaBodyJointV0_HIPS]->rotation().w() == Catch::Approx(0.8f)); + CHECK((*rotations.values())[core::SomaBodyJointV0_HIPS]->is_valid()); +} + +TEST_CASE("SOMA body joint rotations v0 round trip through FlatBuffers", "[soma_body_v0][flatbuffers]") +{ + core::SomaBodyJointRotationsV0T pose; + pose.joint_rotations = std::make_shared(); + pose.joint_rotations->mutable_values()->Mutate( + core::SomaBodyJointV0_HEAD, core::SomaJointRotationV0(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true)); + pose.global_translation = std::make_shared(1.0f, 2.0f, 3.0f); + pose.global_translation_is_valid = true; + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaBodyJointRotationsV0::Pack(builder, &pose)); + + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(decoded->joint_rotations() != nullptr); + REQUIRE(decoded->global_translation() != nullptr); + const auto* head = (*decoded->joint_rotations()->values())[core::SomaBodyJointV0_HEAD]; + CHECK(head->rotation().z() == Catch::Approx(0.6f)); + CHECK(head->rotation().w() == Catch::Approx(0.8f)); + CHECK(head->is_valid()); + CHECK(decoded->global_translation()->x() == Catch::Approx(1.0f)); + CHECK(decoded->global_translation_is_valid()); +} + +TEST_CASE("SOMA body record v0 preserves quaternion components and validity", "[soma_body_v0][flatbuffers]") +{ + const float scale = 1.0f / std::sqrt(30.0f); + core::SomaBodyJointRotationsV0RecordT record; + record.data = std::make_shared(); + record.data->joint_rotations = std::make_shared(); + record.data->joint_rotations->mutable_values()->Mutate( + core::SomaBodyJointV0_HIPS, + core::SomaJointRotationV0(core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale), true)); + record.data->joint_rotations->mutable_values()->Mutate( + core::SomaBodyJointV0_RIGHT_TOE_END, + core::SomaJointRotationV0(core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale), true)); + record.data->global_translation = std::make_shared(1.0f, -2.0f, 3.0f); + record.data->global_translation_is_valid = false; + record.timestamp = std::make_shared(100, 200, 300); + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaBodyJointRotationsV0Record::Pack(builder, &record)); + flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); + REQUIRE(verifier.VerifyBuffer(nullptr)); + + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(decoded->data() != nullptr); + REQUIRE(decoded->timestamp() != nullptr); + REQUIRE(decoded->data()->joint_rotations() != nullptr); + REQUIRE(decoded->data()->global_translation() != nullptr); + const auto* values = decoded->data()->joint_rotations()->values(); + const auto* hips = (*values)[core::SomaBodyJointV0_HIPS]; + const auto* toe = (*values)[core::SomaBodyJointV0_RIGHT_TOE_END]; + CHECK(hips->rotation().x() == Catch::Approx(scale)); + CHECK(hips->rotation().y() == Catch::Approx(-2.0f * scale)); + CHECK(hips->rotation().z() == Catch::Approx(3.0f * scale)); + CHECK(hips->rotation().w() == Catch::Approx(-4.0f * scale)); + CHECK(toe->rotation().x() == Catch::Approx(-scale)); + CHECK(toe->rotation().y() == Catch::Approx(2.0f * scale)); + CHECK(toe->rotation().z() == Catch::Approx(-3.0f * scale)); + CHECK(toe->rotation().w() == Catch::Approx(4.0f * scale)); + CHECK(hips->is_valid()); + CHECK(toe->is_valid()); + CHECK_FALSE((*values)[core::SomaBodyJointV0_HEAD]->is_valid()); + CHECK(decoded->data()->global_translation()->x() == Catch::Approx(1.0f)); + CHECK(decoded->data()->global_translation()->y() == Catch::Approx(-2.0f)); + CHECK(decoded->data()->global_translation()->z() == Catch::Approx(3.0f)); + CHECK_FALSE(decoded->data()->global_translation_is_valid()); + CHECK(decoded->timestamp()->available_time_local_common_clock() == 100); + CHECK(decoded->timestamp()->sample_time_local_common_clock() == 200); + CHECK(decoded->timestamp()->sample_time_raw_device_clock() == 300); +} diff --git a/tests/python/core/retargeting_engine/test_soma_body_evaluator.py b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py new file mode 100644 index 0000000000..2215df2185 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py @@ -0,0 +1,85 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from unittest.mock import MagicMock + +import numpy as np + +from isaaccapture.retargeting_engine.utilities.soma_body_evaluator import ( + _SomaBodyEvaluator, +) + + +def evaluator_with_two_branches() -> _SomaBodyEvaluator: + layer = MagicMock() + layer.public_joint_names = ("Root", *(f"Joint{index}" for index in range(77))) + layer.output_joint_parent_ids = np.array([0, 0, 1, 1, *range(3, 77)]) + return _SomaBodyEvaluator(layer) + + +def test_evaluator_exposes_native_joint_order_and_topology(): + evaluator = evaluator_with_two_branches() + assert evaluator.joint_names[:3] == ("Joint0", "Joint1", "Joint2") + assert evaluator.bones[:3] == ((0, 1), (0, 2), (2, 3)) + assert len(evaluator.joint_names) == 77 + assert len(evaluator.bones) == 76 + + +def test_evaluator_propagates_validity_through_ancestors(): + evaluator = evaluator_with_two_branches() + controls = np.ones(77, dtype=bool) + controls[1] = False + valid = evaluator._joint_validity(controls, translation_valid=True) + assert valid[0] + assert not valid[1] + assert valid[2] + assert valid[3] + assert not evaluator._joint_validity(controls, translation_valid=False).any() + + +def test_evaluator_defaults_omitted_rotations_to_invalid_identity(): + data = MagicMock( + joint_rotations=None, + global_translation=MagicMock(x=1.0, y=2.0, z=3.0), + global_translation_is_valid=True, + ) + + rotations, control_valid, translation, translation_valid = ( + _SomaBodyEvaluator._pose_inputs(data) + ) + + np.testing.assert_array_equal(rotations, np.tile([0.0, 0.0, 0.0, 1.0], (77, 1))) + assert not control_valid.any() + np.testing.assert_array_equal(translation, [1.0, 2.0, 3.0]) + assert translation_valid + assert ( + not evaluator_with_two_branches() + ._joint_validity(control_valid, translation_valid) + .any() + ) + + +def test_evaluator_marks_omitted_translation_invalid(): + rotations = np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)) + data = MagicMock( + joint_rotations=MagicMock( + rotations=rotations, + is_valid=np.ones(77, dtype=np.uint8), + ), + global_translation=None, + global_translation_is_valid=True, + ) + + actual_rotations, control_valid, translation, translation_valid = ( + _SomaBodyEvaluator._pose_inputs(data) + ) + + np.testing.assert_array_equal(actual_rotations, rotations) + assert control_valid.all() + np.testing.assert_array_equal(translation, np.zeros(3, dtype=np.float32)) + assert not translation_valid + assert ( + not evaluator_with_two_branches() + ._joint_validity(control_valid, translation_valid) + .any() + ) diff --git a/tests/python/core/retargeting_engine/test_soma_body_source.py b/tests/python/core/retargeting_engine/test_soma_body_source.py new file mode 100644 index 0000000000..429534f204 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_soma_body_source.py @@ -0,0 +1,189 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from unittest.mock import MagicMock + +import numpy as np +import pytest + +from isaaccapture.deviceio_trackers import ( + SomaBodyJointPosesV0Tracker, + SomaBodyJointRotationsV0Tracker, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + DeviceIOFullBodyPoseTracked, + DeviceIOSomaBodyJointPosesV0Tracked, + DeviceIOSomaBodyJointRotationsV0Tracked, + SomaBodyRepresentation, + SomaBodySource, +) +from isaaccapture.retargeting_engine.interface.base_retargeter import _make_output_group +from isaaccapture.retargeting_engine.interface.tensor_group import TensorGroup +from isaaccapture.retargeting_engine.tensor_types import SomaBodyInputIndex +from isaaccapture.schema import ( + BodyJoints, + FullBodyPose, + Point, + SomaBodyJointPoseArrayV0, + SomaBodyJointPosesV0, + SomaBodyJointRotationArrayV0, + SomaBodyJointRotationsV0, +) +from isaaccapture.teleop_session_manager.helpers import _get_trackers_from_pipeline + + +def pose(): + joints = SomaBodyJointRotationArrayV0() + joints.rotations[:] = (0, 0, 0, 1) + joints.is_valid[:] = 1 + return SomaBodyJointRotationsV0(joints, Point(1, 2, 3), True) + + +def fake_layer(): + layer = MagicMock() + layer.public_joint_names = ("Root", *(f"Joint{index}" for index in range(77))) + layer.output_joint_parent_ids = np.array([0, 0, *range(1, 77)]) + return layer + + +def source(): + return SomaBodySource("body", "vendor.soma", fake_layer()) + + +def evaluated_pose(): + joints = SomaBodyJointPoseArrayV0() + joints.positions[:] = np.arange(231, dtype=np.float32).reshape(77, 3) + joints.orientations[:] = np.tile([0, 0, 0, 1], (77, 1)) + joints.is_valid[:] = 1 + return SomaBodyJointPosesV0(joints) + + +def payload_group(group_type, data): + group = TensorGroup(group_type) + group[0] = data + return group + + +def test_tracker_discovery_and_specs(): + soma_source = source() + assert isinstance(soma_source.get_tracker(), SomaBodyJointRotationsV0Tracker) + assert soma_source.get_vendor() is None + assert _get_trackers_from_pipeline(soma_source) == [soma_source.get_tracker()] + assert list(soma_source.input_spec()) == ["deviceio_soma_body"] + assert list(soma_source.output_spec()) == [SomaBodySource.BODY] + assert soma_source.output_spec()[SomaBodySource.BODY].is_optional + assert len(soma_source.joint_names) == 77 + assert len(soma_source.bones) == 76 + + +def test_polling_preserves_payload_before_source_evaluation(): + soma_source = source() + tracker = MagicMock() + soma_source._tracker = tracker + raw = pose() + tracker.get_data.return_value = raw + session = object() + inputs = soma_source.poll_tracker(session) + tracker.get_data.assert_called_once_with(session) + assert inputs["deviceio_soma_body"][0] is raw + + +def test_source_evaluates_received_payload_into_body_tensors(): + soma_source = source() + raw = pose() + expected = ( + np.ones((77, 3), dtype=np.float32), + np.tile([0, 0, 0, 1], (77, 1)).astype(np.float32), + np.ones(77, dtype=np.uint8), + ) + soma_source._evaluator.evaluate = MagicMock(return_value=expected) + inputs = payload_group(soma_source.input_spec()["deviceio_soma_body"], raw) + + evaluated = soma_source({"deviceio_soma_body": inputs})[SomaBodySource.BODY] + + soma_source._evaluator.evaluate.assert_called_once_with(raw) + np.testing.assert_array_equal( + evaluated[SomaBodyInputIndex.JOINT_POSITIONS], expected[0] + ) + np.testing.assert_array_equal( + evaluated[SomaBodyInputIndex.JOINT_ORIENTATIONS], expected[1] + ) + np.testing.assert_array_equal( + evaluated[SomaBodyInputIndex.JOINT_VALID], expected[2] + ) + + +def test_joint_pose_source_maps_received_payload_without_fk(): + soma_source = SomaBodySource( + "body", + "vendor.soma", + representation=SomaBodyRepresentation.JOINT_POSES, + ) + assert isinstance(soma_source.get_tracker(), SomaBodyJointPosesV0Tracker) + raw = evaluated_pose() + assert soma_source._evaluator is None + inputs = payload_group(soma_source.input_spec()["deviceio_soma_body"], raw) + + evaluated = soma_source({"deviceio_soma_body": inputs})[SomaBodySource.BODY] + + np.testing.assert_array_equal( + evaluated[SomaBodyInputIndex.JOINT_POSITIONS], raw.joint_poses.positions + ) + np.testing.assert_array_equal( + evaluated[SomaBodyInputIndex.JOINT_ORIENTATIONS], + raw.joint_poses.orientations, + ) + np.testing.assert_array_equal( + evaluated[SomaBodyInputIndex.JOINT_VALID], raw.joint_poses.is_valid + ) + + +def test_absent_sample_clears_reused_output(): + soma_source = source() + soma_source._evaluator.evaluate = MagicMock( + return_value=( + np.zeros((77, 3), dtype=np.float32), + np.zeros((77, 4), dtype=np.float32), + np.zeros(77, dtype=np.uint8), + ) + ) + inputs = payload_group(soma_source.input_spec()["deviceio_soma_body"], pose()) + outputs = { + SomaBodySource.BODY: _make_output_group( + soma_source.output_spec()[SomaBodySource.BODY] + ) + } + soma_source.compute({"deviceio_soma_body": inputs}, outputs) + assert not outputs[SomaBodySource.BODY].is_none + inputs[0] = None + soma_source.compute({"deviceio_soma_body": inputs}, outputs) + assert outputs[SomaBodySource.BODY].is_none + inputs[0] = pose() + soma_source.compute({"deviceio_soma_body": inputs}, outputs) + assert not outputs[SomaBodySource.BODY].is_none + + +def test_soma_and_fullbody_payloads_are_distinct(): + soma = DeviceIOSomaBodyJointRotationsV0Tracked() + soma_poses = DeviceIOSomaBodyJointPosesV0Tracked() + fullbody = DeviceIOFullBodyPoseTracked() + with pytest.raises(ValueError, match="type mismatch"): + soma.check_compatibility(fullbody) + with pytest.raises(TypeError, match="SomaBodyJointRotationsV0"): + payload_group(soma, FullBodyPose(BodyJoints(), False)) + with pytest.raises(TypeError, match="FullBodyPose"): + payload_group(fullbody, pose()) + with pytest.raises(ValueError, match="type mismatch"): + soma.check_compatibility(soma_poses) + with pytest.raises(TypeError, match="SomaBodyJointPosesV0"): + payload_group(soma_poses, pose()) + + +def test_empty_collection_rejected(): + with pytest.raises(ValueError, match="collection_id"): + SomaBodySource("body", "", fake_layer()) + + +def test_joint_rotations_require_prepared_layer(): + with pytest.raises(ValueError, match="prepared SOMA layer"): + SomaBodySource("body", "vendor.soma") diff --git a/tests/python/core/schema/test_soma_body_joint_poses_v0.py b/tests/python/core/schema/test_soma_body_joint_poses_v0.py new file mode 100644 index 0000000000..96bdbe99b6 --- /dev/null +++ b/tests/python/core/schema/test_soma_body_joint_poses_v0.py @@ -0,0 +1,51 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import pytest + +from isaaccapture.schema import ( + DeviceDataTimestamp, + SomaBodyJointPoseArrayV0, + SomaBodyJointPosesV0, + SomaBodyJointPosesV0Record, + SomaBodyJointV0, +) + + +def test_soma_body_joint_pose_views_alias_storage(): + joints = SomaBodyJointPoseArrayV0() + positions = np.arange(231, dtype=np.float32).reshape(77, 3) + orientations = np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)) + joints.positions[:] = positions + joints.orientations[:] = orientations + joints.is_valid[int(SomaBodyJointV0.HEAD)] = 1 + + np.testing.assert_array_equal(joints.positions, positions) + np.testing.assert_array_equal(joints.orientations, orientations) + head = joints.values(int(SomaBodyJointV0.HEAD)) + assert head.pose.position.x == positions[int(SomaBodyJointV0.HEAD), 0] + assert head.pose.orientation.w == 1.0 + assert head.is_valid + + +def test_soma_body_joint_pose_index_check(): + with pytest.raises(IndexError): + SomaBodyJointPoseArrayV0().values(77) + + +def test_soma_body_joint_poses_record_round_trip(): + joints = SomaBodyJointPoseArrayV0() + joints.positions[int(SomaBodyJointV0.HIPS)] = [1.0, 2.0, 3.0] + joints.orientations[:, 3] = 1.0 + joints.is_valid[:] = 1 + pose = SomaBodyJointPosesV0(joints) + record = SomaBodyJointPosesV0Record(pose, DeviceDataTimestamp(10, 20, 30)) + + np.testing.assert_array_equal(record.data.joint_poses.positions, joints.positions) + np.testing.assert_array_equal( + record.data.joint_poses.orientations, joints.orientations + ) + np.testing.assert_array_equal(record.data.joint_poses.is_valid, joints.is_valid) + assert record.data.to_bytes() == pose.to_bytes() + assert record.timestamp.available_time_local_common_clock == 10 diff --git a/tests/python/core/schema/test_soma_body_v0.py b/tests/python/core/schema/test_soma_body_v0.py new file mode 100644 index 0000000000..c069dacdaf --- /dev/null +++ b/tests/python/core/schema/test_soma_body_v0.py @@ -0,0 +1,213 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import gc + +import numpy as np +import pytest + +from isaaccapture.schema import ( + DeviceDataTimestamp, + Point, + Quaternion, + SomaBodyJointV0, + SomaBodyJointRotationArrayV0, + SomaBodyJointRotationsV0, + SomaBodyJointRotationsV0Record, + SomaJointRotationV0, +) + + +def test_soma_body_v0_joint_order(): + expected = [ + "HIPS", + "SPINE1", + "SPINE2", + "CHEST", + "NECK1", + "NECK2", + "HEAD", + "HEAD_END", + "JAW", + "LEFT_EYE", + "RIGHT_EYE", + "LEFT_SHOULDER", + "LEFT_ARM", + "LEFT_FORE_ARM", + "LEFT_HAND", + "LEFT_HAND_THUMB1", + "LEFT_HAND_THUMB2", + "LEFT_HAND_THUMB3", + "LEFT_HAND_THUMB_END", + "LEFT_HAND_INDEX1", + "LEFT_HAND_INDEX2", + "LEFT_HAND_INDEX3", + "LEFT_HAND_INDEX4", + "LEFT_HAND_INDEX_END", + "LEFT_HAND_MIDDLE1", + "LEFT_HAND_MIDDLE2", + "LEFT_HAND_MIDDLE3", + "LEFT_HAND_MIDDLE4", + "LEFT_HAND_MIDDLE_END", + "LEFT_HAND_RING1", + "LEFT_HAND_RING2", + "LEFT_HAND_RING3", + "LEFT_HAND_RING4", + "LEFT_HAND_RING_END", + "LEFT_HAND_PINKY1", + "LEFT_HAND_PINKY2", + "LEFT_HAND_PINKY3", + "LEFT_HAND_PINKY4", + "LEFT_HAND_PINKY_END", + "RIGHT_SHOULDER", + "RIGHT_ARM", + "RIGHT_FORE_ARM", + "RIGHT_HAND", + "RIGHT_HAND_THUMB1", + "RIGHT_HAND_THUMB2", + "RIGHT_HAND_THUMB3", + "RIGHT_HAND_THUMB_END", + "RIGHT_HAND_INDEX1", + "RIGHT_HAND_INDEX2", + "RIGHT_HAND_INDEX3", + "RIGHT_HAND_INDEX4", + "RIGHT_HAND_INDEX_END", + "RIGHT_HAND_MIDDLE1", + "RIGHT_HAND_MIDDLE2", + "RIGHT_HAND_MIDDLE3", + "RIGHT_HAND_MIDDLE4", + "RIGHT_HAND_MIDDLE_END", + "RIGHT_HAND_RING1", + "RIGHT_HAND_RING2", + "RIGHT_HAND_RING3", + "RIGHT_HAND_RING4", + "RIGHT_HAND_RING_END", + "RIGHT_HAND_PINKY1", + "RIGHT_HAND_PINKY2", + "RIGHT_HAND_PINKY3", + "RIGHT_HAND_PINKY4", + "RIGHT_HAND_PINKY_END", + "LEFT_LEG", + "LEFT_SHIN", + "LEFT_FOOT", + "LEFT_TOE_BASE", + "LEFT_TOE_END", + "RIGHT_LEG", + "RIGHT_SHIN", + "RIGHT_FOOT", + "RIGHT_TOE_BASE", + "RIGHT_TOE_END", + ] + + assert len(expected) == int(SomaBodyJointV0.NUM_JOINTS) == 77 + for index, name in enumerate(expected): + assert int(getattr(SomaBodyJointV0, name)) == index + + +def test_soma_body_v0_joint_rotation(): + rotation = SomaJointRotationV0(Quaternion(0.0, 0.0, 0.6, 0.8), True) + + assert rotation.rotation.x == 0.0 + assert rotation.rotation.y == 0.0 + assert rotation.rotation.z == pytest.approx(0.6) + assert rotation.rotation.w == pytest.approx(0.8) + assert rotation.is_valid is True + + +def test_soma_body_v0_rotation_views_alias_storage(): + rotations = SomaBodyJointRotationArrayV0() + + assert rotations.rotations.shape == (77, 4) + assert rotations.rotations.dtype == np.float32 + assert rotations.is_valid.shape == (77,) + assert rotations.is_valid.dtype == np.uint8 + assert not rotations.rotations.flags.owndata + + rotations.rotations[SomaBodyJointV0.HEAD] = [0.0, 0.0, 0.6, 0.8] + rotations.is_valid[SomaBodyJointV0.HEAD] = 1 + + head = rotations.values(int(SomaBodyJointV0.HEAD)) + assert head.rotation.z == pytest.approx(0.6) + assert head.rotation.w == pytest.approx(0.8) + assert head.is_valid is True + + +def test_soma_body_v0_rotation_index_check(): + with pytest.raises(IndexError): + SomaBodyJointRotationArrayV0().values(77) + + +def test_soma_body_v0_pose_construction_and_lifetime(): + rotations = SomaBodyJointRotationArrayV0() + rotations.rotations[:] = [0.0, 0.0, 0.0, 1.0] + rotations.rotations[SomaBodyJointV0.RIGHT_TOE_END] = [0.0, 0.0, 0.6, 0.8] + rotations.is_valid[:] = 1 + + pose = SomaBodyJointRotationsV0(rotations, Point(1.0, 2.0, 3.0), True) + quaternion_rotations = pose.joint_rotations.rotations + del pose + gc.collect() + + assert quaternion_rotations[76, 3] == pytest.approx(0.8) + + +def test_soma_body_v0_pose_defaults_and_global_translation(): + pose = SomaBodyJointRotationsV0() + + assert pose.joint_rotations is not None + assert pose.global_translation.x == 0.0 + assert pose.global_translation_is_valid is False + + translated = SomaBodyJointRotationsV0( + global_translation=Point(1.0, 2.0, 3.0), + global_translation_is_valid=True, + ) + assert translated.global_translation.y == pytest.approx(2.0) + assert translated.global_translation_is_valid is True + + +def test_soma_body_v0_pose_record(): + record = SomaBodyJointRotationsV0Record( + SomaBodyJointRotationsV0(), DeviceDataTimestamp(100, 200, 300) + ) + + assert record.data is not None + assert record.timestamp.sample_time_local_common_clock == 200 + + +@pytest.mark.parametrize("sign", [-1.0, 1.0]) +@pytest.mark.parametrize("translation_is_valid", [False, True]) +def test_soma_body_v0_record_preserves_quaternion_layout(sign, translation_is_valid): + quaternion = np.array([1.0, -2.0, 3.0, -4.0], dtype=np.float32) + quaternion *= sign / np.linalg.norm(quaternion) + rotations = SomaBodyJointRotationArrayV0() + rotations.rotations[SomaBodyJointV0.HIPS] = quaternion + rotations.rotations[SomaBodyJointV0.RIGHT_TOE_END] = -quaternion + rotations.is_valid[SomaBodyJointV0.HIPS] = 1 + rotations.is_valid[SomaBodyJointV0.RIGHT_TOE_END] = 1 + expected_rotations = rotations.rotations.copy() + expected_validity = rotations.is_valid.copy() + pose = SomaBodyJointRotationsV0( + rotations, Point(1.0, -2.0, 3.0), translation_is_valid + ) + record = SomaBodyJointRotationsV0Record(pose, DeviceDataTimestamp(100, 200, 300)) + + rotations.rotations[:] = 0.0 + rotations.is_valid[:] = 0 + pose.joint_rotations.rotations[:] = 0.0 + pose.joint_rotations.is_valid[:] = 0 + del pose, rotations + gc.collect() + + data = record.data + np.testing.assert_array_equal(data.joint_rotations.rotations, expected_rotations) + np.testing.assert_array_equal(data.joint_rotations.is_valid, expected_validity) + hips = data.joint_rotations.values(int(SomaBodyJointV0.HIPS)).rotation + np.testing.assert_array_equal([hips.x, hips.y, hips.z, hips.w], quaternion) + assert data.global_translation.x == pytest.approx(1.0) + assert data.global_translation.y == pytest.approx(-2.0) + assert data.global_translation.z == pytest.approx(3.0) + assert data.global_translation_is_valid is translation_is_valid + assert record.timestamp.available_time_local_common_clock == 100 + assert record.timestamp.sample_time_local_common_clock == 200 + assert record.timestamp.sample_time_raw_device_clock == 300 diff --git a/tests/python/examples/CMakeLists.txt b/tests/python/examples/CMakeLists.txt index 6d2c6b5a4b..c9b4f91b8b 100644 --- a/tests/python/examples/CMakeLists.txt +++ b/tests/python/examples/CMakeLists.txt @@ -2,3 +2,5 @@ # SPDX-License-Identifier: Apache-2.0 add_subdirectory(camera_viz) +add_subdirectory(deviceio_live_view) +add_subdirectory(soma_body_publisher) diff --git a/tests/python/examples/deviceio_live_view/CMakeLists.txt b/tests/python/examples/deviceio_live_view/CMakeLists.txt new file mode 100644 index 0000000000..fa25a62155 --- /dev/null +++ b/tests/python/examples/deviceio_live_view/CMakeLists.txt @@ -0,0 +1,21 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +file(GLOB TEST_FILES + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py" +) + +foreach(test_file ${TEST_FILES}) + get_filename_component(test_name "${test_file}" NAME_WE) + add_test( + NAME "deviceio_live_view_${test_name}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev + pytest -v --tb=short "${test_file}" + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + set_tests_properties("deviceio_live_view_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "deviceio_live_view;unit" + ) +endforeach() diff --git a/tests/python/examples/deviceio_live_view/conftest.py b/tests/python/examples/deviceio_live_view/conftest.py new file mode 100644 index 0000000000..69d3119def --- /dev/null +++ b/tests/python/examples/deviceio_live_view/conftest.py @@ -0,0 +1,25 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys +from pathlib import Path + +import pytest + +sys.path.insert(0, str(Path(__file__).resolve().parents[2])) + +from repo_paths import repo_root # noqa: E402 + +sys.path.insert(0, str(repo_root() / "examples" / "deviceio_live_view" / "python")) + + +def pytest_addoption(parser): + parser.addoption("--soma-assets", type=Path, help="SOMA-X assets directory") + + +@pytest.fixture(scope="module") +def soma_assets(request): + path = request.config.getoption("--soma-assets") + if path is None: + pytest.skip("Pass --soma-assets to run pinned SOMA-X semantic tests") + return path diff --git a/tests/python/examples/deviceio_live_view/pyproject.toml b/tests/python/examples/deviceio_live_view/pyproject.toml new file mode 100644 index 0000000000..940d702d84 --- /dev/null +++ b/tests/python/examples/deviceio_live_view/pyproject.toml @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "deviceio-live-view-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = ["pytest", "numpy", "viser>=0.2.0"] + +[tool.pytest.ini_options] +pythonpath = ["."] diff --git a/tests/python/examples/deviceio_live_view/test_soma_viewer.py b/tests/python/examples/deviceio_live_view/test_soma_viewer.py new file mode 100644 index 0000000000..c73ed10fb2 --- /dev/null +++ b/tests/python/examples/deviceio_live_view/test_soma_viewer.py @@ -0,0 +1,361 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from unittest.mock import MagicMock + +import numpy as np +import pytest +from isaaccapture.deviceio_trackers import ( + SomaBodyJointPosesV0Tracker, + SomaBodyJointRotationsV0Tracker, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + FullBodySource, + SomaBodyRepresentation, + SomaBodySource, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import soma_body_source +from isaaccapture.retargeting_engine.interface.base_retargeter import _make_output_group +from isaaccapture.retargeting_engine.interface.output_combiner import OutputCombiner +from isaaccapture.retargeting_engine.interface.tensor_group import TensorGroup +from isaaccapture.retargeting_engine.interface.tensor_group_type import OptionalType +from isaaccapture.retargeting_engine.tensor_types import ( + SomaBodyInput, + SomaBodyInputIndex, +) +from isaaccapture.schema import ( + Point, + SomaBodyJointPoseArrayV0, + SomaBodyJointPosesV0, + SomaBodyJointRotationArrayV0, + SomaBodyJointRotationsV0, +) +from isaaccapture_examples.deviceio_live_view import live_deviceio, soma_body +from isaaccapture_examples.deviceio_live_view.body_pipeline import ( + BodySchema, + BodyViewLayout, + create_body_view_pipeline, +) +from isaaccapture_examples.deviceio_live_view.deviceio_pipeline import ( + build_all_human_pipeline, +) +from isaaccapture_examples.deviceio_live_view.deviceio_viser import ( + HumanDeviceIOViz, + _valid_bone_segments, +) +from isaaccapture_examples.deviceio_live_view.full_body_pose import ( + FULL_BODY_POSE_LAYOUT, +) + + +def fake_layer(): + layer = MagicMock() + layer.public_joint_names = ("Root", *(f"Joint{index}" for index in range(77))) + layer.output_joint_parent_ids = np.array([0, 0, *range(1, 77)]) + return layer + + +def soma_joint_rotations(quaternions, translation): + joints = SomaBodyJointRotationArrayV0() + joints.rotations[:] = quaternions + joints.is_valid[:] = 1 + return SomaBodyJointRotationsV0(joints, Point(*translation), True) + + +def soma_joint_poses(positions, orientations): + joints = SomaBodyJointPoseArrayV0() + joints.positions[:] = positions + joints.orientations[:] = orientations + joints.is_valid[:] = 1 + return SomaBodyJointPosesV0(joints) + + +def fake_viz(layout: BodyViewLayout = FULL_BODY_POSE_LAYOUT): + server = MagicMock() + for name in ("add_point_cloud", "add_line_segments", "add_frame"): + getattr(server.scene, name).side_effect = lambda **kwargs: MagicMock(**kwargs) + return HumanDeviceIOViz(server, MagicMock(), layout) + + +def test_default_pipeline_keeps_full_body_source(): + pipeline = build_all_human_pipeline() + sources = {source.name: source for source in pipeline.get_leaf_nodes()} + assert set(sources) == {"hands", "head", "controllers", "body"} + assert type(sources["body"]) is FullBodySource + assert set(pipeline.output_types()) == { + "hand_left", + "hand_right", + "head", + "controller_left", + "controller_right", + "body", + } + + +@pytest.mark.parametrize("explicit", [False, True]) +def test_soma_pipeline_replaces_only_body_source(monkeypatch, tmp_path, explicit): + monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) + tracker_factory = MagicMock(wraps=SomaBodyJointRotationsV0Tracker) + monkeypatch.setattr( + soma_body_source, "SomaBodyJointRotationsV0Tracker", tracker_factory + ) + body = create_body_view_pipeline( + tmp_path, + body_schema=BodySchema.SOMA if explicit else None, + soma_collection_id="vendor_body", + ) + pipeline = build_all_human_pipeline(body=body) + sources = {source.name: source for source in pipeline.get_leaf_nodes()} + assert set(sources) == {"hands", "head", "controllers", "body"} + assert type(sources["body"]) is SomaBodySource + assert isinstance(sources["body"].get_tracker(), SomaBodyJointRotationsV0Tracker) + tracker_factory.assert_called_once_with("vendor_body") + assert len(body.layout.joint_names) == 77 + assert len(body.layout.bones) == 76 + + +def test_soma_joint_pose_pipeline_selects_direct_tracker(monkeypatch, tmp_path): + monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) + tracker_factory = MagicMock(wraps=SomaBodyJointPosesV0Tracker) + monkeypatch.setattr( + soma_body_source, "SomaBodyJointPosesV0Tracker", tracker_factory + ) + body = create_body_view_pipeline( + tmp_path, + body_schema=BodySchema.SOMA, + soma_collection_id="vendor_body", + soma_body_representation=SomaBodyRepresentation.JOINT_POSES, + ) + pipeline = build_all_human_pipeline(body=body) + source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") + assert isinstance(source.get_tracker(), SomaBodyJointPosesV0Tracker) + tracker_factory.assert_called_once_with("vendor_body") + + +def test_explicit_full_body_selection_does_not_load_soma(monkeypatch, tmp_path): + create = MagicMock(side_effect=AssertionError("SOMA must remain optional")) + monkeypatch.setattr(soma_body, "create_layer", create) + body = create_body_view_pipeline(tmp_path, body_schema=BodySchema.FULL_BODY_POSE) + pipeline = build_all_human_pipeline(body=body) + source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") + assert type(source) is FullBodySource + create.assert_not_called() + + +def test_soma_evaluation_runs_once_per_step_downstream(monkeypatch, tmp_path): + monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) + body = create_body_view_pipeline(tmp_path) + selector = body.output + pipeline = build_all_human_pipeline(body=body) + source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") + raw = soma_joint_rotations(np.tile([0, 0, 0, 1], (77, 1)), [1, 2, 3]) + source._tracker = MagicMock() + source._tracker.get_data.return_value = raw + evaluate = MagicMock( + return_value=( + np.zeros((77, 3), dtype=np.float32), + np.tile([0, 0, 0, 1], (77, 1)).astype(np.float32), + np.ones(77, dtype=np.uint8), + ) + ) + monkeypatch.setattr(source._evaluator, "evaluate", evaluate) + inputs = source.poll_tracker(object()) + combined = OutputCombiner({"view": selector, "other_consumer": selector}) + result = combined.execute_pipeline({source.name: inputs}) + evaluate.assert_called_once_with(raw) + assert result["view"] is result["other_consumer"] + combined.execute_pipeline({source.name: inputs}) + assert evaluate.call_count == 2 + + +def test_viewer_uses_selected_native_body_layout_and_hides_missing_devices(): + pipeline = build_all_human_pipeline() + result = { + name: _make_output_group(kind) for name, kind in pipeline.output_types().items() + } + for group in result.values(): + group.set_none() + + layout = BodyViewLayout( + joint_names=tuple(f"Joint{index}" for index in range(77)), + bones=((0, 1), (1, 2)), + positions_index=int(SomaBodyInputIndex.JOINT_POSITIONS), + valid_index=int(SomaBodyInputIndex.JOINT_VALID), + ) + body = _make_output_group(OptionalType(SomaBodyInput())) + positions = np.arange(231, dtype=np.float32).reshape(77, 3) / 100 + valid = np.zeros(77, dtype=np.uint8) + valid[:3] = (1, 1, 0) + body[SomaBodyInputIndex.JOINT_POSITIONS] = positions + body[SomaBodyInputIndex.JOINT_ORIENTATIONS] = np.tile( + np.array([0, 0, 0, 1], dtype=np.float32), (77, 1) + ) + body[SomaBodyInputIndex.JOINT_VALID] = valid + result["body"] = body + + viz = fake_viz(layout) + active = viz.update(result) + assert active["body_active"] + assert active["body_joints"] == 2 + assert not any( + active[key] + for key in ( + "hand_left", + "hand_right", + "head", + "controller_left", + "controller_right", + ) + ) + np.testing.assert_array_equal(viz.body.points.points, positions) + np.testing.assert_array_equal( + viz.body.bones.points, _valid_bone_segments(positions, valid, layout.bones) + ) + assert len(viz.body.bones.points) == 1 + viz._ground.follow.assert_called_once() + body.set_none() + assert not viz.update(result)["body_active"] + assert not viz.body.points.visible + assert not viz.body.bones.visible + + +@pytest.mark.parametrize( + "schema,use_assets,expected,collection", + [ + (None, False, BodySchema.FULL_BODY_POSE, "soma_demo"), + (None, True, BodySchema.SOMA, "soma_demo"), + ("soma", True, BodySchema.SOMA, "vendor_body"), + ("full-body-pose", True, BodySchema.FULL_BODY_POSE, "soma_demo"), + ], +) +def test_live_cli_selects_source_and_launches_runtime( + monkeypatch, tmp_path, schema, use_assets, expected, collection +): + body = create_body_view_pipeline() + select = MagicMock(return_value=body) + monkeypatch.setattr(live_deviceio, "create_body_view_pipeline", select) + pipeline = build_all_human_pipeline() + build = MagicMock(return_value=pipeline) + monkeypatch.setattr(live_deviceio, "build_all_human_pipeline", build) + launch = MagicMock() + launch.return_value.__enter__.return_value.owns_runtime = False + monkeypatch.setattr(live_deviceio.CloudXRLauncher, "launch_context", launch) + server = MagicMock() + monkeypatch.setattr(live_deviceio.viser, "ViserServer", lambda **kwargs: server) + session = MagicMock() + session.__enter__.return_value = session + session.step.side_effect = KeyboardInterrupt + create_session = MagicMock(return_value=session) + monkeypatch.setattr(live_deviceio, "TeleopSession", create_session) + args = ["viewer"] + if schema is not None: + args += ["--body-schema", schema] + if use_assets: + args += ["--soma-data-root", str(tmp_path)] + args += ["--soma-collection-id", collection] + assert live_deviceio.main(args) == 0 + select.assert_called_once_with( + tmp_path if use_assets else None, + body_schema=expected, + soma_collection_id=collection, + soma_body_representation="joint-rotations", + ) + build.assert_called_once_with(body=body) + launch.assert_called_once() + assert create_session.call_args.args[0].pipeline is pipeline + + +@pytest.mark.parametrize( + "args,message", + [ + (["--body-schema", "soma"], "requires --soma-data-root"), + (["--body-schema", "unknown"], "invalid choice"), + ( + [ + "--body-schema", + "soma", + "--soma-data-root", + "/unused", + "--soma-collection-id", + "", + ], + "collection_id must not be empty", + ), + ], +) +def test_bad_body_selection_fails_before_launch(monkeypatch, capsys, args, message): + monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) + launch = MagicMock() + monkeypatch.setattr(live_deviceio.CloudXRLauncher, "launch_context", launch) + server = MagicMock() + monkeypatch.setattr(live_deviceio.viser, "ViserServer", server) + with pytest.raises(SystemExit) as error: + live_deviceio.main(["viewer", *args]) + assert error.value.code == 2 + assert message in capsys.readouterr().err + launch.assert_not_called() + server.assert_not_called() + + +def test_native_soma_pose_reaches_renderer(soma_assets): + body = create_body_view_pipeline(soma_assets) + pipeline = build_all_human_pipeline(body=body) + source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") + source._tracker = MagicMock() + inputs = {} + for node in pipeline.get_leaf_nodes(): + inputs[node.name] = {} + for key, kind in node.input_spec().items(): + group = TensorGroup(kind) + group[0] = None + inputs[node.name][key] = group + viz = fake_viz(body.layout) + pose = soma_joint_rotations(np.tile([0, 0, 0, 1], (77, 1)), [0, 0, 0]) + source._tracker.get_data.return_value = pose + inputs[source.name] = source.poll_tracker(object()) + result = pipeline.execute_pipeline(inputs) + assert viz.update(result)["body_joints"] == 77 + expected_positions, _, _ = source._evaluator.evaluate(pose) + np.testing.assert_allclose(viz.body.points.points, expected_positions) + + source._tracker.get_data.return_value = None + inputs[source.name] = source.poll_tracker(object()) + assert not viz.update(pipeline.execute_pipeline(inputs))["body_active"] + + invalid = soma_joint_rotations(np.tile([0, 0, 0, 1], (77, 1)), [0, 0, 0]) + invalid.joint_rotations.is_valid[10] = 0 + source._tracker.get_data.return_value = invalid + inputs[source.name] = source.poll_tracker(object()) + active = viz.update(pipeline.execute_pipeline(inputs)) + assert active["body_active"] + assert 0 < active["body_joints"] < 77 + + +def test_evaluated_soma_pose_reaches_renderer_without_fk(soma_assets): + body = create_body_view_pipeline( + soma_assets, + body_schema=BodySchema.SOMA, + soma_body_representation=SomaBodyRepresentation.JOINT_POSES, + ) + pipeline = build_all_human_pipeline(body=body) + source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") + source._tracker = MagicMock() + source._evaluator.evaluate = MagicMock( + side_effect=AssertionError("joint poses must bypass FK") + ) + inputs = {} + for node in pipeline.get_leaf_nodes(): + inputs[node.name] = {} + for key, kind in node.input_spec().items(): + group = TensorGroup(kind) + group[0] = None + inputs[node.name][key] = group + positions = np.arange(231, dtype=np.float32).reshape(77, 3) / 100 + orientations = np.tile([0, 0, 0, 1], (77, 1)).astype(np.float32) + source._tracker.get_data.return_value = soma_joint_poses(positions, orientations) + inputs[source.name] = source.poll_tracker(object()) + + viz = fake_viz(body.layout) + assert viz.update(pipeline.execute_pipeline(inputs))["body_joints"] == 77 + np.testing.assert_array_equal(viz.body.points.points, positions) + source._evaluator.evaluate.assert_not_called() diff --git a/tests/python/examples/soma_body_publisher/CMakeLists.txt b/tests/python/examples/soma_body_publisher/CMakeLists.txt new file mode 100644 index 0000000000..fa33402f98 --- /dev/null +++ b/tests/python/examples/soma_body_publisher/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +file(GLOB TEST_FILES + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py" +) + +foreach(test_file ${TEST_FILES}) + get_filename_component(test_name "${test_file}" NAME_WE) + add_test( + NAME "soma_body_publisher_${test_name}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev + pytest -v --tb=short "${test_file}" + --soma-pusher $ + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + set_tests_properties("soma_body_publisher_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "soma_body_publisher;unit" + ) +endforeach() diff --git a/tests/python/examples/soma_body_publisher/conftest.py b/tests/python/examples/soma_body_publisher/conftest.py new file mode 100644 index 0000000000..a432ae052a --- /dev/null +++ b/tests/python/examples/soma_body_publisher/conftest.py @@ -0,0 +1,26 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys +from pathlib import Path + +import pytest + +sys.path.insert(0, str(Path(__file__).resolve().parents[2])) + +from repo_paths import repo_root # noqa: E402 + +sys.path.insert(0, str(repo_root() / "examples" / "soma_body_publisher" / "python")) + + +def pytest_addoption(parser): + parser.addoption("--soma-assets", type=Path, help="SOMA-X assets directory") + parser.addoption("--soma-pusher", type=Path, help="Built soma_body_pusher") + + +@pytest.fixture(scope="module") +def soma_assets(request): + path = request.config.getoption("--soma-assets") + if path is None: + pytest.skip("Pass --soma-assets to run pinned SOMA-X semantic tests") + return path diff --git a/tests/python/examples/soma_body_publisher/pyproject.toml b/tests/python/examples/soma_body_publisher/pyproject.toml new file mode 100644 index 0000000000..d44c7a901b --- /dev/null +++ b/tests/python/examples/soma_body_publisher/pyproject.toml @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "soma-body-publisher-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = ["pytest", "numpy"] +soma = ["py-soma-x==0.3.1"] + +[tool.pytest.ini_options] +pythonpath = ["."] diff --git a/tests/python/examples/soma_body_publisher/test_publisher.py b/tests/python/examples/soma_body_publisher/test_publisher.py new file mode 100644 index 0000000000..15b59aa3b5 --- /dev/null +++ b/tests/python/examples/soma_body_publisher/test_publisher.py @@ -0,0 +1,199 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import signal +import struct +import subprocess + +import numpy as np +import pytest + +from isaaccapture.retargeting_engine.utilities.soma_body_evaluator import ( + _SomaBodyEvaluator, +) +from isaaccapture.schema import ( + DeviceDataTimestamp, + SomaBodyJointPosesV0Record, + SomaBodyJointRotationsV0Record, +) +from isaaccapture_examples.soma_body_publisher import publisher +from isaaccapture_examples.soma_body_publisher.publisher import ( + create_layer, + demo_controls, + evaluate_demo_controls, + soma_joint_poses, + soma_joint_rotations, +) + + +def test_nested_soma_serialization_uses_payload_root(): + q = np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)) + pose = soma_joint_rotations(q, [1, 2, 3]) + record = SomaBodyJointRotationsV0Record(pose, DeviceDataTimestamp(10, 20, 30)) + assert record.data.to_bytes() == pose.to_bytes() + assert record.to_bytes() != pose.to_bytes() + + evaluated = soma_joint_poses( + np.zeros((77, 3), dtype=np.float32), + np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)), + ) + evaluated_record = SomaBodyJointPosesV0Record( + evaluated, DeviceDataTimestamp(10, 20, 30) + ) + assert evaluated_record.data.to_bytes() == evaluated.to_bytes() + + +@pytest.mark.parametrize("exit_code", [0, -signal.SIGINT, 1]) +def test_publisher_interrupt_preserves_real_failures(monkeypatch, tmp_path, exit_code): + from unittest.mock import MagicMock + + process = MagicMock() + process.__enter__.return_value = process + process.stdin.write.side_effect = KeyboardInterrupt + process.wait.return_value = exit_code + monkeypatch.setattr(publisher.subprocess, "Popen", lambda *args, **kwargs: process) + monkeypatch.setattr(publisher, "create_layer", lambda _: None) + monkeypatch.setattr( + publisher, + "demo_controls", + lambda *args: (np.tile([0, 0, 0, 1], (1, 77, 1)), [[0, 0, 0]], None), + ) + args = ["soma_body_publisher", "--data-root", str(tmp_path), "--pusher", "pusher"] + if exit_code == 1: + with pytest.raises(RuntimeError, match="pusher failed"): + publisher.main(args) + else: + assert publisher.main(args) == 0 + process.stdin.close.assert_called_once() + + +@pytest.mark.parametrize( + "packet", + [b"x", struct.pack(" Date: Tue, 6 Oct 2026 09:45:23 -0400 Subject: [PATCH 02/12] feat(soma): add standalone hand pose transport pipeline Add versioned SOMA V0 hand joint-rotation and evaluated joint-pose schemas. Extend the shared primitives with handedness and integrate generated trackers, Python bindings, schema goldens, documentation, and validation coverage. Normalize both hand representations into SomaHandInput using upstream FK or direct mapping. Add independent left and right trackers, a bundled-motion hand publisher, and DeviceIO viewer selection between OpenXR and native SOMA hands. Keep OpenXR hands as the default and keep body and hand source selection independent. Signed-off-by: Michael Haselton --- docs/source/device/trackers.rst | 42 +++ docs/source/references/generated_trackers.rst | 6 + examples/CMakeLists.txt | 1 + examples/deviceio_live_view/README.md | 34 ++- .../deviceio_live_view/deviceio_pipeline.py | 11 +- .../deviceio_live_view/deviceio_viser.py | 96 +++---- .../deviceio_live_view/hand_pipeline.py | 68 +++++ .../deviceio_live_view/live_deviceio.py | 40 ++- .../deviceio_live_view/openxr_hand_pose.py | 60 +++++ .../deviceio_live_view/soma_hand.py | 118 +++++++++ examples/soma_hand_publisher/CMakeLists.txt | 9 + examples/soma_hand_publisher/README.md | 56 ++++ .../cpp/soma_hand_pusher/CMakeLists.txt | 15 ++ .../cpp/soma_hand_pusher/soma_hand_pusher.cpp | 182 +++++++++++++ examples/soma_hand_publisher/pyproject.toml | 24 ++ .../soma_hand_publisher/__init__.py | 2 + .../soma_hand_publisher/__main__.py | 8 + .../soma_hand_publisher/publisher.py | 241 ++++++++++++++++++ src/core/deviceio_trackers/trackers.toml | 16 ++ src/core/schema/fbs/soma_common_v0.fbs | 9 +- .../schema/fbs/soma_hand_joint_poses_v0.fbs | 37 +++ src/core/schema/fbs/soma_hand_v0.fbs | 71 ++++++ src/core/schema/golden/soma_body_v0.bfbs | Bin 6696 -> 6912 bytes src/core/schema/golden/soma_common_v0.bfbs | Bin 716 -> 1024 bytes .../golden/soma_hand_joint_poses_v0.bfbs | Bin 0 -> 4632 bytes src/core/schema/golden/soma_hand_v0.bfbs | Bin 0 -> 3728 bytes src/core/schema/python/CMakeLists.txt | 2 + src/core/schema/python/schema_module.cpp | 6 +- .../schema/python/soma_common_v0_bindings.h | 5 + .../soma_hand_joint_poses_v0_bindings.h | 100 ++++++++ .../schema/python/soma_hand_v0_bindings.h | 119 +++++++++ .../deviceio_source_nodes/__init__.py | 11 + .../deviceio_tensor_types.py | 30 +++ .../deviceio_source_nodes/soma_hand_source.py | 109 ++++++++ .../tensor_types/__init__.py | 6 + .../tensor_types/indices.py | 4 + .../tensor_types/standard_types.py | 28 ++ .../utilities/soma_hand_evaluator.py | 94 +++++++ src/python/isaaccapture/schema/__init__.py | 22 ++ tests/cpp/core/schema/CMakeLists.txt | 2 + .../schema/test_soma_hand_joint_poses_v0.cpp | 49 ++++ tests/cpp/core/schema/test_soma_hand_v0.cpp | 130 ++++++++++ .../test_soma_hand_source.py | 139 ++++++++++ .../schema/test_soma_hand_joint_poses_v0.py | 54 ++++ tests/python/core/schema/test_soma_hand_v0.py | 173 +++++++++++++ tests/python/examples/CMakeLists.txt | 1 + .../deviceio_live_view/test_soma_viewer.py | 143 ++++++++++- .../soma_hand_publisher/CMakeLists.txt | 22 ++ .../examples/soma_hand_publisher/conftest.py | 28 ++ .../soma_hand_publisher/pyproject.toml | 14 + .../soma_hand_publisher/test_publisher.py | 118 +++++++++ 51 files changed, 2475 insertions(+), 80 deletions(-) create mode 100644 examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py create mode 100644 examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/openxr_hand_pose.py create mode 100644 examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py create mode 100644 examples/soma_hand_publisher/CMakeLists.txt create mode 100644 examples/soma_hand_publisher/README.md create mode 100644 examples/soma_hand_publisher/cpp/soma_hand_pusher/CMakeLists.txt create mode 100644 examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp create mode 100644 examples/soma_hand_publisher/pyproject.toml create mode 100644 examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/__init__.py create mode 100644 examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/__main__.py create mode 100644 examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py create mode 100644 src/core/schema/fbs/soma_hand_joint_poses_v0.fbs create mode 100644 src/core/schema/fbs/soma_hand_v0.fbs create mode 100644 src/core/schema/golden/soma_hand_joint_poses_v0.bfbs create mode 100644 src/core/schema/golden/soma_hand_v0.bfbs create mode 100644 src/core/schema/python/soma_hand_joint_poses_v0_bindings.h create mode 100644 src/core/schema/python/soma_hand_v0_bindings.h create mode 100644 src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py create mode 100644 src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py create mode 100644 tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp create mode 100644 tests/cpp/core/schema/test_soma_hand_v0.cpp create mode 100644 tests/python/core/retargeting_engine/test_soma_hand_source.py create mode 100644 tests/python/core/schema/test_soma_hand_joint_poses_v0.py create mode 100644 tests/python/core/schema/test_soma_hand_v0.py create mode 100644 tests/python/examples/soma_hand_publisher/CMakeLists.txt create mode 100644 tests/python/examples/soma_hand_publisher/conftest.py create mode 100644 tests/python/examples/soma_hand_publisher/pyproject.toml create mode 100644 tests/python/examples/soma_hand_publisher/test_publisher.py diff --git a/docs/source/device/trackers.rst b/docs/source/device/trackers.rst index 372db901d8..99e42cc8e5 100644 --- a/docs/source/device/trackers.rst +++ b/docs/source/device/trackers.rst @@ -26,6 +26,8 @@ reading it from OpenXR tensor collections via the - :code-file:`Se3Tracker ` -- generic SE3 (6-DoF) pose sources (tracker pucks, mocap rigid bodies, logical trackers) (generated) - :code-file:`SomaBodyJointRotationsV0Tracker ` -- SOMA v0 body joint rotations with 77 controls (generated) - :code-file:`SomaBodyJointPosesV0Tracker ` -- evaluated SOMA v0 body poses for 77 joints (generated) +- :code-file:`SomaHandJointRotationsV0Tracker ` -- SOMA v0 hand joint rotations with 25 controls (generated) +- :code-file:`SomaHandJointPosesV0Tracker ` -- evaluated SOMA v0 hand poses for 25 joints (generated) All trackers follow the same lifecycle: @@ -335,6 +337,46 @@ body-schema selection. It does not map SOMA into ``FullBodyPose``. - :code-file:`tests/python/core/schema/test_soma_body_v0.py` - :code-file:`tests/python/core/retargeting_engine/test_soma_body_source.py` +SomaHandJointRotationsV0Tracker and SomaHandJointPosesV0Tracker +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Reads either of the two SOMA hand transport profiles defined and verified against SOMA-X v0.3.1. +``SomaHandJointRotationsV0`` contains 25 unit XYZW quaternions in ``SomaHandJointV0`` order. Wrist +is the global rotation; the remaining rotations are joint-local relative to the fixed v0.3.1 +reference contract. ``global_translation`` is applied to Wrist and uses the same units and +reference-frame convention as ``SomaBodyJointRotationsV0``. + +When a producer has already evaluated forward kinematics, it can instead publish +``SomaHandJointPosesV0``. That profile carries a global position, orientation, and validity for each +of the same 25 joints and requires no downstream FK. Create one tracker per hand collection; each +payload identifies its side through ``handedness``. As with the body contract, a vendor adapter +converts its device-native hand data before calling ``SchemaPusher``. + +For a retargeting graph, ``SomaHandSource`` selects the rotation tracker by default and emits an +evaluated ``SomaHandInput``. Pass ``representation="joint-poses"`` to select the evaluated-pose +tracker. A rotation source requires a prepared SOMA hand layer; an evaluated-pose source does not. +The DeviceIO live-view example selects body and hand inputs independently and creates one source for +each hand. + +- Schemas: :code-file:`src/core/schema/fbs/soma_common_v0.fbs`, :code-file:`src/core/schema/fbs/soma_hand_v0.fbs`, :code-file:`src/core/schema/fbs/soma_hand_joint_poses_v0.fbs` +- Manifest: :code-file:`src/core/deviceio_trackers/trackers.toml` (``soma_hand_joint_rotations_v0`` and ``soma_hand_joint_poses_v0``) +- Rotation C++ header: ``#include `` +- Rotation Python import: ``from isaaccapture.deviceio_trackers import SomaHandJointRotationsV0Tracker`` +- Evaluated-pose C++ header: ``#include `` +- Evaluated-pose Python import: ``from isaaccapture.deviceio_trackers import SomaHandJointPosesV0Tracker`` +- Graph source: :code-file:`src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py` +- Live-view example: :code-file:`examples/deviceio_live_view/README.md` +- Demo publisher: :code-file:`examples/soma_hand_publisher/README.md` +- Rotation record channels: ``soma_hand_joint_rotations_v0``, ``soma_hand_joint_rotations_v0_tracked`` | MCAP schema: ``core.SomaHandJointRotationsV0Record`` +- Evaluated-pose record channels: ``soma_hand_joint_poses_v0``, ``soma_hand_joint_poses_v0_tracked`` | MCAP schema: ``core.SomaHandJointPosesV0Record`` +- Tests: + + - :code-file:`tests/cpp/core/schema/test_soma_hand_v0.cpp` + - :code-file:`tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp` + - :code-file:`tests/python/core/schema/test_soma_hand_v0.py` + - :code-file:`tests/python/core/schema/test_soma_hand_joint_poses_v0.py` + - :code-file:`tests/python/core/retargeting_engine/test_soma_hand_source.py` + FrameMetadataTrackerOak ~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/docs/source/references/generated_trackers.rst b/docs/source/references/generated_trackers.rst index 8b23e61f4e..b796c5efcd 100644 --- a/docs/source/references/generated_trackers.rst +++ b/docs/source/references/generated_trackers.rst @@ -95,6 +95,12 @@ Generated today * - ``soma_body_joint_poses_v0`` - ``SomaBodyJointPosesV0Tracker`` - Evaluated SOMA v0 positions and orientations for the same 77 public joints + * - ``soma_hand_joint_rotations_v0`` + - ``SomaHandJointRotationsV0Tracker`` + - SOMA v0 hand joint rotations with 25 controls; defined against SOMA-X v0.3.1 + * - ``soma_hand_joint_poses_v0`` + - ``SomaHandJointPosesV0Tracker`` + - Evaluated SOMA v0 positions and orientations for the same 25 joints * - ``oglo_tactile`` - ``OgloTactileTracker`` - MCAP channels are ``oglo``/``oglo_tracked``, so ``channel`` is overridden diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index af5f8720b0..9b96fea7f9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -18,6 +18,7 @@ if(BUILD_EXAMPLES) add_subdirectory(mcap_record_replay) add_subdirectory(deviceio_live_view) add_subdirectory(soma_body_publisher) + add_subdirectory(soma_hand_publisher) add_subdirectory(haptic_feedback) add_subdirectory(sharpa_avatar) add_subdirectory(vehicle_teleop) diff --git a/examples/deviceio_live_view/README.md b/examples/deviceio_live_view/README.md index e324fc0eb7..0457a8d848 100644 --- a/examples/deviceio_live_view/README.md +++ b/examples/deviceio_live_view/README.md @@ -77,8 +77,38 @@ Missing data hides the body. For joint rotations, invalid translation invalidate the complete skeleton. An invalid joint control also invalidates its descendants because their global poses depend on that ancestor. Evaluated joint poses carry independent per-joint validity. Other valid branches remain visible. -Independent hands, subject calibration, a `FullBodyPose` compatibility -retargeter, and recording/replay examples are outside this live-view POC. +Body and hand sources are selected independently. Subject calibration, body-hand +fusion, a `FullBodyPose` compatibility retargeter, and recording/replay examples +are outside this live-view POC. + +## SOMA hand POC + +OpenXR `HandPose` remains the default hand input. Select independent left and +right SOMA hand collections with `--hand-schema soma`: + +```text +SomaHandJointRotationsV0 -> tracker -> upstream FK --+ + +-> SomaHandInput -> viewer +SomaHandJointPosesV0 ----> tracker -> direct map -------+ +``` + +```bash +uv run --no-sync python -m isaaccapture_examples.deviceio_live_view --accept-eula \ + --hand-schema soma \ + --soma-data-root /path/to/SOMA-X-v0.3.1-assets +``` + +The rotation profile requires the v0.3.1 `SOMAHand.npz` asset and uses the +neutral hand identity for FK. Use `--soma-hand-representation joint-poses` for +evaluated poses, which do not require SOMA-X during per-frame source evaluation. +The default collections are `soma_hand_left_demo` and +`soma_hand_right_demo`; override them with +`--soma-left-hand-collection-id` and `--soma-right-hand-collection-id`. + +The independent [SOMA hand publisher](../soma_hand_publisher/README.md) sends +both hands from the bundled animation. The viewer renders the 25-joint SOMA +topology directly. It does not merge these streams with the hand joints embedded +in a SOMA body payload. The focused tests cover upstream FK agreement, body-source selection, native SOMA topology, and the shared renderer. Real-asset checks are optional: diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py index 96e5d2f326..0d18346dad 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py @@ -5,14 +5,12 @@ from isaaccapture.retargeting_engine.deviceio_source_nodes import ( ControllersSource, - HandsSource, HeadSource, ) from isaaccapture.retargeting_engine.interface import OutputCombiner from .body_pipeline import BodyViewPipeline, create_body_view_pipeline - -HANDS_CHANNEL = "hands" +from .hand_pipeline import HandViewPipeline, create_hand_view_pipeline def build_all_human_pipeline( @@ -22,9 +20,9 @@ def build_all_human_pipeline( soma_collection_id="soma_demo", soma_body_representation="joint-rotations", body: BodyViewPipeline | None = None, + hands: HandViewPipeline | None = None, ): """Wire every human-related DeviceIO source into one pipeline.""" - hands = HandsSource(name=HANDS_CHANNEL) head = HeadSource(name="head") controllers = ControllersSource(name="controllers") selected_body = body or create_body_view_pipeline( @@ -33,10 +31,11 @@ def build_all_human_pipeline( soma_collection_id=soma_collection_id, soma_body_representation=soma_body_representation, ) + selected_hands = hands or create_hand_view_pipeline() return OutputCombiner( { - "hand_left": hands.output(HandsSource.LEFT), - "hand_right": hands.output(HandsSource.RIGHT), + "hand_left": selected_hands.left, + "hand_right": selected_hands.right, "head": head.output("head"), "controller_left": controllers.output(ControllersSource.LEFT), "controller_right": controllers.output(ControllersSource.RIGHT), diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_viser.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_viser.py index 6829eb31a3..2fe5e544ea 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_viser.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_viser.py @@ -6,13 +6,12 @@ Viz classes: ``HandViz``, ``ControllerViz``, ``BodyViz``, ``HeadViz``, ``HumanDeviceIOViz``. -Rendering helpers: ``HAND_BONES``, ``BODY_BONES``, ``controller_state``. +Rendering helpers: ``BODY_BONES``, ``controller_state``. """ import numpy as np import viser -from isaaccapture.retargeting_engine.tensor_types import HandInputIndex from isaaccapture.retargeting_engine.tensor_types.indices import ( ControllerInputIndex, HeadInputIndex, @@ -20,6 +19,8 @@ from .body_pipeline import BodyViewLayout from .full_body_pose import FULL_BODY_POSE_BONES, FULL_BODY_POSE_LAYOUT +from .hand_pipeline import HandViewLayout +from .openxr_hand_pose import OPENXR_HAND_LAYOUT BODY_JOINT_NAMES = list(FULL_BODY_POSE_LAYOUT.joint_names) BODY_BONES = FULL_BODY_POSE_BONES @@ -102,50 +103,6 @@ def setup_scene(server) -> GroundGrid: return GroundGrid(server, grid) -# OpenXR hand-joint connectivity (parent → child) for skeleton rendering. -# Indices follow XR_HAND_JOINT_*_EXT: 0=PALM, 1=WRIST, thumb has 4 joints -# (no intermediate), the other 4 fingers have 5 joints each — 26 total. -HAND_BONES: tuple[tuple[int, int], ...] = ( - # Thumb - (1, 2), - (2, 3), - (3, 4), - (4, 5), - # Index - (1, 6), - (6, 7), - (7, 8), - (8, 9), - (9, 10), - # Middle - (1, 11), - (11, 12), - (12, 13), - (13, 14), - (14, 15), - # Ring - (1, 16), - (16, 17), - (17, 18), - (18, 19), - (19, 20), - # Little - (1, 21), - (21, 22), - (22, 23), - (23, 24), - (24, 25), -) - - -def _bone_segments(positions: np.ndarray) -> np.ndarray: - """Return (N, 2, 3) segment array for the parent→child hand bones.""" - return np.stack( - [np.stack([positions[a], positions[b]], axis=0) for a, b in HAND_BONES], - axis=0, - ).astype(np.float32) - - def _valid_bone_segments( positions: np.ndarray, valid: np.ndarray, @@ -218,34 +175,36 @@ def __init__( server: viser.ViserServer, name: str, color: tuple[float, float, float], + layout: HandViewLayout, ): + self.layout = layout self.color = np.array(color, dtype=np.float32) - zero_pts = np.zeros((26, 3), dtype=np.float32) - zero_segs = np.zeros((len(HAND_BONES), 2, 3), dtype=np.float32) + zero_pts = np.zeros((len(layout.joint_names), 3), dtype=np.float32) + zero_segs = np.zeros((0, 2, 3), dtype=np.float32) self.points = server.scene.add_point_cloud( name=f"/{name}/joints", points=zero_pts, - colors=np.tile(self.color, (26, 1)), + colors=np.tile(self.color, (len(layout.joint_names), 1)), point_size=0.008, ) self.bones = server.scene.add_line_segments( name=f"/{name}/bones", points=zero_segs, - colors=np.tile(self.color, (len(HAND_BONES), 2, 1)), + colors=np.zeros((0, 2, 3), dtype=np.float32), line_width=2.0, ) - def update(self, positions: np.ndarray, valid: bool) -> None: - if valid: - self.points.points = positions.astype(np.float32) - self.points.colors = np.tile(self.color, (positions.shape[0], 1)) - self.bones.points = _bone_segments(positions) - else: - zero_pts = np.zeros_like(positions, dtype=np.float32) - self.points.points = zero_pts - self.points.colors = np.tile(INVALID_COLOR, (positions.shape[0], 1)) - self.bones.points = np.zeros((len(HAND_BONES), 2, 3), dtype=np.float32) + def update(self, positions: np.ndarray, valid: np.ndarray) -> None: + positions = positions.astype(np.float32) + valid_bool = valid.astype(bool) + self.points.points = positions + colors = np.tile(self.color, (positions.shape[0], 1)) + colors[~valid_bool] = INVALID_COLOR + self.points.colors = colors + segments = _valid_bone_segments(positions, valid_bool, self.layout.bones) + self.bones.points = segments + self.bones.colors = np.tile(self.color, (segments.shape[0], 2, 1)) class ControllerViz: @@ -464,11 +423,13 @@ def __init__( server: viser.ViserServer, ground: GroundGrid | None = None, body_layout: BodyViewLayout = FULL_BODY_POSE_LAYOUT, + hand_layout: HandViewLayout = OPENXR_HAND_LAYOUT, ): self._ground = ground self._body_layout = body_layout - self.hand_left = HandViz(server, "hand_left", LEFT_COLOR) - self.hand_right = HandViz(server, "hand_right", RIGHT_COLOR) + self._hand_layout = hand_layout + self.hand_left = HandViz(server, "hand_left", LEFT_COLOR, hand_layout) + self.hand_right = HandViz(server, "hand_right", RIGHT_COLOR, hand_layout) self.head = HeadViz(server) self.controller_left = ControllerViz(server, "controller_left", LEFT_COLOR) self.controller_right = ControllerViz(server, "controller_right", RIGHT_COLOR) @@ -480,10 +441,17 @@ def _update_hand_if_active(self, viz: HandViz, hand) -> bool: viz.bones.visible = False return False - positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS], dtype=np.float32) + positions = np.asarray( + hand[self._hand_layout.positions_index], dtype=np.float32 + ) + valid = np.asarray(hand[self._hand_layout.valid_index], dtype=np.uint8) + if not np.any(valid): + viz.points.visible = False + viz.bones.visible = False + return False viz.points.visible = True viz.bones.visible = True - viz.update(positions, valid=True) + viz.update(positions, valid) return True def _update_controller_if_active(self, viz: ControllerViz, controller) -> bool: diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py new file mode 100644 index 0000000000..0e82e33853 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py @@ -0,0 +1,68 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Hand-schema selection contracts for the DeviceIO live viewer.""" + +from dataclasses import dataclass +from enum import Enum +from pathlib import Path + +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + SomaHandRepresentation, +) +from isaaccapture.retargeting_engine.interface.retargeter_core_types import ( + OutputSelector, +) + + +class HandSchema(str, Enum): + OPENXR_HAND_POSE = "openxr-hand-pose" + SOMA = "soma" + + +@dataclass(frozen=True) +class HandViewLayout: + joint_names: tuple[str, ...] + bones: tuple[tuple[int, int], ...] + positions_index: int + valid_index: int + + +@dataclass(frozen=True) +class HandViewPipeline: + left: OutputSelector + right: OutputSelector + layout: HandViewLayout + + +def create_hand_view_pipeline( + soma_data_root: Path | None = None, + *, + hand_schema: HandSchema | str = HandSchema.OPENXR_HAND_POSE, + soma_left_collection_id: str = "soma_hand_left_demo", + soma_right_collection_id: str = "soma_hand_right_demo", + soma_hand_representation: SomaHandRepresentation + | str = SomaHandRepresentation.JOINT_ROTATIONS, +) -> HandViewPipeline: + selected = HandSchema(hand_schema) + if selected is HandSchema.OPENXR_HAND_POSE: + from .openxr_hand_pose import create_openxr_hand_pipeline + + return create_openxr_hand_pipeline() + + representation = SomaHandRepresentation(soma_hand_representation) + if ( + representation is SomaHandRepresentation.JOINT_ROTATIONS + and soma_data_root is None + ): + raise ValueError( + "--hand-schema soma with joint-rotations requires --soma-data-root" + ) + from .soma_hand import create_soma_hand_pipeline + + return create_soma_hand_pipeline( + soma_data_root, + soma_left_collection_id, + soma_right_collection_id, + representation, + ) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py index bc0f7d3b2d..c4b211e9aa 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py @@ -29,11 +29,15 @@ TeleopSession, TeleopSessionConfig, ) -from isaaccapture.retargeting_engine.deviceio_source_nodes import SomaBodyRepresentation +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + SomaBodyRepresentation, + SomaHandRepresentation, +) from .body_pipeline import BodySchema, create_body_view_pipeline, resolve_body_schema from .deviceio_pipeline import build_all_human_pipeline from .deviceio_viser import HumanDeviceIOViz, setup_scene +from .hand_pipeline import HandSchema, create_hand_view_pipeline def main(argv: list[str]) -> int: @@ -61,6 +65,28 @@ def main(argv: list[str]) -> int: default=SomaBodyRepresentation.JOINT_ROTATIONS.value, help="SOMA FlatBuffer profile consumed by the viewer", ) + parser.add_argument( + "--hand-schema", + choices=[schema.value for schema in HandSchema], + default=HandSchema.OPENXR_HAND_POSE.value, + help="Input hand schema (default: openxr-hand-pose)", + ) + parser.add_argument( + "--soma-left-hand-collection-id", + default="soma_hand_left_demo", + help="Left SOMA hand publisher's tensor collection ID", + ) + parser.add_argument( + "--soma-right-hand-collection-id", + default="soma_hand_right_demo", + help="Right SOMA hand publisher's tensor collection ID", + ) + parser.add_argument( + "--soma-hand-representation", + choices=[representation.value for representation in SomaHandRepresentation], + default=SomaHandRepresentation.JOINT_ROTATIONS.value, + help="SOMA hand FlatBuffer profile consumed by the viewer", + ) CloudXRLauncher.add_launcher_arguments(parser) args = parser.parse_args(argv[1:]) @@ -72,7 +98,14 @@ def main(argv: list[str]) -> int: soma_collection_id=args.soma_collection_id, soma_body_representation=args.soma_body_representation, ) - pipeline = build_all_human_pipeline(body=body) + hands = create_hand_view_pipeline( + args.soma_data_root, + hand_schema=args.hand_schema, + soma_left_collection_id=args.soma_left_hand_collection_id, + soma_right_collection_id=args.soma_right_hand_collection_id, + soma_hand_representation=args.soma_hand_representation, + ) + pipeline = build_all_human_pipeline(body=body, hands=hands) except ValueError as error: parser.error(str(error)) @@ -89,12 +122,13 @@ def main(argv: list[str]) -> int: print("[live] waiting for headset connection… (Ctrl+C to stop)") with TeleopSession(config) as session: - viz = HumanDeviceIOViz(server, ground, body.layout) + viz = HumanDeviceIOViz(server, ground, body.layout, hands.layout) print( f"[live] viser listening on {args.host}:{args.port} " f"(http://localhost:{args.port})" ) print(f"[live] body schema: {body_schema.value}") + print(f"[live] hand schema: {args.hand_schema}") try: while True: result = session.step() diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/openxr_hand_pose.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/openxr_hand_pose.py new file mode 100644 index 0000000000..4cde85f3c3 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/openxr_hand_pose.py @@ -0,0 +1,60 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""OpenXR HandPose source and layout for the DeviceIO live viewer.""" + +from isaaccapture.retargeting_engine.deviceio_source_nodes import HandsSource +from isaaccapture.retargeting_engine.tensor_types import HandInputIndex +from isaaccapture.schema import HandJoint + +from .hand_pipeline import HandViewLayout, HandViewPipeline + + +OPENXR_HAND_BONES: tuple[tuple[int, int], ...] = ( + (1, 2), + (2, 3), + (3, 4), + (4, 5), + (1, 6), + (6, 7), + (7, 8), + (8, 9), + (9, 10), + (1, 11), + (11, 12), + (12, 13), + (13, 14), + (14, 15), + (1, 16), + (16, 17), + (17, 18), + (18, 19), + (19, 20), + (1, 21), + (21, 22), + (22, 23), + (23, 24), + (24, 25), +) + +OPENXR_HAND_LAYOUT = HandViewLayout( + joint_names=tuple( + name + for name, joint in sorted( + HandJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ), + bones=OPENXR_HAND_BONES, + positions_index=int(HandInputIndex.JOINT_POSITIONS), + valid_index=int(HandInputIndex.JOINT_VALID), +) + + +def create_openxr_hand_pipeline() -> HandViewPipeline: + source = HandsSource(name="hands") + return HandViewPipeline( + left=source.output(HandsSource.LEFT), + right=source.output(HandsSource.RIGHT), + layout=OPENXR_HAND_LAYOUT, + ) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py new file mode 100644 index 0000000000..dcc2e21526 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py @@ -0,0 +1,118 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""SOMA hand sources and layout for the DeviceIO live viewer.""" + +from pathlib import Path + +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + SomaHandRepresentation, + SomaHandSource, +) +from isaaccapture.retargeting_engine.tensor_types import SomaHandInputIndex +from isaaccapture.schema import SomaHandednessV0, SomaHandJointV0 + +from .hand_pipeline import HandViewLayout, HandViewPipeline + + +SOMA_HAND_BONES: tuple[tuple[int, int], ...] = ( + (0, 1), + (1, 2), + (2, 3), + (3, 4), + (0, 5), + (5, 6), + (6, 7), + (7, 8), + (8, 9), + (0, 10), + (10, 11), + (11, 12), + (12, 13), + (13, 14), + (0, 15), + (15, 16), + (16, 17), + (17, 18), + (18, 19), + (0, 20), + (20, 21), + (21, 22), + (22, 23), + (23, 24), +) + +SOMA_HAND_LAYOUT = HandViewLayout( + joint_names=tuple( + name + for name, joint in sorted( + SomaHandJointV0.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ), + bones=SOMA_HAND_BONES, + positions_index=int(SomaHandInputIndex.JOINT_POSITIONS), + valid_index=int(SomaHandInputIndex.JOINT_VALID), +) + + +def create_layer(data_root: Path, hand_type: str): + import soma + import torch + + if soma.__version__ != "0.3.1": + raise RuntimeError(f"The POC requires SOMA-X 0.3.1, got {soma.__version__}") + layer = soma.SOMAHandLayer( + data_root=str(data_root), + hand_type=hand_type, + device="cpu", + identity_model_type="soma", + lod="low", + output_unit=soma.Unit.METERS, + correctives_model_path=None, + ) + expected = [name.replace("_", "") for name in SOMA_HAND_LAYOUT.joint_names] + upstream_prefix = f"{hand_type.capitalize()}Hand" + actual = [ + name.removeprefix(upstream_prefix).upper() + for name in layer.rig_data["joint_names"] + ] + actual[0] = "WRIST" + if actual != [name.upper() for name in expected]: + raise ValueError("SOMA hand layer joint order differs from the V0 FBS") + with torch.no_grad(): + layer.prepare_identity( + torch.zeros((1, layer.identity_model.num_identity_coeffs)) + ) + return layer + + +def create_soma_hand_pipeline( + data_root: Path | None, + left_collection_id: str, + right_collection_id: str, + representation: SomaHandRepresentation, +) -> HandViewPipeline: + layers = (None, None) + if representation is SomaHandRepresentation.JOINT_ROTATIONS: + assert data_root is not None + layers = (create_layer(data_root, "left"), create_layer(data_root, "right")) + left = SomaHandSource( + "hand_left", + left_collection_id, + SomaHandednessV0.LEFT, + layers[0], + representation=representation, + ) + right = SomaHandSource( + "hand_right", + right_collection_id, + SomaHandednessV0.RIGHT, + layers[1], + representation=representation, + ) + return HandViewPipeline( + left=left.output(SomaHandSource.HAND), + right=right.output(SomaHandSource.HAND), + layout=SOMA_HAND_LAYOUT, + ) diff --git a/examples/soma_hand_publisher/CMakeLists.txt b/examples/soma_hand_publisher/CMakeLists.txt new file mode 100644 index 0000000000..1417a90271 --- /dev/null +++ b/examples/soma_hand_publisher/CMakeLists.txt @@ -0,0 +1,9 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +add_subdirectory(cpp/soma_hand_pusher) + +include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) +install_python_example(DESTINATION examples/soma_hand_publisher) diff --git a/examples/soma_hand_publisher/README.md b/examples/soma_hand_publisher/README.md new file mode 100644 index 0000000000..33fcd9cf4c --- /dev/null +++ b/examples/soma_hand_publisher/README.md @@ -0,0 +1,56 @@ + + +# SOMA Hand Publisher + +Publishes the left and right hand subsets of the motion bundled with SOMA-X. +This is a deterministic input generator for testing SOMA hand consumers. It is +not a vendor plugin or vendor network protocol. + +The Python process expresses both hands through the pinned SOMA-X v0.3.1 +`SOMAHandLayer`, then writes paired FlatBuffers to `soma_hand_pusher`. The +native process validates each payload and publishes independent +`soma_hand_left_demo` and `soma_hand_right_demo` collections on one OpenXR +session. + +```text +SOMA-X example_animation.npy + -> left/right SOMAHandLayer controls and evaluated poses + -> soma_hand_pusher + -> left/right SchemaPusher collections +``` + +Install the example and build the native pusher: + +```bash +uv pip install -e ./examples/soma_hand_publisher +cmake --build ./build --target soma_hand_pusher +``` + +Start the DeviceIO viewer with SOMA hands, connect its client, then run the +publisher in another terminal: + +```bash +uv run --no-sync python -m isaaccapture_examples.deviceio_live_view \ + --accept-eula \ + --hand-schema soma \ + --soma-data-root /path/to/SOMA-X-v0.3.1-assets + +source ~/.cloudxr/run/cloudxr.env +uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ + --data-root /path/to/SOMA-X-v0.3.1-assets \ + --pusher ./build/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher \ + --loop +``` + +Joint rotations are the default. To publish evaluated joint poses, add +`--hand-representation joint-poses` to the publisher and +`--soma-hand-representation joint-poses` to the viewer. The two modes publish +`soma_hand_joint_rotations_v0` and `soma_hand_joint_poses_v0`, respectively. + +The assets must include `SOMAHand.npz`, `example_animation.npy`, and the neutral +SOMA identity assets from the v0.3.1 asset release. Add `--validate-only` to +process every paired frame through the native FlatBuffer verifier without +starting OpenXR. diff --git a/examples/soma_hand_publisher/cpp/soma_hand_pusher/CMakeLists.txt b/examples/soma_hand_publisher/cpp/soma_hand_pusher/CMakeLists.txt new file mode 100644 index 0000000000..cfead1828b --- /dev/null +++ b/examples/soma_hand_publisher/cpp/soma_hand_pusher/CMakeLists.txt @@ -0,0 +1,15 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +find_package(Threads REQUIRED) + +add_executable(soma_hand_pusher soma_hand_pusher.cpp) +target_link_libraries(soma_hand_pusher PRIVATE + pusherio::pusherio + oxr::oxr_core + isaacteleop_schema + Threads::Threads +) +install(TARGETS soma_hand_pusher + RUNTIME DESTINATION examples/soma_hand_publisher/cpp +) diff --git a/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp b/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp new file mode 100644 index 0000000000..f4194d5d1c --- /dev/null +++ b/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp @@ -0,0 +1,182 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace +{ + +enum class HandRepresentation +{ + JointRotations, + JointPoses, +}; + +struct Arguments +{ + bool validate_only = false; + HandRepresentation representation = HandRepresentation::JointRotations; +}; + +Arguments parse_arguments(int argc, char** argv) +{ + Arguments arguments; + for (int index = 1; index < argc; ++index) + { + const std::string argument = argv[index]; + if (argument == "--validate-only") + { + arguments.validate_only = true; + } + else if (argument == "--hand-representation" && index + 1 < argc) + { + const std::string value = argv[++index]; + if (value == "joint-rotations") + { + arguments.representation = HandRepresentation::JointRotations; + } + else if (value == "joint-poses") + { + arguments.representation = HandRepresentation::JointPoses; + } + else + { + throw std::invalid_argument("Unknown SOMA hand representation: " + value); + } + } + else + { + throw std::invalid_argument("Unknown or incomplete argument: " + argument); + } + } + return arguments; +} + +uint64_t little_endian(const uint8_t* bytes, size_t size) +{ + uint64_t value = 0; + for (size_t index = 0; index < size; ++index) + { + value |= static_cast(bytes[index]) << (8 * index); + } + return value; +} + +template +const Table* verify(const std::vector& bytes) +{ + flatbuffers::Verifier verifier(bytes.data(), bytes.size()); + if (!verifier.VerifyBuffer(nullptr)) + { + throw std::invalid_argument("Invalid SOMA hand FlatBuffer"); + } + return flatbuffers::GetRoot
(bytes.data()); +} + +} // namespace + +int main(int argc, char** argv) +{ + if (argc == 2 && std::string(argv[1]) == "--help") + { + std::cout << "Usage: soma_hand_pusher [--hand-representation joint-rotations|joint-poses] " + "[--validate-only]\n" + "Reads paired, framed SOMA hand FlatBuffers from stdin.\n"; + return 0; + } + auto logger = isaaccapture::Logger::get("isaaccapture.examples.soma_hand_pusher"); + try + { + const auto arguments = parse_arguments(argc, argv); + const bool joint_poses = arguments.representation == HandRepresentation::JointPoses; + const size_t max_flatbuffer_size = joint_poses ? 2048 : 1024; + const char* tensor_identifier = joint_poses ? "soma_hand_joint_poses_v0" : "soma_hand_joint_rotations_v0"; + std::unique_ptr session; + std::unique_ptr left_pusher; + std::unique_ptr right_pusher; + size_t frames = 0; + std::array header{}; + while (std::cin.read(reinterpret_cast(header.data()), header.size())) + { + const auto left_size = little_endian(header.data(), 4); + const auto right_size = little_endian(header.data() + 4, 4); + const auto raw_time = little_endian(header.data() + 8, 8); + if (left_size == 0 || left_size > max_flatbuffer_size || right_size == 0 || + right_size > max_flatbuffer_size || raw_time > INT64_MAX) + { + throw std::invalid_argument("Invalid SOMA hand demo packet header"); + } + std::vector left_bytes(left_size); + std::vector right_bytes(right_size); + if (!std::cin.read(reinterpret_cast(left_bytes.data()), left_bytes.size()) || + !std::cin.read(reinterpret_cast(right_bytes.data()), right_bytes.size())) + { + throw std::invalid_argument("Truncated SOMA hand demo payload"); + } + + core::SomaHandednessV0 left_handedness; + core::SomaHandednessV0 right_handedness; + if (joint_poses) + { + left_handedness = verify(left_bytes)->handedness(); + right_handedness = verify(right_bytes)->handedness(); + } + else + { + left_handedness = verify(left_bytes)->handedness(); + right_handedness = verify(right_bytes)->handedness(); + } + if (left_handedness != core::SomaHandednessV0_LEFT || right_handedness != core::SomaHandednessV0_RIGHT) + { + throw std::invalid_argument("SOMA hand demo payload handedness does not match its collection"); + } + + if (!arguments.validate_only) + { + if (!left_pusher) + { + session = std::make_unique( + "SomaHandDemoPublisher", core::SchemaPusher::get_required_extensions(), false); + left_pusher = std::make_unique( + session->get_handles(), core::SchemaPusherConfig{ .collection_id = "soma_hand_left_demo", + .max_flatbuffer_size = max_flatbuffer_size, + .tensor_identifier = tensor_identifier, + .localized_name = "SOMA Left Hand Demo" }); + right_pusher = std::make_unique( + session->get_handles(), core::SchemaPusherConfig{ .collection_id = "soma_hand_right_demo", + .max_flatbuffer_size = max_flatbuffer_size, + .tensor_identifier = tensor_identifier, + .localized_name = "SOMA Right Hand Demo" }); + } + const auto now = core::os_monotonic_now_ns(); + left_pusher->push_buffer(left_bytes.data(), left_bytes.size(), now, static_cast(raw_time)); + right_pusher->push_buffer(right_bytes.data(), right_bytes.size(), now, static_cast(raw_time)); + } + ++frames; + } + if (std::cin.gcount() != 0) + { + throw std::invalid_argument("Truncated SOMA hand demo packet header"); + } + logger->info("{} {} paired SOMA hand demo frames", arguments.validate_only ? "Validated" : "Published", frames); + return 0; + } + catch (const std::exception& error) + { + logger->error("{}", error.what()); + return 1; + } +} diff --git a/examples/soma_hand_publisher/pyproject.toml b/examples/soma_hand_publisher/pyproject.toml new file mode 100644 index 0000000000..bcd25a8e39 --- /dev/null +++ b/examples/soma_hand_publisher/pyproject.toml @@ -0,0 +1,24 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[project] +name = "isaaccapture-examples-soma-hand-publisher" +version = "0.0.0" +description = "Isaac Teleop SOMA hand demo publisher" +requires-python = ">=3.11,<3.14" +dependencies = [ + "isaaccapture", + "numpy>=1.23.0", + "py-soma-x==0.3.1", +] + +[tool.hatch.build.targets.wheel] +only-include = ["python/isaaccapture_examples/soma_hand_publisher"] +sources = ["python"] + +[tool.uv.sources] +isaaccapture = { path = "../..", editable = true } diff --git a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/__init__.py b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/__init__.py new file mode 100644 index 0000000000..52a7a9daf0 --- /dev/null +++ b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/__init__.py @@ -0,0 +1,2 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 diff --git a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/__main__.py b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/__main__.py new file mode 100644 index 0000000000..72d96631d8 --- /dev/null +++ b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/__main__.py @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys + +from .publisher import main + +sys.exit(main(sys.argv)) diff --git a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py new file mode 100644 index 0000000000..27e22008b9 --- /dev/null +++ b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py @@ -0,0 +1,241 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Publish both hands from the bundled SOMA motion through SOMA hand transport.""" + +import argparse +import logging +import math +import signal +import struct +import subprocess +import sys +import time +from pathlib import Path + +import numpy as np + +from isaaccapture.schema import ( + Point, + SomaHandednessV0, + SomaHandJointPoseArrayV0, + SomaHandJointPosesV0, + SomaHandJointRotationArrayV0, + SomaHandJointRotationsV0, +) + +logger = logging.getLogger("isaaccapture.examples.soma_hand_publisher") + + +def create_layers(data_root: Path): + """Create the neutral full-body and hand layers used by the bundled motion.""" + import soma + import torch + + if soma.__version__ != "0.3.1": + raise RuntimeError(f"The POC requires SOMA-X 0.3.1, got {soma.__version__}") + body = soma.SOMALayer( + data_root=str(data_root), + identity_model_type="soma", + device="cpu", + lod="low", + output_unit=soma.Unit.METERS, + enable_procedural_transforms=False, + correctives_model_path=None, + ) + hands = { + side: soma.SOMAHandLayer( + data_root=str(data_root), + hand_type=side, + device="cpu", + identity_model_type="soma", + lod="low", + output_unit=soma.Unit.METERS, + correctives_model_path=None, + ) + for side in ("left", "right") + } + with torch.no_grad(): + body.prepare_identity(torch.zeros((1, body.identity_model.num_identity_coeffs))) + for hand in hands.values(): + hand.prepare_identity( + torch.zeros((1, hand.identity_model.num_identity_coeffs)) + ) + return body, hands + + +def demo_hand_frames(path: Path, body, hands): + """Extract both hand subsets and express them through SOMAHandLayer.""" + import torch + from soma.geometry.rig_utils import ( + joint_local_to_world, + joint_world_to_local, + precompute_joint_orient, + remove_joint_orient_local, + ) + from soma.geometry.transforms import matrix_to_quaternion_xyzw + + motion = np.load(path, allow_pickle=False) + if ( + motion.ndim != 4 + or motion.shape[1] not in (78, 94) + or motion.shape[2:] != (4, 4) + or not len(motion) + ): + raise ValueError("Expected bundled SOMA motion shaped (T, 78 or 94, 4, 4)") + local = torch.as_tensor(motion[..., :3, :3], dtype=torch.float32) + if local.shape[1] == 94: + twists = (*range(40, 44), *range(72, 76), *range(81, 85), *range(90, 94)) + local = local[:, [index for index in range(94) if index not in twists]] + orient = body.t_pose_world[body.public_transform_joint_indices, :3, :3] + world = joint_local_to_world(local, body.public_joint_parent_ids) + relative = joint_world_to_local( + world @ orient.transpose(-2, -1), body.public_joint_parent_ids + )[:, 1:] + with torch.no_grad(): + body_output = body.pose( + relative, + transl=torch.as_tensor(motion[:, 1, :3, 3], dtype=torch.float32), + pose2rot=False, + fk_only=True, + apply_correctives=False, + ) + + frames = {} + for side, start in (("left", 14), ("right", 42)): + hand = hands[side] + target = body_output["transforms"][:, start + 1 : start + 26] + absolute_local = joint_world_to_local( + target[..., :3, :3], hand.joint_parent_ids + ) + hand_orient, hand_parent_orient_t = precompute_joint_orient( + hand.t_pose_world, hand.joint_parent_ids + ) + controls = remove_joint_orient_local( + absolute_local, hand_orient, hand_parent_orient_t + ) + translation = target[:, 0, :3, 3] + with torch.no_grad(): + evaluated = hand.pose( + controls, + global_translation=translation, + pose2rot=False, + fk_only=True, + apply_correctives=False, + ) + frames[side] = { + "rotations": matrix_to_quaternion_xyzw(controls).numpy(), + "translation": translation.numpy(), + "positions": evaluated["joints"].numpy(), + "orientations": matrix_to_quaternion_xyzw( + evaluated["transforms"][..., :3, :3] + ).numpy(), + } + return frames + + +def soma_joint_rotations(rotations, translation, handedness): + joints = SomaHandJointRotationArrayV0() + joints.rotations[:] = rotations + joints.is_valid[:] = 1 + return SomaHandJointRotationsV0(joints, Point(*translation), True, handedness) + + +def soma_joint_poses(positions, orientations, handedness): + joints = SomaHandJointPoseArrayV0() + joints.positions[:] = positions + joints.orientations[:] = orientations + joints.is_valid[:] = 1 + return SomaHandJointPosesV0(joints, handedness) + + +def _payload(frame, index: int, representation: str, handedness): + if representation == "joint-poses": + return soma_joint_poses( + frame["positions"][index], frame["orientations"][index], handedness + ).to_bytes() + return soma_joint_rotations( + frame["rotations"][index], frame["translation"][index], handedness + ).to_bytes() + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--data-root", required=True, type=Path) + parser.add_argument( + "--pusher", required=True, type=Path, help="Built soma_hand_pusher executable" + ) + parser.add_argument("--rate", type=float, default=30.0) + parser.add_argument("--loop", action="store_true") + parser.add_argument( + "--hand-representation", + choices=("joint-rotations", "joint-poses"), + default="joint-rotations", + help="SOMA FlatBuffer profile to publish (default: joint-rotations)", + ) + parser.add_argument( + "--validate-only", + action="store_true", + help="Validate FBS packets without OpenXR", + ) + args = parser.parse_args(argv[1:]) + if not math.isfinite(args.rate) or args.rate <= 0: + parser.error("rate must be finite and positive") + + body, hand_layers = create_layers(args.data_root) + frames = demo_hand_frames( + args.data_root / "example_animation.npy", body, hand_layers + ) + command = [ + str(args.pusher.resolve()), + "--hand-representation", + args.hand_representation, + ] + (["--validate-only"] if args.validate_only else []) + with subprocess.Popen(command, stdin=subprocess.PIPE) as process: + interrupted = False + try: + start = time.monotonic() + frame_number = 0 + while True: + for index in range(len(frames["left"]["positions"])): + remaining = start + frame_number / args.rate - time.monotonic() + if remaining > 0 and not args.validate_only: + time.sleep(remaining) + left = _payload( + frames["left"], + index, + args.hand_representation, + SomaHandednessV0.LEFT, + ) + right = _payload( + frames["right"], + index, + args.hand_representation, + SomaHandednessV0.RIGHT, + ) + process.stdin.write( + struct.pack( + "6ufFhL7^>1(?F4cAVowv6tUnS)x-pV3~78_1n%v0u+o2f7irRwrBmyZ{M!EK3_-#ze26@7Q$31H zrtR=Cps&bXm+u?oCZj^$;GW^TgPbSCmk3R(b}IzX%d#2&&x;fa>Y69ZV(7=io~K+F!r zb3_>!9!LPOI0M565e9}mKy`nF7#KE)O>C2^-}3DL{~DkQ7NANHI0EF)5QFeRrmO+# zx&aj10n`an1Hw~)rlkPs0-!nvpxz%q`4FfakYeC?{{MdoP#?@p2_QcKs0YSpU=RX& PfQ5mBfnoD+Mpj7x9*-=I diff --git a/src/core/schema/golden/soma_common_v0.bfbs b/src/core/schema/golden/soma_common_v0.bfbs index 96cd221eb69e28fba6a72b0a56eddc6586dfdf21..a9326fca3b2a8ab9209e8337005dd2aa6cc22ad8 100644 GIT binary patch delta 414 zcmZusyAA3k}fUwv^M7(;jnJz@lYUA3j{xW)5CqORTo8O`fiQ4+NjV3{H?=2x-ofp0 zlhct9DP#r`QxTR(;BEjFAR~k?im0|Ai-OQX3%LsALvtboT!9(Dq0_*0wf4$`)nd|~ zEM3oCdYjFd#-?h^mWFD~p_y?3#z{)4eSIM!Ao@X_$DlY&4 delta 104 zcmZqRIKw(Yfze>1qHwqd18%?sl*(abU@!sFAhrw;vjFjxzyJT+K-oZPh7=(20w@*( Oq(SmPuz5eDBNG6BFb=Z- diff --git a/src/core/schema/golden/soma_hand_joint_poses_v0.bfbs b/src/core/schema/golden/soma_hand_joint_poses_v0.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..36ddf85e290a045341fd7783ae5d490273a53ac5 GIT binary patch literal 4632 zcmaKvKX6n<9LM*PBSeTM;Hg1T&+&>YG93N)gAXX#9Nly?wVi4tM5z``+$%fBXBp z`z~pWIey~!1f5Bkw27Ne(`9;i?lnnsklvUq$bd9B2=4ASFpN15V&FxsjfqXL*kO=q z%ql+*Sh)U*F>Bq(=`^PAC1lZ0BQwtYtv$wUFt+-#F$|eo;5w*-b6^nkf;d?Jb$fe> zHF{S2h4=Nn!FxOJi|nDT%abY3JicO~cFp4xK67Sd?4!>M z!(&;VHxDydVKlC zwOlLyVe*P`=bu+Ya^2%9#^WPnCqr}1h9`f&6HdS4tx?#u26Ii~S;O9a9{*7bKIWRN z$CpoC>tx*7>*Qmuse63VHEM$`_?TWTAFd^pN2k%%^Ks*UgfS@-s5U) zWGp*883uUOldssjcHLW}e6wo^@TSMtzOLPA#V5e2e;fXh9M_(-;uGM!$5-54>t)>8 z8|g!U?=tS}<*2bO>v3goK6m!?@zCDeo_vm>s9o{aNWNWz{u>^jBPwbut@!9ay4}!6 z`l>x_#YOixT-B`f-tsuI`?K+piM-j*S*KepIL~zIs0LMMNucp-1Yw%m;xX?0qh7YH zfN9VNbe8NmGs~sf^8CC#G`s9B%3j@cX4~g?TTPsk}=h= zxUN0$*2>P&;S*ZQs;=My3HMAfCSUYed1A;fosm4rRK1)Xn>aK4;mC=R;jDQ;&A8ZW z{ysm_`8-I2cfk&u?LBd(Hebf>@iJ2-^W&EFT`XogqPsW~jM?Kwa+O2%%1n8_UYxne zbDDikEFSSoAzyhTzU(X&>qRd>cR(FTeVG-o%oQta|h4EB=tA^xPedDT^4t|ciyp}Ba{5|mE zoN3ftS8yK0BtP(Z5At_mhis77jZqD%=C!Y1gP~tygN&zuY;k^%cqbO1GHKuAaGqqVKMc;;QeTG_PNu-1PTwEwts>KUVjaDMe zK*^s|OyUvq0lmQPD{(MsMh&V?0ZSY*h`pzQ6z5OT~J5ZWexEc`%oP zvjj>&IimWpKKLY}o2TS%J9bJh$4{vNPySEW=w8NXM?QR}2(yz8a#lKyq?G>y!^+%%SkV xi#>fiR+@B>9YId$JFvs<U+`EfEbDuE%JqQ{12bYKC%D+ literal 0 HcmV?d00001 diff --git a/src/core/schema/golden/soma_hand_v0.bfbs b/src/core/schema/golden/soma_hand_v0.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..afd122d4cb3c79f6b3006e164d91c3848e67dafb GIT binary patch literal 3728 zcmaJ^J#Z686n?UWEkKC{A&E&GMwzIf&>#aUTu=}qgG6k_u|?v{c;<4pl%PbGh$IYT zC@CmhP*Av_prD{gk%H0%1qB5~3KtX66O1L@10hP!p?l&)9&})zQ22W zPKwBdix(C!$uUVwRALgB1fItwDHE7Q^1u|329m(lQF2A*fC%uWF@(*Bz|@Ec#6%8> zbv38SO zJd;b?_MvwjYrB|L{sDf!!uk4p?axl<9=J&$2E4?d@_wg-M?;|=;sf9ujEn&sjfkhw%>_YD*a2F^6Nr3TPX*4UR78&maSCd$l`>=NcGhC44T)@$bYm~z~8_z0WK z6^fsK<4qTHc;5c2(@A1Z0Ph1jHXRwH)5>IF>+3H6Jtbq_8N@$}+&lbt9sUH)RUbnA zXGGc#pK;MKGl+j$W8~hcKBmT#(Zu7AMVN{tgF9o2mGrJm-R4P$mM3Ht;f$C z{djf4@xYPcyJ}^!vghzwb2?rO7-g<@`cF-Y(eM?Z6~W8INqy%`xKc-!IgTpgQ(_(<@vPS;=R(Xlg#j|Ar& zKJ%_)0_Uo~p*=|O9jvRrXfgBI=tmn0$8AT3`i%_ir@yKnEy|3? zgSfCgj;-G4m~9>m*@fh^?M_U>~`PYtyD3cP?}cA>R5;@f^Jflp)$idAnv%qU=&LwTnjbyQR5@5{v&B2*AWHK#$PEj| zyXBVE#n^rf{C_S!Z8rT|_WiMTM5y29BLV#f;0JwRlthk<9XG#tE>{u7>lK_8#3eF? zwG_Y{YEI}+CKFbnF^;}I0crsGZ=Hi&1_x_4ZI8%-_Yer<#qY?T{=ih8fIjAH z#N+{VXB@&VM+E!Brj|6(m, "SomaHandednessV0") + .value("UNSPECIFIED", SomaHandednessV0_UNSPECIFIED) + .value("LEFT", SomaHandednessV0_LEFT) + .value("RIGHT", SomaHandednessV0_RIGHT); + py::class_(m, "SomaJointRotationV0") .def(py::init<>()) .def(py::init(), py::arg("rotation"), py::arg("is_valid") = false) diff --git a/src/core/schema/python/soma_hand_joint_poses_v0_bindings.h b/src/core/schema/python/soma_hand_joint_poses_v0_bindings.h new file mode 100644 index 0000000000..d0ab4d540f --- /dev/null +++ b/src/core/schema/python/soma_hand_joint_poses_v0_bindings.h @@ -0,0 +1,100 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline const SomaHandJointPoseV0& first_soma_hand_joint_pose_v0(const py::object& self) +{ + return *(*self.cast().values())[0]; +} + +constexpr py::ssize_t SOMA_HAND_JOINT_POSE_STRIDE = static_cast(sizeof(SomaHandJointPoseV0)); +constexpr py::ssize_t SOMA_HAND_JOINT_POSE_COUNT = static_cast(SomaHandJointV0_NUM_JOINTS); + +static_assert(sizeof(SomaHandJointPoseArrayV0) == + sizeof(SomaHandJointPoseV0) * static_cast(SomaHandJointV0_NUM_JOINTS), + "SomaHandJointPoseArrayV0.values length must equal SomaHandJointV0::NUM_JOINTS"); + +inline void bind_soma_hand_joint_poses_v0(py::module& m) +{ + py::class_(m, "SomaHandJointPoseV0") + .def(py::init<>()) + .def(py::init(), py::arg("pose"), py::arg("is_valid") = false) + .def_property_readonly("pose", &SomaHandJointPoseV0::pose, py::return_value_policy::reference_internal) + .def_property_readonly("is_valid", &SomaHandJointPoseV0::is_valid); + + py::class_(m, "SomaHandJointPoseArrayV0") + .def(py::init<>()) + .def( + "values", + [](const SomaHandJointPoseArrayV0& self, size_t index) -> const SomaHandJointPoseV0* + { + if (index >= static_cast(SomaHandJointV0_NUM_JOINTS)) + { + throw py::index_error("SomaHandJointPoseArrayV0 index out of range"); + } + return (*self.values())[index]; + }, + py::arg("index"), py::return_value_policy::reference_internal) + .def_property_readonly( + "positions", + [](py::object self) + { + const auto* first = + reinterpret_cast(&first_soma_hand_joint_pose_v0(self).pose().position()); + return strided_field_view(self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 3); + }, + "Joint positions as a writable (25, 3) float32 view in SomaHandJointV0 order.") + .def_property_readonly( + "orientations", + [](py::object self) + { + const auto* first = + reinterpret_cast(&first_soma_hand_joint_pose_v0(self).pose().orientation()); + return strided_field_view(self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 4); + }, + "Unit XYZW quaternions as a writable (25, 4) float32 view.") + .def_property_readonly( + "is_valid", + [offset = FBS_FIELD_OFFSET(SomaHandJointPoseV0, is_valid)](py::object self) + { + const auto* first = fbs_field_address(first_soma_hand_joint_pose_v0(self), offset); + return strided_field_view( + self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 0); + }, + "Per-joint validity as a writable (25,) uint8 view."); + + serialized_class(m, "SomaHandJointPosesV0", "Encoded evaluated SOMA hand poses v0.") + .def(py::init( + [](const SomaHandJointPoseArrayV0& joint_poses, SomaHandednessV0 handedness) + { + SomaHandJointPosesV0T native; + native.joint_poses = std::make_shared(joint_poses); + native.handedness = handedness; + return pack(native); + }), + py::arg("joint_poses") = SomaHandJointPoseArrayV0(), py::arg("handedness") = SomaHandednessV0_UNSPECIFIED) + .def_property_readonly( + "joint_poses", field(&SomaHandJointPosesV0::joint_poses), py::return_value_policy::reference_internal) + .def_property_readonly("handedness", field(&SomaHandJointPosesV0::handedness)); + + bind_record( + m, "SomaHandJointPosesV0Record", "SomaHandJointPosesV0"); +} + +} // namespace core diff --git a/src/core/schema/python/soma_hand_v0_bindings.h b/src/core/schema/python/soma_hand_v0_bindings.h new file mode 100644 index 0000000000..90137cd765 --- /dev/null +++ b/src/core/schema/python/soma_hand_v0_bindings.h @@ -0,0 +1,119 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline const SomaJointRotationV0& first_soma_hand_rotation(const py::object& self) +{ + return *(*self.cast().values())[0]; +} + +constexpr py::ssize_t SOMA_HAND_JOINT_STRIDE = static_cast(sizeof(SomaJointRotationV0)); +constexpr py::ssize_t SOMA_HAND_JOINT_COUNT = static_cast(SomaHandJointV0_NUM_JOINTS); + +static_assert(sizeof(SomaHandJointRotationArrayV0) == + sizeof(SomaJointRotationV0) * static_cast(SomaHandJointV0_NUM_JOINTS), + "SomaHandJointRotationArrayV0.values length must equal SomaHandJointV0::NUM_JOINTS"); + +inline void bind_soma_hand_v0(py::module& m) +{ + py::enum_(m, "SomaHandJointV0") + .value("WRIST", SomaHandJointV0_WRIST) + .value("THUMB1", SomaHandJointV0_THUMB1) + .value("THUMB2", SomaHandJointV0_THUMB2) + .value("THUMB3", SomaHandJointV0_THUMB3) + .value("THUMB_END", SomaHandJointV0_THUMB_END) + .value("INDEX1", SomaHandJointV0_INDEX1) + .value("INDEX2", SomaHandJointV0_INDEX2) + .value("INDEX3", SomaHandJointV0_INDEX3) + .value("INDEX4", SomaHandJointV0_INDEX4) + .value("INDEX_END", SomaHandJointV0_INDEX_END) + .value("MIDDLE1", SomaHandJointV0_MIDDLE1) + .value("MIDDLE2", SomaHandJointV0_MIDDLE2) + .value("MIDDLE3", SomaHandJointV0_MIDDLE3) + .value("MIDDLE4", SomaHandJointV0_MIDDLE4) + .value("MIDDLE_END", SomaHandJointV0_MIDDLE_END) + .value("RING1", SomaHandJointV0_RING1) + .value("RING2", SomaHandJointV0_RING2) + .value("RING3", SomaHandJointV0_RING3) + .value("RING4", SomaHandJointV0_RING4) + .value("RING_END", SomaHandJointV0_RING_END) + .value("PINKY1", SomaHandJointV0_PINKY1) + .value("PINKY2", SomaHandJointV0_PINKY2) + .value("PINKY3", SomaHandJointV0_PINKY3) + .value("PINKY4", SomaHandJointV0_PINKY4) + .value("PINKY_END", SomaHandJointV0_PINKY_END) + .value("NUM_JOINTS", SomaHandJointV0_NUM_JOINTS); + + py::class_(m, "SomaHandJointRotationArrayV0") + .def(py::init<>()) + .def( + "values", + [](const SomaHandJointRotationArrayV0& self, size_t index) -> const SomaJointRotationV0* + { + if (index >= static_cast(SomaHandJointV0_NUM_JOINTS)) + { + throw py::index_error("SomaHandJointRotationArrayV0 index out of range"); + } + return (*self.values())[index]; + }, + py::arg("index"), py::return_value_policy::reference_internal) + .def_property_readonly( + "rotations", + [](py::object self) + { + const auto* first = reinterpret_cast(&first_soma_hand_rotation(self).rotation()); + return strided_field_view(self, first, SOMA_HAND_JOINT_STRIDE, SOMA_HAND_JOINT_COUNT, 4); + }, + "Unit XYZW quaternions as a writable (25, 4) float32 view.") + .def_property_readonly( + "is_valid", + [offset = FBS_FIELD_OFFSET(SomaJointRotationV0, is_valid)](py::object self) + { + const auto* first = fbs_field_address(first_soma_hand_rotation(self), offset); + return strided_field_view(self, first, SOMA_HAND_JOINT_STRIDE, SOMA_HAND_JOINT_COUNT, 0); + }, + "Per-joint validity as a writable (25,) uint8 view."); + + serialized_class(m, "SomaHandJointRotationsV0", "Encoded SOMA hand joint rotations v0.") + .def(py::init( + [](const SomaHandJointRotationArrayV0& joint_rotations, const Point& global_translation, + bool global_translation_is_valid, SomaHandednessV0 handedness) + { + SomaHandJointRotationsV0T native; + native.joint_rotations = std::make_shared(joint_rotations); + native.global_translation = std::make_shared(global_translation); + native.global_translation_is_valid = global_translation_is_valid; + native.handedness = handedness; + return pack(native); + }), + py::arg("joint_rotations") = SomaHandJointRotationArrayV0(), py::arg("global_translation") = Point(), + py::arg("global_translation_is_valid") = false, py::arg("handedness") = SomaHandednessV0_UNSPECIFIED) + .def_property_readonly("joint_rotations", field(&SomaHandJointRotationsV0::joint_rotations), + py::return_value_policy::reference_internal) + .def_property_readonly("global_translation", field(&SomaHandJointRotationsV0::global_translation), + py::return_value_policy::reference_internal) + .def_property_readonly( + "global_translation_is_valid", field(&SomaHandJointRotationsV0::global_translation_is_valid)) + .def_property_readonly("handedness", field(&SomaHandJointRotationsV0::handedness)); + + bind_record( + m, "SomaHandJointRotationsV0Record", "SomaHandJointRotationsV0"); +} + +} // namespace core diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py index 79c328d293..ab3235d23c 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py @@ -26,6 +26,7 @@ from .joint_state_source import JointStateSource from .full_body_source import FullBodySource from .soma_body_source import SomaBodyRepresentation, SomaBodySource +from .soma_hand_source import SomaHandRepresentation, SomaHandSource from .message_channel_source import MessageChannelSource from .message_channel_sink import MessageChannelSink from .message_channel_config import ( @@ -44,6 +45,8 @@ FullBodyPoseTrackedType, SomaBodyJointRotationsV0TrackedType, SomaBodyJointPosesV0TrackedType, + SomaHandJointRotationsV0TrackedType, + SomaHandJointPosesV0TrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, @@ -53,6 +56,8 @@ DeviceIOFullBodyPoseTracked, DeviceIOSomaBodyJointRotationsV0Tracked, DeviceIOSomaBodyJointPosesV0Tracked, + DeviceIOSomaHandJointRotationsV0Tracked, + DeviceIOSomaHandJointPosesV0Tracked, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, MessageChannelStatusType, @@ -81,6 +86,8 @@ "FullBodySource", "SomaBodySource", "SomaBodyRepresentation", + "SomaHandSource", + "SomaHandRepresentation", "MessageChannelSource", "MessageChannelSink", "MessageChannelConfig", @@ -96,6 +103,8 @@ "FullBodyPoseTrackedType", "SomaBodyJointRotationsV0TrackedType", "SomaBodyJointPosesV0TrackedType", + "SomaHandJointRotationsV0TrackedType", + "SomaHandJointPosesV0TrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", "MessageChannelStatusType", @@ -108,6 +117,8 @@ "DeviceIOFullBodyPoseTracked", "DeviceIOSomaBodyJointRotationsV0Tracked", "DeviceIOSomaBodyJointPosesV0Tracked", + "DeviceIOSomaHandJointRotationsV0Tracked", + "DeviceIOSomaHandJointPosesV0Tracked", "DeviceIOMessageChannelMessagesTracked", "MessageChannelMessagesTrackedGroup", "MessageChannelStatusGroup", diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py index b56170e12b..dfa90e3a91 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py @@ -24,6 +24,8 @@ FullBodyPose, SomaBodyJointPosesV0, SomaBodyJointRotationsV0, + SomaHandJointPosesV0, + SomaHandJointRotationsV0, MessageChannelMessagesTracked, ) @@ -129,6 +131,18 @@ class SomaBodyJointPosesV0TrackedType(_PayloadTensorType): _payload_cls = SomaBodyJointPosesV0 +class SomaHandJointRotationsV0TrackedType(_PayloadTensorType): + """SomaHandJointRotationsV0 payload from a generated SOMA hand tracker.""" + + _payload_cls = SomaHandJointRotationsV0 + + +class SomaHandJointPosesV0TrackedType(_PayloadTensorType): + """SomaHandJointPosesV0 payload from a generated SOMA hand tracker.""" + + _payload_cls = SomaHandJointPosesV0 + + class MessageChannelMessagesTrackedType(_RequiredPayloadTensorType): """MessageChannelMessagesTracked batch from DeviceIO MessageChannelTracker.""" @@ -245,6 +259,22 @@ def DeviceIOSomaBodyJointPosesV0Tracked() -> TensorGroupType: ) +def DeviceIOSomaHandJointRotationsV0Tracked() -> TensorGroupType: + """SOMA V0 hand controls, or None when the tracker is inactive.""" + return TensorGroupType( + "deviceio_soma_hand_joint_rotations_v0", + [SomaHandJointRotationsV0TrackedType("soma_hand_tracked")], + ) + + +def DeviceIOSomaHandJointPosesV0Tracked() -> TensorGroupType: + """Evaluated SOMA V0 hand poses, or None when the tracker is inactive.""" + return TensorGroupType( + "deviceio_soma_hand_joint_poses_v0", + [SomaHandJointPosesV0TrackedType("soma_hand_tracked")], + ) + + def DeviceIOMessageChannelMessagesTracked() -> TensorGroupType: """Message batch from DeviceIO MessageChannelTracker.""" return TensorGroupType( diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py new file mode 100644 index 0000000000..2d92ba03fd --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py @@ -0,0 +1,109 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""DeviceIO SOMA hand source for rotation and evaluated-pose transports.""" + +from enum import Enum +from typing import TYPE_CHECKING, Any + +import numpy as np + +from isaaccapture.deviceio_trackers import ( + SomaHandJointPosesV0Tracker, + SomaHandJointRotationsV0Tracker, +) +from isaaccapture.schema import SomaHandednessV0 + +from .deviceio_tensor_types import ( + DeviceIOSomaHandJointPosesV0Tracked, + DeviceIOSomaHandJointRotationsV0Tracked, +) +from .interface import IDeviceIOSource +from ..interface.retargeter_core_types import RetargeterIO, RetargeterIOType +from ..interface.tensor_group import TensorGroup +from ..interface.tensor_group_type import OptionalType +from ..tensor_types import SomaHandInput, SomaHandInputIndex +from ..utilities.soma_hand_evaluator import _SomaHandEvaluator + +if TYPE_CHECKING: + from isaaccapture.deviceio import ITracker + + +class SomaHandRepresentation(str, Enum): + JOINT_ROTATIONS = "joint-rotations" + JOINT_POSES = "joint-poses" + + +class SomaHandSource(IDeviceIOSource): + """Normalize one SOMA V0 hand collection into evaluated joint poses.""" + + HAND = "soma_hand" + + def __init__( + self, + name: str, + collection_id: str, + handedness: SomaHandednessV0, + layer: Any | None = None, + *, + representation: SomaHandRepresentation + | str = SomaHandRepresentation.JOINT_ROTATIONS, + ) -> None: + if not collection_id: + raise ValueError("SOMA hand collection_id must not be empty") + if handedness not in (SomaHandednessV0.LEFT, SomaHandednessV0.RIGHT): + raise ValueError("SOMA hand source requires LEFT or RIGHT handedness") + self.handedness = handedness + self.representation = SomaHandRepresentation(representation) + if self.representation is SomaHandRepresentation.JOINT_ROTATIONS: + if layer is None: + raise ValueError("joint-rotations requires a prepared SOMA hand layer") + self._tracker = SomaHandJointRotationsV0Tracker(collection_id) + self._input_type = DeviceIOSomaHandJointRotationsV0Tracked() + else: + self._tracker = SomaHandJointPosesV0Tracker(collection_id) + self._input_type = DeviceIOSomaHandJointPosesV0Tracked() + self._evaluator = _SomaHandEvaluator(layer) if layer is not None else None + super().__init__(name) + + def get_tracker(self) -> "ITracker": + return self._tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + group = TensorGroup(self.input_spec()["deviceio_soma_hand"]) + group[0] = self._tracker.get_data(deviceio_session) + return {"deviceio_soma_hand": group} + + def input_spec(self) -> RetargeterIOType: + return {"deviceio_soma_hand": self._input_type} + + def output_spec(self) -> RetargeterIOType: + return {self.HAND: OptionalType(SomaHandInput())} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + data = inputs["deviceio_soma_hand"][0] + group = outputs[self.HAND] + if data is None: + group.set_none() + return + if data.handedness != self.handedness: + raise ValueError( + f"SOMA hand payload handedness {data.handedness} does not match " + f"source handedness {self.handedness}" + ) + + if self.representation is SomaHandRepresentation.JOINT_ROTATIONS: + assert self._evaluator is not None + positions, orientations, valid = self._evaluator.evaluate(data) + elif data.joint_poses is None: + positions = np.zeros((25, 3), dtype=np.float32) + orientations = np.zeros((25, 4), dtype=np.float32) + orientations[:, 3] = 1.0 + valid = np.zeros(25, dtype=np.uint8) + else: + positions = np.asarray(data.joint_poses.positions, dtype=np.float32) + orientations = np.asarray(data.joint_poses.orientations, dtype=np.float32) + valid = np.asarray(data.joint_poses.is_valid, dtype=np.uint8) + group[SomaHandInputIndex.JOINT_POSITIONS] = positions + group[SomaHandInputIndex.JOINT_ORIENTATIONS] = orientations + group[SomaHandInputIndex.JOINT_VALID] = valid diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py b/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py index 11577bb286..514b057465 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py @@ -9,6 +9,7 @@ from .ndarray_types import NDArrayType, DLDeviceType, DLDataType from .standard_types import ( HandInput, + SomaHandInput, HeadInput, ControllerInput, FullBodyInput, @@ -16,6 +17,7 @@ TransformMatrix, Generic3AxisPedalInput, NUM_HAND_JOINTS, + NUM_SOMA_HAND_JOINTS, NUM_BODY_JOINTS, NUM_SOMA_BODY_JOINTS, RobotHandJoints, @@ -32,6 +34,7 @@ ) from .indices import ( HandInputIndex, + SomaHandInputIndex, HeadInputIndex, ControllerInputIndex, Generic3AxisPedalInputIndex, @@ -53,6 +56,7 @@ "DLDataType", # Standard types "HandInput", + "SomaHandInput", "HeadInput", "ControllerInput", "FullBodyInput", @@ -60,6 +64,7 @@ "TransformMatrix", "Generic3AxisPedalInput", "NUM_HAND_JOINTS", + "NUM_SOMA_HAND_JOINTS", "NUM_BODY_JOINTS", "NUM_SOMA_BODY_JOINTS", "RobotHandJoints", @@ -74,6 +79,7 @@ "NUM_END_EFFECTOR_FORCE_AXES", # Indices "HandInputIndex", + "SomaHandInputIndex", "HeadInputIndex", "ControllerInputIndex", "Generic3AxisPedalInputIndex", diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py b/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py index 954f1d1f68..40a954b7ac 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py @@ -17,6 +17,7 @@ from enum import IntEnum from .standard_types import ( HandInput, + SomaHandInput, HeadInput, ControllerInput, Generic3AxisPedalInput, @@ -38,6 +39,9 @@ def _create_index_enum(name: str, group_type, prefix: str = "") -> IntEnum: # Generate indices dynamically HandInputIndex: Any = _create_index_enum("HandInputIndex", HandInput(), "hand_") +SomaHandInputIndex: Any = _create_index_enum( + "SomaHandInputIndex", SomaHandInput(), "soma_hand_" +) HeadInputIndex: Any = _create_index_enum("HeadInputIndex", HeadInput(), "head_") ControllerInputIndex: Any = _create_index_enum( "ControllerInputIndex", ControllerInput(), "controller_" diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/standard_types.py b/src/python/isaaccapture/retargeting_engine/tensor_types/standard_types.py index e39ed4e9a4..ab16e30ad4 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/standard_types.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/standard_types.py @@ -17,6 +17,7 @@ # Constants NUM_HAND_JOINTS = 26 # XR_HAND_JOINT_COUNT_EXT from OpenXR +NUM_SOMA_HAND_JOINTS = 25 # SOMAHandLayer wrist plus 24 finger joints NUM_BODY_JOINTS = 24 # XR_BODY_JOINT_COUNT_BD from XR_BD_body_tracking NUM_SOMA_BODY_JOINTS = 77 # Public SOMA joints, excluding virtual Root @@ -86,6 +87,33 @@ def input_spec(self) -> RetargeterIO: ) +def SomaHandInput() -> TensorGroupType: + """Evaluated global poses for the 25 SOMA hand joints.""" + return TensorGroupType( + "soma_hand", + [ + NDArrayType( + "soma_hand_joint_positions", + shape=(NUM_SOMA_HAND_JOINTS, 3), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + NDArrayType( + "soma_hand_joint_orientations", + shape=(NUM_SOMA_HAND_JOINTS, 4), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + NDArrayType( + "soma_hand_joint_valid", + shape=(NUM_SOMA_HAND_JOINTS,), + dtype=DLDataType.UINT, + dtype_bits=8, + ), + ], + ) + + # ============================================================================ # Head Tracking Types # ============================================================================ diff --git a/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py b/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py new file mode 100644 index 0000000000..1231fe53a6 --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py @@ -0,0 +1,94 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Internal evaluation of transported SOMA hand controls.""" + +from importlib import import_module +from typing import Any + +import numpy as np + +from .soma_body_evaluator import _numpy + + +class _SomaHandEvaluator: + """Run upstream SOMA hand FK and expose its 25 evaluated joints.""" + + def __init__(self, layer: Any) -> None: + joint_names = tuple(str(name) for name in layer.rig_data["joint_names"]) + parent_ids = _numpy(layer.joint_parent_ids).astype(np.int64) + if len(joint_names) != 25 or parent_ids.shape != (25,): + raise ValueError("SOMA hand evaluator requires 25 joints") + if parent_ids[0] != 0 or any( + parent < 0 or parent >= child + for child, parent in enumerate(parent_ids[1:], 1) + ): + raise ValueError("SOMA hand joints must follow parent-before-child order") + + self.layer = layer + self.joint_names = joint_names + self.parent_ids = tuple(int(parent) for parent in parent_ids) + self.bones = tuple( + (int(parent), child) for child, parent in enumerate(parent_ids[1:], 1) + ) + + def _joint_validity( + self, control_valid: np.ndarray, translation_valid: bool + ) -> np.ndarray: + valid = np.zeros(25, dtype=np.uint8) + valid[0] = translation_valid and control_valid[0] + for child in range(1, 25): + valid[child] = valid[self.parent_ids[child]] and control_valid[child] + return valid + + @staticmethod + def _pose_inputs( + data: Any, + ) -> tuple[np.ndarray, np.ndarray, np.ndarray, bool]: + joint_rotations = data.joint_rotations + if joint_rotations is None: + control_valid = np.zeros(25, dtype=bool) + rotations = np.zeros((25, 4), dtype=np.float32) + rotations[:, 3] = 1.0 + else: + control_valid = np.asarray(joint_rotations.is_valid, dtype=bool) + rotations = np.asarray(joint_rotations.rotations, dtype=np.float32).copy() + rotations[~control_valid] = (0.0, 0.0, 0.0, 1.0) + + translation = data.global_translation + translation_valid = ( + bool(data.global_translation_is_valid) and translation is not None + ) + translation_xyz = np.asarray( + [translation.x, translation.y, translation.z] + if translation_valid + else [0.0, 0.0, 0.0], + dtype=np.float32, + ) + return rotations, control_valid, translation_xyz, translation_valid + + def evaluate(self, data: Any) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + torch = import_module("torch") + transforms = import_module("soma.geometry.transforms") + rotations, control_valid, translation_xyz, translation_valid = ( + self._pose_inputs(data) + ) + + with torch.no_grad(): + output = self.layer.pose( + transforms.quaternion_xyzw_to_matrix( + torch.from_numpy(rotations).unsqueeze(0) + ), + global_translation=torch.from_numpy(translation_xyz).unsqueeze(0), + pose2rot=False, + fk_only=True, + apply_correctives=False, + ) + orientations = transforms.matrix_to_quaternion_xyzw( + output["transforms"][0, :, :3, :3] + ) + return ( + _numpy(output["joints"][0]).astype(np.float32, copy=False), + _numpy(orientations).astype(np.float32, copy=False), + self._joint_validity(control_valid, translation_valid), + ) diff --git a/src/python/isaaccapture/schema/__init__.py b/src/python/isaaccapture/schema/__init__.py index 07601e70f5..d5ef0097dc 100644 --- a/src/python/isaaccapture/schema/__init__.py +++ b/src/python/isaaccapture/schema/__init__.py @@ -85,6 +85,7 @@ FullBodyPose, FullBodyPoseRecord, # Shared SOMA types. + SomaHandednessV0, SomaJointRotationV0, # SOMA body joint-rotation v0 types. SomaBodyJointV0, @@ -96,6 +97,16 @@ SomaBodyJointPoseArrayV0, SomaBodyJointPosesV0, SomaBodyJointPosesV0Record, + # SOMA hand joint-rotation v0 types. + SomaHandJointV0, + SomaHandJointRotationArrayV0, + SomaHandJointRotationsV0, + SomaHandJointRotationsV0Record, + # SOMA evaluated hand joint-pose v0 types. + SomaHandJointPoseV0, + SomaHandJointPoseArrayV0, + SomaHandJointPosesV0, + SomaHandJointPosesV0Record, ) # Deprecated aliases, resolved lazily via __getattr__ so accessing them emits a @@ -202,6 +213,7 @@ def __getattr__(name: str): "FullBodyPose", "FullBodyPoseRecord", # Shared SOMA types. + "SomaHandednessV0", "SomaJointRotationV0", # SOMA body joint-rotation v0 types. "SomaBodyJointV0", @@ -213,4 +225,14 @@ def __getattr__(name: str): "SomaBodyJointPoseArrayV0", "SomaBodyJointPosesV0", "SomaBodyJointPosesV0Record", + # SOMA hand joint-rotation v0 types. + "SomaHandJointV0", + "SomaHandJointRotationArrayV0", + "SomaHandJointRotationsV0", + "SomaHandJointRotationsV0Record", + # SOMA evaluated hand joint-pose v0 types. + "SomaHandJointPoseV0", + "SomaHandJointPoseArrayV0", + "SomaHandJointPosesV0", + "SomaHandJointPosesV0Record", ] diff --git a/tests/cpp/core/schema/CMakeLists.txt b/tests/cpp/core/schema/CMakeLists.txt index 3450c4d1fe..59d25bcfb2 100644 --- a/tests/cpp/core/schema/CMakeLists.txt +++ b/tests/cpp/core/schema/CMakeLists.txt @@ -18,6 +18,8 @@ add_executable(schema_tests test_serialized.cpp test_soma_body_v0.cpp test_soma_body_joint_poses_v0.cpp + test_soma_hand_v0.cpp + test_soma_hand_joint_poses_v0.cpp test_schema_conform.cpp ) target_link_libraries(schema_tests PRIVATE diff --git a/tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp b/tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp new file mode 100644 index 0000000000..4c5d7c6006 --- /dev/null +++ b/tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp @@ -0,0 +1,49 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include + +namespace +{ + +constexpr auto vt(uint16_t field_id) +{ + return static_cast((field_id + 2) * 2); +} + +} // namespace + +static_assert(core::SomaHandJointPosesV0::VT_JOINT_POSES == vt(0)); +static_assert(core::SomaHandJointPosesV0::VT_HANDEDNESS == vt(1)); +static_assert(core::SomaHandJointPosesV0Record::VT_DATA == vt(0)); +static_assert(core::SomaHandJointPosesV0Record::VT_TIMESTAMP == vt(1)); +static_assert(sizeof(core::SomaHandJointPoseArrayV0) == 25 * sizeof(core::SomaHandJointPoseV0)); + +TEST_CASE("SOMA hand joint poses v0 round trip through FlatBuffers", "[soma_hand_joint_poses_v0][flatbuffers]") +{ + core::SomaHandJointPosesV0T pose; + pose.joint_poses = std::make_shared(); + pose.joint_poses->mutable_values()->Mutate( + core::SomaHandJointV0_INDEX_END, + core::SomaHandJointPoseV0( + core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)), true)); + pose.handedness = core::SomaHandednessV0_LEFT; + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaHandJointPosesV0::Pack(builder, &pose)); + flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); + REQUIRE(verifier.VerifyBuffer(nullptr)); + + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(decoded->joint_poses() != nullptr); + const auto* index_end = (*decoded->joint_poses()->values())[core::SomaHandJointV0_INDEX_END]; + CHECK(index_end->pose().position().x() == Catch::Approx(1.0f)); + CHECK(index_end->pose().orientation().w() == Catch::Approx(0.8f)); + CHECK(index_end->is_valid()); + CHECK(decoded->handedness() == core::SomaHandednessV0_LEFT); +} diff --git a/tests/cpp/core/schema/test_soma_hand_v0.cpp b/tests/cpp/core/schema/test_soma_hand_v0.cpp new file mode 100644 index 0000000000..5d9eaf82a9 --- /dev/null +++ b/tests/cpp/core/schema/test_soma_hand_v0.cpp @@ -0,0 +1,130 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#define VT(field) (field + 2) * 2 +static_assert(core::SomaHandJointRotationsV0::VT_JOINT_ROTATIONS == VT(0)); +static_assert(core::SomaHandJointRotationsV0::VT_GLOBAL_TRANSLATION == VT(1)); +static_assert(core::SomaHandJointRotationsV0::VT_GLOBAL_TRANSLATION_IS_VALID == VT(2)); +static_assert(core::SomaHandJointRotationsV0::VT_HANDEDNESS == VT(3)); +static_assert(core::SomaHandJointRotationsV0Record::VT_DATA == VT(0)); +static_assert(core::SomaHandJointRotationsV0Record::VT_TIMESTAMP == VT(1)); + +#define TYPE(field) decltype(std::declval().field()) +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); +static_assert(std::is_same_v); + +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaHandJointRotationArrayV0) == 25 * sizeof(core::SomaJointRotationV0)); + +static_assert(core::SomaHandJointV0_WRIST == 0); +static_assert(core::SomaHandJointV0_INDEX1 == 5); +static_assert(core::SomaHandJointV0_MIDDLE1 == 10); +static_assert(core::SomaHandJointV0_RING1 == 15); +static_assert(core::SomaHandJointV0_PINKY_END == 24); +static_assert(core::SomaHandJointV0_NUM_JOINTS == 25); + +TEST_CASE("SOMA hand v0 rotation array has the public layout", "[soma_hand_v0][struct]") +{ + core::SomaHandJointRotationArrayV0 rotations; + + REQUIRE(rotations.values()->size() == static_cast(core::SomaHandJointV0_NUM_JOINTS)); + CHECK((*rotations.values())[core::SomaHandJointV0_WRIST]->rotation().x() == 0.0f); + CHECK_FALSE((*rotations.values())[core::SomaHandJointV0_PINKY_END]->is_valid()); + + const core::SomaJointRotationV0 wrist(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true); + rotations.mutable_values()->Mutate(core::SomaHandJointV0_WRIST, wrist); + + CHECK((*rotations.values())[core::SomaHandJointV0_WRIST]->rotation().z() == Catch::Approx(0.6f)); + CHECK((*rotations.values())[core::SomaHandJointV0_WRIST]->rotation().w() == Catch::Approx(0.8f)); + CHECK((*rotations.values())[core::SomaHandJointV0_WRIST]->is_valid()); +} + +TEST_CASE("SOMA hand joint rotations v0 round trip through FlatBuffers", "[soma_hand_v0][flatbuffers]") +{ + core::SomaHandJointRotationsV0T pose; + pose.joint_rotations = std::make_shared(); + pose.joint_rotations->mutable_values()->Mutate( + core::SomaHandJointV0_INDEX_END, core::SomaJointRotationV0(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true)); + pose.global_translation = std::make_shared(1.0f, 2.0f, 3.0f); + pose.global_translation_is_valid = true; + pose.handedness = core::SomaHandednessV0_RIGHT; + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaHandJointRotationsV0::Pack(builder, &pose)); + + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(decoded->joint_rotations() != nullptr); + REQUIRE(decoded->global_translation() != nullptr); + const auto* index_end = (*decoded->joint_rotations()->values())[core::SomaHandJointV0_INDEX_END]; + CHECK(index_end->rotation().z() == Catch::Approx(0.6f)); + CHECK(index_end->rotation().w() == Catch::Approx(0.8f)); + CHECK(index_end->is_valid()); + CHECK(decoded->global_translation()->x() == Catch::Approx(1.0f)); + CHECK(decoded->global_translation_is_valid()); + CHECK(decoded->handedness() == core::SomaHandednessV0_RIGHT); +} + +TEST_CASE("SOMA hand record v0 preserves quaternion components and validity", "[soma_hand_v0][flatbuffers]") +{ + const float scale = 1.0f / std::sqrt(30.0f); + core::SomaHandJointRotationsV0RecordT record; + record.data = std::make_shared(); + record.data->joint_rotations = std::make_shared(); + record.data->joint_rotations->mutable_values()->Mutate( + core::SomaHandJointV0_WRIST, + core::SomaJointRotationV0(core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale), true)); + record.data->joint_rotations->mutable_values()->Mutate( + core::SomaHandJointV0_PINKY_END, + core::SomaJointRotationV0(core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale), true)); + record.data->global_translation = std::make_shared(1.0f, -2.0f, 3.0f); + record.data->global_translation_is_valid = false; + record.data->handedness = core::SomaHandednessV0_LEFT; + record.timestamp = std::make_shared(100, 200, 300); + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaHandJointRotationsV0Record::Pack(builder, &record)); + flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); + REQUIRE(verifier.VerifyBuffer(nullptr)); + + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(decoded->data() != nullptr); + REQUIRE(decoded->timestamp() != nullptr); + REQUIRE(decoded->data()->joint_rotations() != nullptr); + REQUIRE(decoded->data()->global_translation() != nullptr); + const auto* values = decoded->data()->joint_rotations()->values(); + const auto* wrist = (*values)[core::SomaHandJointV0_WRIST]; + const auto* pinky = (*values)[core::SomaHandJointV0_PINKY_END]; + CHECK(wrist->rotation().x() == Catch::Approx(scale)); + CHECK(wrist->rotation().y() == Catch::Approx(-2.0f * scale)); + CHECK(wrist->rotation().z() == Catch::Approx(3.0f * scale)); + CHECK(wrist->rotation().w() == Catch::Approx(-4.0f * scale)); + CHECK(pinky->rotation().x() == Catch::Approx(-scale)); + CHECK(pinky->rotation().y() == Catch::Approx(2.0f * scale)); + CHECK(pinky->rotation().z() == Catch::Approx(-3.0f * scale)); + CHECK(pinky->rotation().w() == Catch::Approx(4.0f * scale)); + CHECK(wrist->is_valid()); + CHECK(pinky->is_valid()); + CHECK_FALSE((*values)[core::SomaHandJointV0_INDEX1]->is_valid()); + CHECK(decoded->data()->global_translation()->x() == Catch::Approx(1.0f)); + CHECK(decoded->data()->global_translation()->y() == Catch::Approx(-2.0f)); + CHECK(decoded->data()->global_translation()->z() == Catch::Approx(3.0f)); + CHECK_FALSE(decoded->data()->global_translation_is_valid()); + CHECK(decoded->data()->handedness() == core::SomaHandednessV0_LEFT); + CHECK(decoded->timestamp()->available_time_local_common_clock() == 100); + CHECK(decoded->timestamp()->sample_time_local_common_clock() == 200); + CHECK(decoded->timestamp()->sample_time_raw_device_clock() == 300); +} diff --git a/tests/python/core/retargeting_engine/test_soma_hand_source.py b/tests/python/core/retargeting_engine/test_soma_hand_source.py new file mode 100644 index 0000000000..12f6fbf89a --- /dev/null +++ b/tests/python/core/retargeting_engine/test_soma_hand_source.py @@ -0,0 +1,139 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from unittest.mock import MagicMock + +import numpy as np +import pytest + +from isaaccapture.deviceio_trackers import ( + SomaHandJointPosesV0Tracker, + SomaHandJointRotationsV0Tracker, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + DeviceIOSomaHandJointPosesV0Tracked, + DeviceIOSomaHandJointRotationsV0Tracked, + SomaHandRepresentation, + SomaHandSource, +) +from isaaccapture.retargeting_engine.interface.tensor_group import TensorGroup +from isaaccapture.retargeting_engine.tensor_types import SomaHandInputIndex +from isaaccapture.schema import ( + Point, + SomaHandednessV0, + SomaHandJointPoseArrayV0, + SomaHandJointPosesV0, + SomaHandJointRotationArrayV0, + SomaHandJointRotationsV0, +) + + +def fake_layer(): + layer = MagicMock() + layer.rig_data = {"joint_names": [f"Joint{index}" for index in range(25)]} + layer.joint_parent_ids = np.array([0, 0, 1, 2, 3, *range(20)])[:25] + return layer + + +def rotations(handedness=SomaHandednessV0.LEFT): + joints = SomaHandJointRotationArrayV0() + joints.rotations[:] = (0, 0, 0, 1) + joints.is_valid[:] = 1 + return SomaHandJointRotationsV0(joints, Point(1, 2, 3), True, handedness) + + +def poses(handedness=SomaHandednessV0.LEFT): + joints = SomaHandJointPoseArrayV0() + joints.positions[:] = np.arange(75, dtype=np.float32).reshape(25, 3) + joints.orientations[:] = np.tile([0, 0, 0, 1], (25, 1)) + joints.is_valid[:] = 1 + return SomaHandJointPosesV0(joints, handedness) + + +def payload_group(group_type, data): + group = TensorGroup(group_type) + group[0] = data + return group + + +def test_rotation_source_evaluates_received_payload(): + source = SomaHandSource("left", "vendor.left", SomaHandednessV0.LEFT, fake_layer()) + assert isinstance(source.get_tracker(), SomaHandJointRotationsV0Tracker) + raw = rotations() + expected = ( + np.ones((25, 3), dtype=np.float32), + np.tile([0, 0, 0, 1], (25, 1)).astype(np.float32), + np.ones(25, dtype=np.uint8), + ) + source._evaluator.evaluate = MagicMock(return_value=expected) + inputs = payload_group(source.input_spec()["deviceio_soma_hand"], raw) + + evaluated = source({"deviceio_soma_hand": inputs})[SomaHandSource.HAND] + + source._evaluator.evaluate.assert_called_once_with(raw) + np.testing.assert_array_equal( + evaluated[SomaHandInputIndex.JOINT_POSITIONS], expected[0] + ) + np.testing.assert_array_equal( + evaluated[SomaHandInputIndex.JOINT_ORIENTATIONS], expected[1] + ) + np.testing.assert_array_equal( + evaluated[SomaHandInputIndex.JOINT_VALID], expected[2] + ) + + +def test_joint_pose_source_maps_without_fk(): + source = SomaHandSource( + "left", + "vendor.left", + SomaHandednessV0.LEFT, + representation=SomaHandRepresentation.JOINT_POSES, + ) + assert isinstance(source.get_tracker(), SomaHandJointPosesV0Tracker) + raw = poses() + inputs = payload_group(source.input_spec()["deviceio_soma_hand"], raw) + + evaluated = source({"deviceio_soma_hand": inputs})[SomaHandSource.HAND] + + assert source._evaluator is None + np.testing.assert_array_equal( + evaluated[SomaHandInputIndex.JOINT_POSITIONS], raw.joint_poses.positions + ) + np.testing.assert_array_equal( + evaluated[SomaHandInputIndex.JOINT_VALID], raw.joint_poses.is_valid + ) + + +def test_payload_handedness_must_match_collection(): + source = SomaHandSource( + "left", + "vendor.left", + SomaHandednessV0.LEFT, + representation=SomaHandRepresentation.JOINT_POSES, + ) + inputs = payload_group( + source.input_spec()["deviceio_soma_hand"], poses(SomaHandednessV0.RIGHT) + ) + with pytest.raises(ValueError, match="handedness"): + source({"deviceio_soma_hand": inputs}) + + +def test_hand_transport_profiles_are_distinct(): + rotation_type = DeviceIOSomaHandJointRotationsV0Tracked() + pose_type = DeviceIOSomaHandJointPosesV0Tracked() + with pytest.raises(ValueError, match="type mismatch"): + rotation_type.check_compatibility(pose_type) + with pytest.raises(TypeError, match="SomaHandJointPosesV0"): + payload_group(pose_type, rotations()) + + +def test_rotation_source_requires_layer_and_valid_side(): + with pytest.raises(ValueError, match="prepared SOMA hand layer"): + SomaHandSource("left", "vendor.left", SomaHandednessV0.LEFT) + with pytest.raises(ValueError, match="LEFT or RIGHT"): + SomaHandSource( + "left", + "vendor.left", + SomaHandednessV0.UNSPECIFIED, + fake_layer(), + ) diff --git a/tests/python/core/schema/test_soma_hand_joint_poses_v0.py b/tests/python/core/schema/test_soma_hand_joint_poses_v0.py new file mode 100644 index 0000000000..0073939b36 --- /dev/null +++ b/tests/python/core/schema/test_soma_hand_joint_poses_v0.py @@ -0,0 +1,54 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import gc + +import numpy as np +import pytest + +from isaaccapture.schema import ( + DeviceDataTimestamp, + SomaHandednessV0, + SomaHandJointPoseArrayV0, + SomaHandJointPosesV0, + SomaHandJointPosesV0Record, + SomaHandJointV0, +) + + +def test_soma_hand_joint_pose_views_alias_storage(): + poses = SomaHandJointPoseArrayV0() + + assert poses.positions.shape == (25, 3) + assert poses.orientations.shape == (25, 4) + assert poses.is_valid.shape == (25,) + assert poses.positions.dtype == np.float32 + assert poses.orientations.dtype == np.float32 + assert poses.is_valid.dtype == np.uint8 + + poses.positions[SomaHandJointV0.INDEX_END] = [1.0, 2.0, 3.0] + poses.orientations[SomaHandJointV0.INDEX_END] = [0.0, 0.0, 0.6, 0.8] + poses.is_valid[SomaHandJointV0.INDEX_END] = 1 + + index_end = poses.values(int(SomaHandJointV0.INDEX_END)) + assert index_end.pose.position.y == pytest.approx(2.0) + assert index_end.pose.orientation.w == pytest.approx(0.8) + assert index_end.is_valid is True + + +def test_soma_hand_joint_poses_record_lifetime(): + poses = SomaHandJointPoseArrayV0() + poses.positions[:] = np.arange(75, dtype=np.float32).reshape(25, 3) + poses.orientations[:, 3] = 1.0 + poses.is_valid[:] = 1 + payload = SomaHandJointPosesV0(poses, SomaHandednessV0.RIGHT) + record = SomaHandJointPosesV0Record(payload, DeviceDataTimestamp(100, 200, 300)) + + poses.positions[:] = 0.0 + payload.joint_poses.positions[:] = 0.0 + del poses, payload + gc.collect() + + assert record.data.handedness == SomaHandednessV0.RIGHT + assert record.data.joint_poses.positions[24, 2] == pytest.approx(74.0) + assert record.timestamp.sample_time_raw_device_clock == 300 diff --git a/tests/python/core/schema/test_soma_hand_v0.py b/tests/python/core/schema/test_soma_hand_v0.py new file mode 100644 index 0000000000..235463307c --- /dev/null +++ b/tests/python/core/schema/test_soma_hand_v0.py @@ -0,0 +1,173 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import gc + +import numpy as np +import pytest + +from isaaccapture.schema import ( + DeviceDataTimestamp, + Point, + Quaternion, + SomaHandednessV0, + SomaHandJointRotationArrayV0, + SomaHandJointRotationsV0, + SomaHandJointRotationsV0Record, + SomaHandJointV0, + SomaJointRotationV0, +) + + +def test_soma_hand_v0_joint_order(): + expected = [ + "WRIST", + "THUMB1", + "THUMB2", + "THUMB3", + "THUMB_END", + "INDEX1", + "INDEX2", + "INDEX3", + "INDEX4", + "INDEX_END", + "MIDDLE1", + "MIDDLE2", + "MIDDLE3", + "MIDDLE4", + "MIDDLE_END", + "RING1", + "RING2", + "RING3", + "RING4", + "RING_END", + "PINKY1", + "PINKY2", + "PINKY3", + "PINKY4", + "PINKY_END", + ] + + assert len(expected) == int(SomaHandJointV0.NUM_JOINTS) == 25 + for index, name in enumerate(expected): + assert int(getattr(SomaHandJointV0, name)) == index + + +def test_soma_hand_v0_joint_rotation(): + rotation = SomaJointRotationV0(Quaternion(0.0, 0.0, 0.6, 0.8), True) + + assert rotation.rotation.x == 0.0 + assert rotation.rotation.y == 0.0 + assert rotation.rotation.z == pytest.approx(0.6) + assert rotation.rotation.w == pytest.approx(0.8) + assert rotation.is_valid is True + + +def test_soma_hand_v0_rotation_views_alias_storage(): + rotations = SomaHandJointRotationArrayV0() + + assert rotations.rotations.shape == (25, 4) + assert rotations.rotations.dtype == np.float32 + assert rotations.is_valid.shape == (25,) + assert rotations.is_valid.dtype == np.uint8 + assert not rotations.rotations.flags.owndata + + rotations.rotations[SomaHandJointV0.INDEX_END] = [0.0, 0.0, 0.6, 0.8] + rotations.is_valid[SomaHandJointV0.INDEX_END] = 1 + + index_end = rotations.values(int(SomaHandJointV0.INDEX_END)) + assert index_end.rotation.z == pytest.approx(0.6) + assert index_end.rotation.w == pytest.approx(0.8) + assert index_end.is_valid is True + + +def test_soma_hand_v0_rotation_index_check(): + with pytest.raises(IndexError): + SomaHandJointRotationArrayV0().values(25) + + +def test_soma_hand_v0_pose_construction_and_lifetime(): + rotations = SomaHandJointRotationArrayV0() + rotations.rotations[:] = [0.0, 0.0, 0.0, 1.0] + rotations.rotations[SomaHandJointV0.PINKY_END] = [0.0, 0.0, 0.6, 0.8] + rotations.is_valid[:] = 1 + + pose = SomaHandJointRotationsV0( + rotations, Point(1.0, 2.0, 3.0), True, SomaHandednessV0.RIGHT + ) + quaternion_rotations = pose.joint_rotations.rotations + del pose + gc.collect() + + assert quaternion_rotations[24, 3] == pytest.approx(0.8) + + +def test_soma_hand_v0_pose_defaults_and_global_translation(): + pose = SomaHandJointRotationsV0() + + assert pose.joint_rotations is not None + assert pose.global_translation.x == 0.0 + assert pose.global_translation_is_valid is False + assert pose.handedness == SomaHandednessV0.UNSPECIFIED + + right = SomaHandJointRotationsV0( + global_translation=Point(1.0, 2.0, 3.0), + global_translation_is_valid=True, + handedness=SomaHandednessV0.RIGHT, + ) + assert right.global_translation.y == pytest.approx(2.0) + assert right.global_translation_is_valid is True + assert right.handedness == SomaHandednessV0.RIGHT + + +def test_soma_hand_v0_pose_record(): + record = SomaHandJointRotationsV0Record( + SomaHandJointRotationsV0(handedness=SomaHandednessV0.LEFT), + DeviceDataTimestamp(100, 200, 300), + ) + + assert record.data is not None + assert record.data.handedness == SomaHandednessV0.LEFT + assert record.timestamp.sample_time_local_common_clock == 200 + + +@pytest.mark.parametrize("sign", [-1.0, 1.0]) +@pytest.mark.parametrize("translation_is_valid", [False, True]) +@pytest.mark.parametrize("handedness", [SomaHandednessV0.LEFT, SomaHandednessV0.RIGHT]) +def test_soma_hand_v0_record_preserves_quaternion_layout( + sign, translation_is_valid, handedness +): + quaternion = np.array([1.0, -2.0, 3.0, -4.0], dtype=np.float32) + quaternion *= sign / np.linalg.norm(quaternion) + rotations = SomaHandJointRotationArrayV0() + rotations.rotations[SomaHandJointV0.WRIST] = quaternion + rotations.rotations[SomaHandJointV0.PINKY_END] = -quaternion + rotations.is_valid[SomaHandJointV0.WRIST] = 1 + rotations.is_valid[SomaHandJointV0.PINKY_END] = 1 + expected_rotations = rotations.rotations.copy() + expected_validity = rotations.is_valid.copy() + pose = SomaHandJointRotationsV0( + rotations, Point(1.0, -2.0, 3.0), translation_is_valid, handedness + ) + record = SomaHandJointRotationsV0Record(pose, DeviceDataTimestamp(100, 200, 300)) + + rotations.rotations[:] = 0.0 + rotations.is_valid[:] = 0 + pose.joint_rotations.rotations[:] = 0.0 + pose.joint_rotations.is_valid[:] = 0 + del pose, rotations + gc.collect() + + data = record.data + np.testing.assert_array_equal(data.joint_rotations.rotations, expected_rotations) + np.testing.assert_array_equal(data.joint_rotations.is_valid, expected_validity) + wrist = data.joint_rotations.values(int(SomaHandJointV0.WRIST)).rotation + np.testing.assert_array_equal([wrist.x, wrist.y, wrist.z, wrist.w], quaternion) + assert data.global_translation.x == pytest.approx(1.0) + assert data.global_translation.y == pytest.approx(-2.0) + assert data.global_translation.z == pytest.approx(3.0) + assert data.global_translation_is_valid is translation_is_valid + assert data.handedness == handedness + assert record.timestamp.available_time_local_common_clock == 100 + assert record.timestamp.sample_time_local_common_clock == 200 + assert record.timestamp.sample_time_raw_device_clock == 300 diff --git a/tests/python/examples/CMakeLists.txt b/tests/python/examples/CMakeLists.txt index c9b4f91b8b..a0cc7b40cc 100644 --- a/tests/python/examples/CMakeLists.txt +++ b/tests/python/examples/CMakeLists.txt @@ -4,3 +4,4 @@ add_subdirectory(camera_viz) add_subdirectory(deviceio_live_view) add_subdirectory(soma_body_publisher) +add_subdirectory(soma_hand_publisher) diff --git a/tests/python/examples/deviceio_live_view/test_soma_viewer.py b/tests/python/examples/deviceio_live_view/test_soma_viewer.py index c73ed10fb2..ad4fc65f3e 100644 --- a/tests/python/examples/deviceio_live_view/test_soma_viewer.py +++ b/tests/python/examples/deviceio_live_view/test_soma_viewer.py @@ -8,11 +8,15 @@ from isaaccapture.deviceio_trackers import ( SomaBodyJointPosesV0Tracker, SomaBodyJointRotationsV0Tracker, + SomaHandJointPosesV0Tracker, + SomaHandJointRotationsV0Tracker, ) from isaaccapture.retargeting_engine.deviceio_source_nodes import ( FullBodySource, SomaBodyRepresentation, SomaBodySource, + SomaHandRepresentation, + SomaHandSource, ) from isaaccapture.retargeting_engine.deviceio_source_nodes import soma_body_source from isaaccapture.retargeting_engine.interface.base_retargeter import _make_output_group @@ -29,6 +33,11 @@ SomaBodyJointPosesV0, SomaBodyJointRotationArrayV0, SomaBodyJointRotationsV0, + SomaHandednessV0, + SomaHandJointPoseArrayV0, + SomaHandJointPosesV0, + SomaHandJointRotationArrayV0, + SomaHandJointRotationsV0, ) from isaaccapture_examples.deviceio_live_view import live_deviceio, soma_body from isaaccapture_examples.deviceio_live_view.body_pipeline import ( @@ -46,6 +55,15 @@ from isaaccapture_examples.deviceio_live_view.full_body_pose import ( FULL_BODY_POSE_LAYOUT, ) +from isaaccapture_examples.deviceio_live_view.hand_pipeline import ( + HandSchema, + HandViewLayout, + create_hand_view_pipeline, +) +from isaaccapture_examples.deviceio_live_view.openxr_hand_pose import ( + OPENXR_HAND_LAYOUT, +) +from isaaccapture_examples.deviceio_live_view.soma_hand import SOMA_HAND_LAYOUT def fake_layer(): @@ -70,11 +88,29 @@ def soma_joint_poses(positions, orientations): return SomaBodyJointPosesV0(joints) -def fake_viz(layout: BodyViewLayout = FULL_BODY_POSE_LAYOUT): +def soma_hand_joint_poses(positions, orientations, handedness): + joints = SomaHandJointPoseArrayV0() + joints.positions[:] = positions + joints.orientations[:] = orientations + joints.is_valid[:] = 1 + return SomaHandJointPosesV0(joints, handedness) + + +def soma_hand_joint_rotations(rotations, translation, handedness): + joints = SomaHandJointRotationArrayV0() + joints.rotations[:] = rotations + joints.is_valid[:] = 1 + return SomaHandJointRotationsV0(joints, Point(*translation), True, handedness) + + +def fake_viz( + layout: BodyViewLayout = FULL_BODY_POSE_LAYOUT, + hand_layout: HandViewLayout = OPENXR_HAND_LAYOUT, +): server = MagicMock() for name in ("add_point_cloud", "add_line_segments", "add_frame"): getattr(server.scene, name).side_effect = lambda **kwargs: MagicMock(**kwargs) - return HumanDeviceIOViz(server, MagicMock(), layout) + return HumanDeviceIOViz(server, MagicMock(), layout, hand_layout) def test_default_pipeline_keeps_full_body_source(): @@ -132,6 +168,64 @@ def test_soma_joint_pose_pipeline_selects_direct_tracker(monkeypatch, tmp_path): tracker_factory.assert_called_once_with("vendor_body") +def test_soma_joint_pose_hands_replace_only_openxr_hands(): + hands = create_hand_view_pipeline( + hand_schema=HandSchema.SOMA, + soma_left_collection_id="vendor.left", + soma_right_collection_id="vendor.right", + soma_hand_representation=SomaHandRepresentation.JOINT_POSES, + ) + pipeline = build_all_human_pipeline(hands=hands) + sources = {source.name: source for source in pipeline.get_leaf_nodes()} + + assert set(sources) == { + "hand_left", + "hand_right", + "head", + "controllers", + "body", + } + assert isinstance(sources["hand_left"], SomaHandSource) + assert isinstance(sources["hand_right"], SomaHandSource) + assert isinstance(sources["hand_left"].get_tracker(), SomaHandJointPosesV0Tracker) + assert hands.layout is SOMA_HAND_LAYOUT + + +def test_soma_hand_pose_reaches_native_25_joint_renderer(): + hands = create_hand_view_pipeline( + hand_schema=HandSchema.SOMA, + soma_hand_representation=SomaHandRepresentation.JOINT_POSES, + ) + pipeline = build_all_human_pipeline(hands=hands) + sources = {source.name: source for source in pipeline.get_leaf_nodes()} + for source in (sources["hand_left"], sources["hand_right"]): + source._tracker = MagicMock() + positions = np.arange(75, dtype=np.float32).reshape(25, 3) / 100 + orientations = np.tile([0, 0, 0, 1], (25, 1)).astype(np.float32) + sources["hand_left"]._tracker.get_data.return_value = soma_hand_joint_poses( + positions, orientations, SomaHandednessV0.LEFT + ) + sources["hand_right"]._tracker.get_data.return_value = None + + inputs = {} + for source in pipeline.get_leaf_nodes(): + inputs[source.name] = {} + for key, kind in source.input_spec().items(): + group = TensorGroup(kind) + group[0] = None + inputs[source.name][key] = group + inputs["hand_left"] = sources["hand_left"].poll_tracker(object()) + inputs["hand_right"] = sources["hand_right"].poll_tracker(object()) + result = pipeline.execute_pipeline(inputs) + viz = fake_viz(hand_layout=hands.layout) + active = viz.update(result) + + assert active["hand_left"] + assert not active["hand_right"] + np.testing.assert_array_equal(viz.hand_left.points.points, positions) + assert len(viz.hand_left.bones.points) == len(SOMA_HAND_LAYOUT.bones) + + def test_explicit_full_body_selection_does_not_load_soma(monkeypatch, tmp_path): create = MagicMock(side_effect=AssertionError("SOMA must remain optional")) monkeypatch.setattr(soma_body, "create_layer", create) @@ -232,8 +326,11 @@ def test_live_cli_selects_source_and_launches_runtime( monkeypatch, tmp_path, schema, use_assets, expected, collection ): body = create_body_view_pipeline() + hands = create_hand_view_pipeline() select = MagicMock(return_value=body) monkeypatch.setattr(live_deviceio, "create_body_view_pipeline", select) + select_hands = MagicMock(return_value=hands) + monkeypatch.setattr(live_deviceio, "create_hand_view_pipeline", select_hands) pipeline = build_all_human_pipeline() build = MagicMock(return_value=pipeline) monkeypatch.setattr(live_deviceio, "build_all_human_pipeline", build) @@ -260,7 +357,14 @@ def test_live_cli_selects_source_and_launches_runtime( soma_collection_id=collection, soma_body_representation="joint-rotations", ) - build.assert_called_once_with(body=body) + select_hands.assert_called_once_with( + tmp_path if use_assets else None, + hand_schema="openxr-hand-pose", + soma_left_collection_id="soma_hand_left_demo", + soma_right_collection_id="soma_hand_right_demo", + soma_hand_representation="joint-rotations", + ) + build.assert_called_once_with(body=body, hands=hands) launch.assert_called_once() assert create_session.call_args.args[0].pipeline is pipeline @@ -331,6 +435,39 @@ def test_native_soma_pose_reaches_renderer(soma_assets): assert 0 < active["body_joints"] < 77 +def test_native_soma_hand_rotations_reach_renderer(soma_assets): + hands = create_hand_view_pipeline(soma_assets, hand_schema=HandSchema.SOMA) + pipeline = build_all_human_pipeline(hands=hands) + sources = {source.name: source for source in pipeline.get_leaf_nodes()} + inputs = {} + for node in pipeline.get_leaf_nodes(): + inputs[node.name] = {} + for key, kind in node.input_spec().items(): + group = TensorGroup(kind) + group[0] = None + inputs[node.name][key] = group + + identity = np.tile([0, 0, 0, 1], (25, 1)).astype(np.float32) + for name, handedness in ( + ("hand_left", SomaHandednessV0.LEFT), + ("hand_right", SomaHandednessV0.RIGHT), + ): + source = sources[name] + assert isinstance(source.get_tracker(), SomaHandJointRotationsV0Tracker) + source._tracker = MagicMock() + source._tracker.get_data.return_value = soma_hand_joint_rotations( + identity, [0, 0, 0], handedness + ) + inputs[name] = source.poll_tracker(object()) + + viz = fake_viz(hand_layout=hands.layout) + active = viz.update(pipeline.execute_pipeline(inputs)) + assert active["hand_left"] + assert active["hand_right"] + assert viz.hand_left.points.points.shape == (25, 3) + assert viz.hand_right.points.points.shape == (25, 3) + + def test_evaluated_soma_pose_reaches_renderer_without_fk(soma_assets): body = create_body_view_pipeline( soma_assets, diff --git a/tests/python/examples/soma_hand_publisher/CMakeLists.txt b/tests/python/examples/soma_hand_publisher/CMakeLists.txt new file mode 100644 index 0000000000..846de2f59f --- /dev/null +++ b/tests/python/examples/soma_hand_publisher/CMakeLists.txt @@ -0,0 +1,22 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +file(GLOB TEST_FILES + RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" + "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py" +) + +foreach(test_file ${TEST_FILES}) + get_filename_component(test_name "${test_file}" NAME_WE) + add_test( + NAME "soma_hand_publisher_${test_name}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev + pytest -v --tb=short "${test_file}" + --soma-hand-pusher $ + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" + ) + set_tests_properties("soma_hand_publisher_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "soma_hand_publisher;unit" + ) +endforeach() diff --git a/tests/python/examples/soma_hand_publisher/conftest.py b/tests/python/examples/soma_hand_publisher/conftest.py new file mode 100644 index 0000000000..8373623d92 --- /dev/null +++ b/tests/python/examples/soma_hand_publisher/conftest.py @@ -0,0 +1,28 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys +from pathlib import Path + +import pytest + +sys.path.insert(0, str(Path(__file__).resolve().parents[2])) + +from repo_paths import repo_root # noqa: E402 + +sys.path.insert(0, str(repo_root() / "examples" / "soma_hand_publisher" / "python")) + + +def pytest_addoption(parser): + parser.addoption("--soma-assets", type=Path, help="SOMA-X assets directory") + parser.addoption( + "--soma-hand-pusher", type=Path, help="Built soma_hand_pusher executable" + ) + + +@pytest.fixture(scope="module") +def soma_assets(request): + path = request.config.getoption("--soma-assets") + if path is None: + pytest.skip("Pass --soma-assets to run pinned SOMA-X semantic tests") + return path diff --git a/tests/python/examples/soma_hand_publisher/pyproject.toml b/tests/python/examples/soma_hand_publisher/pyproject.toml new file mode 100644 index 0000000000..2d44618f54 --- /dev/null +++ b/tests/python/examples/soma_hand_publisher/pyproject.toml @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "soma-hand-publisher-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = ["pytest", "numpy"] +soma = ["py-soma-x==0.3.1"] + +[tool.pytest.ini_options] +pythonpath = ["."] diff --git a/tests/python/examples/soma_hand_publisher/test_publisher.py b/tests/python/examples/soma_hand_publisher/test_publisher.py new file mode 100644 index 0000000000..777f827d6a --- /dev/null +++ b/tests/python/examples/soma_hand_publisher/test_publisher.py @@ -0,0 +1,118 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import struct +import subprocess + +import numpy as np + +from isaaccapture.retargeting_engine.utilities.soma_hand_evaluator import ( + _SomaHandEvaluator, +) +from isaaccapture.schema import SomaHandednessV0 +from isaaccapture_examples.soma_hand_publisher.publisher import ( + create_layers, + demo_hand_frames, + soma_joint_poses, + soma_joint_rotations, +) + + +def test_hand_payload_constructors_preserve_side_and_arrays(): + rotations = np.tile([0.0, 0.0, 0.0, 1.0], (25, 1)) + rotation_payload = soma_joint_rotations(rotations, [1, 2, 3], SomaHandednessV0.LEFT) + assert rotation_payload.handedness == SomaHandednessV0.LEFT + np.testing.assert_array_equal(rotation_payload.joint_rotations.rotations, rotations) + + positions = np.arange(75, dtype=np.float32).reshape(25, 3) + pose_payload = soma_joint_poses(positions, rotations, SomaHandednessV0.RIGHT) + assert pose_payload.handedness == SomaHandednessV0.RIGHT + np.testing.assert_array_equal(pose_payload.joint_poses.positions, positions) + + +def test_pusher_rejects_collection_handedness_mismatch(request): + executable = request.config.getoption("--soma-hand-pusher") + if executable is None: + return + rotations = np.tile([0.0, 0.0, 0.0, 1.0], (25, 1)) + wrong_left = soma_joint_rotations( + rotations, [0, 0, 0], SomaHandednessV0.RIGHT + ).to_bytes() + right = soma_joint_rotations( + rotations, [0, 0, 0], SomaHandednessV0.RIGHT + ).to_bytes() + packet = struct.pack(" Date: Tue, 6 Oct 2026 13:46:16 -0400 Subject: [PATCH 03/12] refactor(soma): align transport naming with IsaacTeleop - remove V0 suffixes from SOMA schemas, trackers, records, and channels - rename the body and hand rotation schemas to *_joint_rotations, drop V0 from SOMA API names, and regenerate schema goldens - remove redundant runtime version checks while retaining the pinned dependency - update examples, tests, bindings, and documentation for the renamed API Signed-off-by: Michael Haselton --- docs/source/device/trackers.rst | 77 ++++---- docs/source/references/generated_trackers.rst | 24 +-- docs/source/references/retargeting/index.rst | 4 +- examples/deviceio_live_view/README.md | 12 +- .../deviceio_live_view/soma_body.py | 8 +- .../deviceio_live_view/soma_hand.py | 12 +- examples/soma_body_publisher/README.md | 4 +- .../cpp/soma_body_pusher/soma_body_pusher.cpp | 10 +- .../soma_body_publisher/publisher.py | 24 ++- examples/soma_hand_publisher/README.md | 2 +- .../cpp/soma_hand_pusher/soma_hand_pusher.cpp | 20 +-- .../soma_hand_publisher/publisher.py | 24 ++- src/core/deviceio_trackers/trackers.toml | 40 ++--- ...ody_common_v0.fbs => soma_body_common.fbs} | 2 +- ...poses_v0.fbs => soma_body_joint_poses.fbs} | 22 +-- ...y_v0.fbs => soma_body_joint_rotations.fbs} | 22 +-- .../{soma_common_v0.fbs => soma_common.fbs} | 6 +- ...poses_v0.fbs => soma_hand_joint_poses.fbs} | 26 +-- ...d_v0.fbs => soma_hand_joint_rotations.fbs} | 24 +-- ...y_common_v0.bfbs => soma_body_common.bfbs} | Bin 4824 -> 4816 bytes .../schema/golden/soma_body_joint_poses.bfbs | Bin 0 -> 6680 bytes .../golden/soma_body_joint_poses_v0.bfbs | Bin 6696 -> 0 bytes ...v0.bfbs => soma_body_joint_rotations.bfbs} | Bin 6912 -> 6904 bytes .../{soma_common_v0.bfbs => soma_common.bfbs} | Bin 1024 -> 1000 bytes .../schema/golden/soma_hand_joint_poses.bfbs | Bin 0 -> 4616 bytes .../golden/soma_hand_joint_poses_v0.bfbs | Bin 4632 -> 0 bytes .../golden/soma_hand_joint_rotations.bfbs | Bin 0 -> 3712 bytes src/core/schema/golden/soma_hand_v0.bfbs | Bin 3728 -> 0 bytes src/core/schema/python/CMakeLists.txt | 10 +- src/core/schema/python/schema_module.cpp | 22 +-- ...ngs.h => soma_body_joint_poses_bindings.h} | 60 +++---- .../soma_body_joint_rotations_bindings.h | 169 ++++++++++++++++++ .../schema/python/soma_body_v0_bindings.h | 169 ------------------ src/core/schema/python/soma_common_bindings.h | 28 +++ .../schema/python/soma_common_v0_bindings.h | 28 --- ...ngs.h => soma_hand_joint_poses_bindings.h} | 62 +++---- .../soma_hand_joint_rotations_bindings.h | 119 ++++++++++++ .../schema/python/soma_hand_v0_bindings.h | 119 ------------ .../deviceio_source_nodes/__init__.py | 32 ++-- .../deviceio_tensor_types.py | 64 +++---- .../deviceio_source_nodes/soma_body_source.py | 18 +- .../deviceio_source_nodes/soma_hand_source.py | 24 +-- src/python/isaaccapture/schema/__init__.py | 88 ++++----- tests/cpp/core/schema/CMakeLists.txt | 8 +- ..._v0.cpp => test_soma_body_joint_poses.cpp} | 30 ++-- ...cpp => test_soma_body_joint_rotations.cpp} | 98 +++++----- ..._v0.cpp => test_soma_hand_joint_poses.cpp} | 34 ++-- ...cpp => test_soma_hand_joint_rotations.cpp} | 110 ++++++------ .../test_soma_body_source.py | 36 ++-- .../test_soma_hand_source.py | 52 +++--- ...es_v0.py => test_soma_body_joint_poses.py} | 26 +-- ...0.py => test_soma_body_joint_rotations.py} | 74 ++++---- ...es_v0.py => test_soma_hand_joint_poses.py} | 28 +-- ...0.py => test_soma_hand_joint_rotations.py} | 86 ++++----- .../deviceio_live_view/test_soma_viewer.py | 66 ++++--- .../soma_body_publisher/test_publisher.py | 10 +- .../soma_hand_publisher/test_publisher.py | 24 ++- 57 files changed, 1023 insertions(+), 1034 deletions(-) rename src/core/schema/fbs/{soma_body_common_v0.fbs => soma_body_common.fbs} (98%) rename src/core/schema/fbs/{soma_body_joint_poses_v0.fbs => soma_body_joint_poses.fbs} (62%) rename src/core/schema/fbs/{soma_body_v0.fbs => soma_body_joint_rotations.fbs} (68%) rename src/core/schema/fbs/{soma_common_v0.fbs => soma_common.fbs} (80%) rename src/core/schema/fbs/{soma_hand_joint_poses_v0.fbs => soma_hand_joint_poses.fbs} (57%) rename src/core/schema/fbs/{soma_hand_v0.fbs => soma_hand_joint_rotations.fbs} (74%) rename src/core/schema/golden/{soma_body_common_v0.bfbs => soma_body_common.bfbs} (90%) create mode 100644 src/core/schema/golden/soma_body_joint_poses.bfbs delete mode 100644 src/core/schema/golden/soma_body_joint_poses_v0.bfbs rename src/core/schema/golden/{soma_body_v0.bfbs => soma_body_joint_rotations.bfbs} (69%) rename src/core/schema/golden/{soma_common_v0.bfbs => soma_common.bfbs} (50%) create mode 100644 src/core/schema/golden/soma_hand_joint_poses.bfbs delete mode 100644 src/core/schema/golden/soma_hand_joint_poses_v0.bfbs create mode 100644 src/core/schema/golden/soma_hand_joint_rotations.bfbs delete mode 100644 src/core/schema/golden/soma_hand_v0.bfbs rename src/core/schema/python/{soma_body_joint_poses_v0_bindings.h => soma_body_joint_poses_bindings.h} (53%) create mode 100644 src/core/schema/python/soma_body_joint_rotations_bindings.h delete mode 100644 src/core/schema/python/soma_body_v0_bindings.h create mode 100644 src/core/schema/python/soma_common_bindings.h delete mode 100644 src/core/schema/python/soma_common_v0_bindings.h rename src/core/schema/python/{soma_hand_joint_poses_v0_bindings.h => soma_hand_joint_poses_bindings.h} (53%) create mode 100644 src/core/schema/python/soma_hand_joint_rotations_bindings.h delete mode 100644 src/core/schema/python/soma_hand_v0_bindings.h rename tests/cpp/core/schema/{test_soma_body_joint_poses_v0.cpp => test_soma_body_joint_poses.cpp} (62%) rename tests/cpp/core/schema/{test_soma_body_v0.cpp => test_soma_body_joint_rotations.cpp} (54%) rename tests/cpp/core/schema/{test_soma_hand_joint_poses_v0.cpp => test_soma_hand_joint_poses.cpp} (52%) rename tests/cpp/core/schema/{test_soma_hand_v0.cpp => test_soma_hand_joint_rotations.cpp} (52%) rename tests/python/core/schema/{test_soma_body_joint_poses_v0.py => test_soma_body_joint_poses.py} (68%) rename tests/python/core/schema/{test_soma_body_v0.py => test_soma_body_joint_rotations.py} (71%) rename tests/python/core/schema/{test_soma_hand_joint_poses_v0.py => test_soma_hand_joint_poses.py} (63%) rename tests/python/core/schema/{test_soma_hand_v0.py => test_soma_hand_joint_rotations.py} (61%) diff --git a/docs/source/device/trackers.rst b/docs/source/device/trackers.rst index 99e42cc8e5..faec8789c3 100644 --- a/docs/source/device/trackers.rst +++ b/docs/source/device/trackers.rst @@ -24,10 +24,10 @@ reading it from OpenXR tensor collections via the - :code-file:`Generic3AxisPedalTracker ` -- foot pedal axis values (generated) - :code-file:`JointStateTracker ` -- named joint-space device state (leader arms, exoskeletons, gloves, ...) (generated) - :code-file:`Se3Tracker ` -- generic SE3 (6-DoF) pose sources (tracker pucks, mocap rigid bodies, logical trackers) (generated) -- :code-file:`SomaBodyJointRotationsV0Tracker ` -- SOMA v0 body joint rotations with 77 controls (generated) -- :code-file:`SomaBodyJointPosesV0Tracker ` -- evaluated SOMA v0 body poses for 77 joints (generated) -- :code-file:`SomaHandJointRotationsV0Tracker ` -- SOMA v0 hand joint rotations with 25 controls (generated) -- :code-file:`SomaHandJointPosesV0Tracker ` -- evaluated SOMA v0 hand poses for 25 joints (generated) +- :code-file:`SomaBodyJointRotationsTracker ` -- SOMA body joint rotations with 77 controls (generated) +- :code-file:`SomaBodyJointPosesTracker ` -- evaluated SOMA body poses for 77 joints (generated) +- :code-file:`SomaHandJointRotationsTracker ` -- SOMA hand joint rotations with 25 controls (generated) +- :code-file:`SomaHandJointPosesTracker ` -- evaluated SOMA hand poses for 25 joints (generated) All trackers follow the same lifecycle: @@ -273,15 +273,14 @@ reads the PICO ``XR_BD_body_tracking`` extension directly. ``FullBodyTrackerPico`` remains available as a deprecated alias for ``FullBodyTracker`` so existing scripts run unchanged. -SOMA body V0 trackers +SOMA body trackers ~~~~~~~~~~~~~~~~~~~~~ -Reads the SOMA body joint-rotation transport schema v0 from an OpenXR tensor collection. ``V0`` -versions the Isaac Teleop schema; it is not an upstream SOMA-X release number. The schema is defined -and verified against SOMA-X v0.3.1 and its v0.3 pose interface. A vendor adapter converts its native -skeleton to :code:`SomaBodyJointRotationsV0`, serializes the checked-in schema, and publishes it -through ``SchemaPusher``. The tracker transports and records the fixed payload; it does not perform -vendor-specific skeleton or reference-pose conversion. +Reads the SOMA body joint-rotation transport schema from an OpenXR tensor collection. The schema is +defined and verified against SOMA-X v0.3.1 and its v0.3 pose interface. A vendor adapter converts +its native skeleton to :code:`SomaBodyJointRotations`, serializes the checked-in schema, and +publishes it through ``SchemaPusher``. The tracker transports and records the fixed payload; it does +not perform vendor-specific skeleton or reference-pose conversion. The payload contains the 77 user-facing joint rotations for SOMA's 78-joint public skeleton. It omits the virtual Root, which is always identity and is added internally by ``SOMALayer``. Hips is the @@ -298,7 +297,7 @@ A SOMA-X consumer can convert the quaternion array with and pass the resulting matrices to ``SOMALayer.pose(..., pose2rot=False)``. When a producer already has evaluated global positions and orientations, it can -publish ``SomaBodyJointPosesV0`` through ``SomaBodyJointPosesV0Tracker`` instead. +publish ``SomaBodyJointPoses`` through ``SomaBodyJointPosesTracker`` instead. That profile carries a global pose and independent validity for each of the same 77 public joints. Positions use meters and orientations use unit XYZW quaternions in the same reference-frame convention. It requires no downstream @@ -319,35 +318,35 @@ not require SOMA-X or PyTorch. The live-view example supplies those evaluation dependencies, renders the SOMA public hierarchy, and offers launch-time body-schema selection. It does not map SOMA into ``FullBodyPose``. -- Schemas: :code-file:`src/core/schema/fbs/soma_body_common_v0.fbs`, :code-file:`src/core/schema/fbs/soma_body_v0.fbs`, :code-file:`src/core/schema/fbs/soma_body_joint_poses_v0.fbs` -- Manifest: :code-file:`src/core/deviceio_trackers/trackers.toml` (``soma_body_joint_rotations_v0`` and ``soma_body_joint_poses_v0``) -- C++ header: ``#include `` -- Python import: ``from isaaccapture.deviceio_trackers import SomaBodyJointRotationsV0Tracker`` -- Evaluated-pose C++ header: ``#include `` -- Evaluated-pose Python import: ``from isaaccapture.deviceio_trackers import SomaBodyJointPosesV0Tracker`` +- Schemas: :code-file:`src/core/schema/fbs/soma_body_common.fbs`, :code-file:`src/core/schema/fbs/soma_body_joint_rotations.fbs`, :code-file:`src/core/schema/fbs/soma_body_joint_poses.fbs` +- Manifest: :code-file:`src/core/deviceio_trackers/trackers.toml` (``soma_body_joint_rotations`` and ``soma_body_joint_poses``) +- C++ header: ``#include `` +- Python import: ``from isaaccapture.deviceio_trackers import SomaBodyJointRotationsTracker`` +- Evaluated-pose C++ header: ``#include `` +- Evaluated-pose Python import: ``from isaaccapture.deviceio_trackers import SomaBodyJointPosesTracker`` - Graph source: :code-file:`src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py` - Live-view example: :code-file:`examples/deviceio_live_view/README.md` - Demo publisher: :code-file:`examples/soma_body_publisher/README.md` -- Record channels: ``soma_body_joint_rotations_v0``, ``soma_body_joint_rotations_v0_tracked`` | MCAP schema: ``core.SomaBodyJointRotationsV0Record`` -- Evaluated-pose record channels: ``soma_body_joint_poses_v0``, ``soma_body_joint_poses_v0_tracked`` | MCAP schema: ``core.SomaBodyJointPosesV0Record`` +- Record channels: ``soma_body_joint_rotations``, ``soma_body_joint_rotations_tracked`` | MCAP schema: ``core.SomaBodyJointRotationsRecord`` +- Evaluated-pose record channels: ``soma_body_joint_poses``, ``soma_body_joint_poses_tracked`` | MCAP schema: ``core.SomaBodyJointPosesRecord`` - Tests: - - :code-file:`tests/cpp/core/schema/test_soma_body_v0.cpp` - - :code-file:`tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp` - - :code-file:`tests/python/core/schema/test_soma_body_v0.py` + - :code-file:`tests/cpp/core/schema/test_soma_body_joint_rotations.cpp` + - :code-file:`tests/cpp/core/schema/test_soma_body_joint_poses.cpp` + - :code-file:`tests/python/core/schema/test_soma_body_joint_rotations.py` - :code-file:`tests/python/core/retargeting_engine/test_soma_body_source.py` -SomaHandJointRotationsV0Tracker and SomaHandJointPosesV0Tracker +SomaHandJointRotationsTracker and SomaHandJointPosesTracker ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Reads either of the two SOMA hand transport profiles defined and verified against SOMA-X v0.3.1. -``SomaHandJointRotationsV0`` contains 25 unit XYZW quaternions in ``SomaHandJointV0`` order. Wrist +``SomaHandJointRotations`` contains 25 unit XYZW quaternions in ``SomaHandJoint`` order. Wrist is the global rotation; the remaining rotations are joint-local relative to the fixed v0.3.1 reference contract. ``global_translation`` is applied to Wrist and uses the same units and -reference-frame convention as ``SomaBodyJointRotationsV0``. +reference-frame convention as ``SomaBodyJointRotations``. When a producer has already evaluated forward kinematics, it can instead publish -``SomaHandJointPosesV0``. That profile carries a global position, orientation, and validity for each +``SomaHandJointPoses``. That profile carries a global position, orientation, and validity for each of the same 25 joints and requires no downstream FK. Create one tracker per hand collection; each payload identifies its side through ``handedness``. As with the body contract, a vendor adapter converts its device-native hand data before calling ``SchemaPusher``. @@ -358,23 +357,23 @@ tracker. A rotation source requires a prepared SOMA hand layer; an evaluated-pos The DeviceIO live-view example selects body and hand inputs independently and creates one source for each hand. -- Schemas: :code-file:`src/core/schema/fbs/soma_common_v0.fbs`, :code-file:`src/core/schema/fbs/soma_hand_v0.fbs`, :code-file:`src/core/schema/fbs/soma_hand_joint_poses_v0.fbs` -- Manifest: :code-file:`src/core/deviceio_trackers/trackers.toml` (``soma_hand_joint_rotations_v0`` and ``soma_hand_joint_poses_v0``) -- Rotation C++ header: ``#include `` -- Rotation Python import: ``from isaaccapture.deviceio_trackers import SomaHandJointRotationsV0Tracker`` -- Evaluated-pose C++ header: ``#include `` -- Evaluated-pose Python import: ``from isaaccapture.deviceio_trackers import SomaHandJointPosesV0Tracker`` +- Schemas: :code-file:`src/core/schema/fbs/soma_common.fbs`, :code-file:`src/core/schema/fbs/soma_hand_joint_rotations.fbs`, :code-file:`src/core/schema/fbs/soma_hand_joint_poses.fbs` +- Manifest: :code-file:`src/core/deviceio_trackers/trackers.toml` (``soma_hand_joint_rotations`` and ``soma_hand_joint_poses``) +- Rotation C++ header: ``#include `` +- Rotation Python import: ``from isaaccapture.deviceio_trackers import SomaHandJointRotationsTracker`` +- Evaluated-pose C++ header: ``#include `` +- Evaluated-pose Python import: ``from isaaccapture.deviceio_trackers import SomaHandJointPosesTracker`` - Graph source: :code-file:`src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py` - Live-view example: :code-file:`examples/deviceio_live_view/README.md` - Demo publisher: :code-file:`examples/soma_hand_publisher/README.md` -- Rotation record channels: ``soma_hand_joint_rotations_v0``, ``soma_hand_joint_rotations_v0_tracked`` | MCAP schema: ``core.SomaHandJointRotationsV0Record`` -- Evaluated-pose record channels: ``soma_hand_joint_poses_v0``, ``soma_hand_joint_poses_v0_tracked`` | MCAP schema: ``core.SomaHandJointPosesV0Record`` +- Rotation record channels: ``soma_hand_joint_rotations``, ``soma_hand_joint_rotations_tracked`` | MCAP schema: ``core.SomaHandJointRotationsRecord`` +- Evaluated-pose record channels: ``soma_hand_joint_poses``, ``soma_hand_joint_poses_tracked`` | MCAP schema: ``core.SomaHandJointPosesRecord`` - Tests: - - :code-file:`tests/cpp/core/schema/test_soma_hand_v0.cpp` - - :code-file:`tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp` - - :code-file:`tests/python/core/schema/test_soma_hand_v0.py` - - :code-file:`tests/python/core/schema/test_soma_hand_joint_poses_v0.py` + - :code-file:`tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp` + - :code-file:`tests/cpp/core/schema/test_soma_hand_joint_poses.cpp` + - :code-file:`tests/python/core/schema/test_soma_hand_joint_rotations.py` + - :code-file:`tests/python/core/schema/test_soma_hand_joint_poses.py` - :code-file:`tests/python/core/retargeting_engine/test_soma_hand_source.py` FrameMetadataTrackerOak diff --git a/docs/source/references/generated_trackers.rst b/docs/source/references/generated_trackers.rst index b796c5efcd..28ad576c54 100644 --- a/docs/source/references/generated_trackers.rst +++ b/docs/source/references/generated_trackers.rst @@ -89,18 +89,18 @@ Generated today * - ``se3_tracker`` - ``Se3Tracker`` - Overrides ``class``; the default would give ``Se3TrackerTracker`` - * - ``soma_body_joint_rotations_v0`` - - ``SomaBodyJointRotationsV0Tracker`` - - SOMA v0 body joint rotations with 77 controls for the 78-joint skeleton; defined against SOMA-X v0.3.1 - * - ``soma_body_joint_poses_v0`` - - ``SomaBodyJointPosesV0Tracker`` - - Evaluated SOMA v0 positions and orientations for the same 77 public joints - * - ``soma_hand_joint_rotations_v0`` - - ``SomaHandJointRotationsV0Tracker`` - - SOMA v0 hand joint rotations with 25 controls; defined against SOMA-X v0.3.1 - * - ``soma_hand_joint_poses_v0`` - - ``SomaHandJointPosesV0Tracker`` - - Evaluated SOMA v0 positions and orientations for the same 25 joints + * - ``soma_body_joint_rotations`` + - ``SomaBodyJointRotationsTracker`` + - SOMA body joint rotations with 77 controls for the 78-joint skeleton; defined against SOMA-X v0.3.1 + * - ``soma_body_joint_poses`` + - ``SomaBodyJointPosesTracker`` + - Evaluated SOMA positions and orientations for the same 77 public joints + * - ``soma_hand_joint_rotations`` + - ``SomaHandJointRotationsTracker`` + - SOMA hand joint rotations with 25 controls; defined against SOMA-X v0.3.1 + * - ``soma_hand_joint_poses`` + - ``SomaHandJointPosesTracker`` + - Evaluated SOMA positions and orientations for the same 25 joints * - ``oglo_tactile`` - ``OgloTactileTracker`` - MCAP channels are ``oglo``/``oglo_tracked``, so ``channel`` is overridden diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 08848dc5e6..ec8783659c 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -17,8 +17,8 @@ Source Nodes * ``JointStateSource`` -- provides name-keyed joint positions from a generic joint-space device (leader arm, exoskeleton, ...). See :doc:`joint_space`. * ``FullBodySource`` -- provides full-body pose (e.g. Pico tracking). -* ``SomaBodySource`` -- polls either a typed ``SomaBodyJointRotationsV0`` or - ``SomaBodyJointPosesV0`` payload from a configured publisher collection. It +* ``SomaBodySource`` -- polls either a typed ``SomaBodyJointRotations`` or + ``SomaBodyJointPoses`` payload from a configured publisher collection. It runs upstream FK for rotations or directly maps evaluated poses, then emits an optional ``SomaBodyInput`` containing the 77 global SOMA joint poses. See :doc:`/device/trackers` and :code-file:`examples/deviceio_live_view/README.md`. diff --git a/examples/deviceio_live_view/README.md b/examples/deviceio_live_view/README.md index 0457a8d848..3d9f737063 100644 --- a/examples/deviceio_live_view/README.md +++ b/examples/deviceio_live_view/README.md @@ -27,9 +27,9 @@ in SOMA's native public order. It uses a neutral identity, not subject calibration. Separate hand tracking is unchanged. ```text -SomaBodyJointRotationsV0 -> tracker -> upstream FK --+ +SomaBodyJointRotations -> tracker -> upstream FK --+ +-> SomaBodyInput -> viewer -SomaBodyJointPosesV0 ----> tracker -> direct map ----+ +SomaBodyJointPoses ----> tracker -> direct map ----+ ``` Use SOMA-X 0.3.1 and its neutral identity assets. For an uninstalled source @@ -62,8 +62,8 @@ Joint rotations are the default SOMA profile. Pass joint poses instead. The publisher and viewer options must match. `SomaBodySource` follows the normal DeviceIO source contract: the session discovers -the generated tracker selected for `SomaBodyJointRotationsV0` or -`SomaBodyJointPosesV0`. The raw tracker remains the transport and recording +the generated tracker selected for `SomaBodyJointRotations` or +`SomaBodyJointPoses`. The raw tracker remains the transport and recording boundary. The source evaluates rotations once per graph step against its caller-supplied SOMA identity; evaluated poses map directly. Both modes emit the same global positions, orientations, and validity as `SomaBodyInput`. The @@ -87,9 +87,9 @@ OpenXR `HandPose` remains the default hand input. Select independent left and right SOMA hand collections with `--hand-schema soma`: ```text -SomaHandJointRotationsV0 -> tracker -> upstream FK --+ +SomaHandJointRotations -> tracker -> upstream FK --+ +-> SomaHandInput -> viewer -SomaHandJointPosesV0 ----> tracker -> direct map -------+ +SomaHandJointPoses ----> tracker -> direct map -------+ ``` ```bash diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py index c5787d9adb..1fd04215aa 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py @@ -10,7 +10,7 @@ SomaBodySource, ) from isaaccapture.retargeting_engine.tensor_types import SomaBodyInputIndex -from isaaccapture.schema import SomaBodyJointV0 +from isaaccapture.schema import SomaBodyJoint from .body_pipeline import BodyViewLayout, BodyViewPipeline @@ -19,8 +19,6 @@ def create_layer(data_root: Path): import soma import torch - if soma.__version__ != "0.3.1": - raise RuntimeError(f"The POC requires SOMA-X 0.3.1, got {soma.__version__}") layer = soma.SOMALayer( data_root=str(data_root), identity_model_type="soma", @@ -33,12 +31,12 @@ def create_layer(data_root: Path): expected = [ name.replace("_", "") for name, joint in sorted( - SomaBodyJointV0.__members__.items(), key=lambda item: int(item[1]) + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) ) if name != "NUM_JOINTS" ] if [name.upper() for name in layer.public_joint_names] != ["ROOT", *expected]: - raise ValueError("SOMA layer joint order differs from the V0 FBS") + raise ValueError("SOMA layer joint order differs from the FBS") with torch.no_grad(): layer.prepare_identity( torch.zeros((1, layer.identity_model.num_identity_coeffs)) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py index dcc2e21526..429cc06458 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py @@ -10,7 +10,7 @@ SomaHandSource, ) from isaaccapture.retargeting_engine.tensor_types import SomaHandInputIndex -from isaaccapture.schema import SomaHandednessV0, SomaHandJointV0 +from isaaccapture.schema import SomaHandedness, SomaHandJoint from .hand_pipeline import HandViewLayout, HandViewPipeline @@ -46,7 +46,7 @@ joint_names=tuple( name for name, joint in sorted( - SomaHandJointV0.__members__.items(), key=lambda item: int(item[1]) + SomaHandJoint.__members__.items(), key=lambda item: int(item[1]) ) if name != "NUM_JOINTS" ), @@ -60,8 +60,6 @@ def create_layer(data_root: Path, hand_type: str): import soma import torch - if soma.__version__ != "0.3.1": - raise RuntimeError(f"The POC requires SOMA-X 0.3.1, got {soma.__version__}") layer = soma.SOMAHandLayer( data_root=str(data_root), hand_type=hand_type, @@ -79,7 +77,7 @@ def create_layer(data_root: Path, hand_type: str): ] actual[0] = "WRIST" if actual != [name.upper() for name in expected]: - raise ValueError("SOMA hand layer joint order differs from the V0 FBS") + raise ValueError("SOMA hand layer joint order differs from the FBS") with torch.no_grad(): layer.prepare_identity( torch.zeros((1, layer.identity_model.num_identity_coeffs)) @@ -100,14 +98,14 @@ def create_soma_hand_pipeline( left = SomaHandSource( "hand_left", left_collection_id, - SomaHandednessV0.LEFT, + SomaHandedness.LEFT, layers[0], representation=representation, ) right = SomaHandSource( "hand_right", right_collection_id, - SomaHandednessV0.RIGHT, + SomaHandedness.RIGHT, layers[1], representation=representation, ) diff --git a/examples/soma_body_publisher/README.md b/examples/soma_body_publisher/README.md index 50d44babd5..60d6fb7383 100644 --- a/examples/soma_body_publisher/README.md +++ b/examples/soma_body_publisher/README.md @@ -41,8 +41,8 @@ uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ Joint rotations are the default. To evaluate the animation before transport, add `--body-representation joint-poses`. Configure the consumer with the same -representation. The two modes publish `soma_body_joint_rotations_v0` and -`soma_body_joint_poses_v0`, respectively. +representation. The two modes publish `soma_body_joint_rotations` and +`soma_body_joint_poses`, respectively. The assets directory must contain `example_animation.npy` and the neutral SOMA identity assets. Add `--validate-only` to process every bundled frame through diff --git a/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp b/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp index e20e2665d9..835af293a9 100644 --- a/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp +++ b/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp @@ -5,8 +5,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -92,7 +92,7 @@ int main(int argc, char** argv) const auto arguments = parse_arguments(argc, argv); const bool joint_poses = arguments.representation == BodyRepresentation::JointPoses; const size_t max_flatbuffer_size = joint_poses ? 4096 : 2048; - const char* tensor_identifier = joint_poses ? "soma_body_joint_poses_v0" : "soma_body_joint_rotations_v0"; + const char* tensor_identifier = joint_poses ? "soma_body_joint_poses" : "soma_body_joint_rotations"; std::unique_ptr session; std::unique_ptr pusher; size_t frames = 0; @@ -112,8 +112,8 @@ int main(int argc, char** argv) throw std::invalid_argument("Truncated SOMA body demo payload"); } flatbuffers::Verifier verifier(bytes.data(), bytes.size()); - const bool valid = joint_poses ? verifier.VerifyBuffer(nullptr) : - verifier.VerifyBuffer(nullptr); + const bool valid = joint_poses ? verifier.VerifyBuffer(nullptr) : + verifier.VerifyBuffer(nullptr); if (!valid) { throw std::invalid_argument("Invalid SOMA body FlatBuffer"); diff --git a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py index 3afaf1d918..5ebd9d6a90 100644 --- a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py +++ b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py @@ -17,11 +17,11 @@ from isaaccapture.schema import ( Point, - SomaBodyJointPoseArrayV0, - SomaBodyJointPosesV0, - SomaBodyJointRotationArrayV0, - SomaBodyJointV0, - SomaBodyJointRotationsV0, + SomaBodyJointPoseArray, + SomaBodyJointPoses, + SomaBodyJointRotationArray, + SomaBodyJoint, + SomaBodyJointRotations, ) logger = logging.getLogger("isaaccapture.examples.soma_body_publisher") @@ -32,8 +32,6 @@ def create_layer(data_root: Path): import soma import torch - if soma.__version__ != "0.3.1": - raise RuntimeError(f"The POC requires SOMA-X 0.3.1, got {soma.__version__}") layer = soma.SOMALayer( data_root=str(data_root), identity_model_type="soma", @@ -46,12 +44,12 @@ def create_layer(data_root: Path): expected = [ name.replace("_", "") for name, joint in sorted( - SomaBodyJointV0.__members__.items(), key=lambda item: int(item[1]) + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) ) if name != "NUM_JOINTS" ] if [name.upper() for name in layer.public_joint_names] != ["ROOT", *expected]: - raise ValueError("SOMA layer joint order differs from the V0 FBS") + raise ValueError("SOMA layer joint order differs from the FBS") with torch.no_grad(): layer.prepare_identity( torch.zeros((1, layer.identity_model.num_identity_coeffs)) @@ -86,10 +84,10 @@ def demo_controls(path: Path, layer): def soma_joint_rotations(quaternions, translation): - joints = SomaBodyJointRotationArrayV0() + joints = SomaBodyJointRotationArray() joints.rotations[:] = quaternions joints.is_valid[:] = 1 - return SomaBodyJointRotationsV0(joints, Point(*translation), True) + return SomaBodyJointRotations(joints, Point(*translation), True) def evaluate_demo_controls(layer, rotations, translations): @@ -110,11 +108,11 @@ def evaluate_demo_controls(layer, rotations, translations): def soma_joint_poses(positions, orientations): - joints = SomaBodyJointPoseArrayV0() + joints = SomaBodyJointPoseArray() joints.positions[:] = positions joints.orientations[:] = orientations joints.is_valid[:] = 1 - return SomaBodyJointPosesV0(joints) + return SomaBodyJointPoses(joints) def main(argv: list[str]) -> int: diff --git a/examples/soma_hand_publisher/README.md b/examples/soma_hand_publisher/README.md index 33fcd9cf4c..271ad08099 100644 --- a/examples/soma_hand_publisher/README.md +++ b/examples/soma_hand_publisher/README.md @@ -48,7 +48,7 @@ uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ Joint rotations are the default. To publish evaluated joint poses, add `--hand-representation joint-poses` to the publisher and `--soma-hand-representation joint-poses` to the viewer. The two modes publish -`soma_hand_joint_rotations_v0` and `soma_hand_joint_poses_v0`, respectively. +`soma_hand_joint_rotations` and `soma_hand_joint_poses`, respectively. The assets must include `SOMAHand.npz`, `example_animation.npy`, and the neutral SOMA identity assets from the v0.3.1 asset release. Add `--validate-only` to diff --git a/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp b/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp index f4194d5d1c..c11847fc6d 100644 --- a/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp +++ b/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp @@ -5,8 +5,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -103,7 +103,7 @@ int main(int argc, char** argv) const auto arguments = parse_arguments(argc, argv); const bool joint_poses = arguments.representation == HandRepresentation::JointPoses; const size_t max_flatbuffer_size = joint_poses ? 2048 : 1024; - const char* tensor_identifier = joint_poses ? "soma_hand_joint_poses_v0" : "soma_hand_joint_rotations_v0"; + const char* tensor_identifier = joint_poses ? "soma_hand_joint_poses" : "soma_hand_joint_rotations"; std::unique_ptr session; std::unique_ptr left_pusher; std::unique_ptr right_pusher; @@ -127,19 +127,19 @@ int main(int argc, char** argv) throw std::invalid_argument("Truncated SOMA hand demo payload"); } - core::SomaHandednessV0 left_handedness; - core::SomaHandednessV0 right_handedness; + core::SomaHandedness left_handedness; + core::SomaHandedness right_handedness; if (joint_poses) { - left_handedness = verify(left_bytes)->handedness(); - right_handedness = verify(right_bytes)->handedness(); + left_handedness = verify(left_bytes)->handedness(); + right_handedness = verify(right_bytes)->handedness(); } else { - left_handedness = verify(left_bytes)->handedness(); - right_handedness = verify(right_bytes)->handedness(); + left_handedness = verify(left_bytes)->handedness(); + right_handedness = verify(right_bytes)->handedness(); } - if (left_handedness != core::SomaHandednessV0_LEFT || right_handedness != core::SomaHandednessV0_RIGHT) + if (left_handedness != core::SomaHandedness_LEFT || right_handedness != core::SomaHandedness_RIGHT) { throw std::invalid_argument("SOMA hand demo payload handedness does not match its collection"); } diff --git a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py index 27e22008b9..7584698b8c 100644 --- a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py +++ b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py @@ -17,11 +17,11 @@ from isaaccapture.schema import ( Point, - SomaHandednessV0, - SomaHandJointPoseArrayV0, - SomaHandJointPosesV0, - SomaHandJointRotationArrayV0, - SomaHandJointRotationsV0, + SomaHandedness, + SomaHandJointPoseArray, + SomaHandJointPoses, + SomaHandJointRotationArray, + SomaHandJointRotations, ) logger = logging.getLogger("isaaccapture.examples.soma_hand_publisher") @@ -32,8 +32,6 @@ def create_layers(data_root: Path): import soma import torch - if soma.__version__ != "0.3.1": - raise RuntimeError(f"The POC requires SOMA-X 0.3.1, got {soma.__version__}") body = soma.SOMALayer( data_root=str(data_root), identity_model_type="soma", @@ -135,18 +133,18 @@ def demo_hand_frames(path: Path, body, hands): def soma_joint_rotations(rotations, translation, handedness): - joints = SomaHandJointRotationArrayV0() + joints = SomaHandJointRotationArray() joints.rotations[:] = rotations joints.is_valid[:] = 1 - return SomaHandJointRotationsV0(joints, Point(*translation), True, handedness) + return SomaHandJointRotations(joints, Point(*translation), True, handedness) def soma_joint_poses(positions, orientations, handedness): - joints = SomaHandJointPoseArrayV0() + joints = SomaHandJointPoseArray() joints.positions[:] = positions joints.orientations[:] = orientations joints.is_valid[:] = 1 - return SomaHandJointPosesV0(joints, handedness) + return SomaHandJointPoses(joints, handedness) def _payload(frame, index: int, representation: str, handedness): @@ -205,13 +203,13 @@ def main(argv: list[str]) -> int: frames["left"], index, args.hand_representation, - SomaHandednessV0.LEFT, + SomaHandedness.LEFT, ) right = _payload( frames["right"], index, args.hand_representation, - SomaHandednessV0.RIGHT, + SomaHandedness.RIGHT, ) process.stdin.write( struct.pack( diff --git a/src/core/deviceio_trackers/trackers.toml b/src/core/deviceio_trackers/trackers.toml index 94f862f669..4bb4783995 100644 --- a/src/core/deviceio_trackers/trackers.toml +++ b/src/core/deviceio_trackers/trackers.toml @@ -29,35 +29,35 @@ max_flatbuffer_size = 256 facade_tensor_constant = true [[tracker]] -name = "soma_body_joint_rotations_v0" -table = "SomaBodyJointRotationsV0" -schema = "soma_body_v0" -class = "SomaBodyJointRotationsV0Tracker" -traits = "SomaBodyJointRotationsV0RecordingTraits" +name = "soma_body_joint_rotations" +table = "SomaBodyJointRotations" +schema = "soma_body_joint_rotations" +class = "SomaBodyJointRotationsTracker" +traits = "SomaBodyJointRotationsRecordingTraits" max_flatbuffer_size = 2048 [[tracker]] -name = "soma_body_joint_poses_v0" -table = "SomaBodyJointPosesV0" -schema = "soma_body_joint_poses_v0" -class = "SomaBodyJointPosesV0Tracker" -traits = "SomaBodyJointPosesV0RecordingTraits" +name = "soma_body_joint_poses" +table = "SomaBodyJointPoses" +schema = "soma_body_joint_poses" +class = "SomaBodyJointPosesTracker" +traits = "SomaBodyJointPosesRecordingTraits" max_flatbuffer_size = 4096 [[tracker]] -name = "soma_hand_joint_rotations_v0" -table = "SomaHandJointRotationsV0" -schema = "soma_hand_v0" -class = "SomaHandJointRotationsV0Tracker" -traits = "SomaHandJointRotationsV0RecordingTraits" +name = "soma_hand_joint_rotations" +table = "SomaHandJointRotations" +schema = "soma_hand_joint_rotations" +class = "SomaHandJointRotationsTracker" +traits = "SomaHandJointRotationsRecordingTraits" max_flatbuffer_size = 1024 [[tracker]] -name = "soma_hand_joint_poses_v0" -table = "SomaHandJointPosesV0" -schema = "soma_hand_joint_poses_v0" -class = "SomaHandJointPosesV0Tracker" -traits = "SomaHandJointPosesV0RecordingTraits" +name = "soma_hand_joint_poses" +table = "SomaHandJointPoses" +schema = "soma_hand_joint_poses" +class = "SomaHandJointPosesTracker" +traits = "SomaHandJointPosesRecordingTraits" max_flatbuffer_size = 2048 [[tracker]] diff --git a/src/core/schema/fbs/soma_body_common_v0.fbs b/src/core/schema/fbs/soma_body_common.fbs similarity index 98% rename from src/core/schema/fbs/soma_body_common_v0.fbs rename to src/core/schema/fbs/soma_body_common.fbs index d391830914..3e1a65baed 100644 --- a/src/core/schema/fbs/soma_body_common_v0.fbs +++ b/src/core/schema/fbs/soma_body_common.fbs @@ -6,7 +6,7 @@ namespace core; // User-facing SOMA-X body pose order. The public skeleton has 78 joints, but its // virtual Root is always identity and is not part of transported body arrays. // Hips is index 0, matching SOMALayer's 77-entry public pose input. -enum SomaBodyJointV0 : uint8 { +enum SomaBodyJoint : uint8 { HIPS = 0, SPINE1 = 1, SPINE2 = 2, diff --git a/src/core/schema/fbs/soma_body_joint_poses_v0.fbs b/src/core/schema/fbs/soma_body_joint_poses.fbs similarity index 62% rename from src/core/schema/fbs/soma_body_joint_poses_v0.fbs rename to src/core/schema/fbs/soma_body_joint_poses.fbs index 8392a2757e..d20f302015 100644 --- a/src/core/schema/fbs/soma_body_joint_poses_v0.fbs +++ b/src/core/schema/fbs/soma_body_joint_poses.fbs @@ -2,34 +2,34 @@ // SPDX-License-Identifier: Apache-2.0 include "pose.fbs"; -include "soma_body_common_v0.fbs"; +include "soma_body_common.fbs"; include "timestamp.fbs"; namespace core; -struct SomaBodyJointPoseV0 { +struct SomaBodyJointPose { pose: Pose; is_valid: bool; } -// Fixed-size evaluated SOMA body joint-pose array in SomaBodyJointV0 order. -struct SomaBodyJointPoseArrayV0 { - values: [SomaBodyJointPoseV0:77]; +// Fixed-size evaluated SOMA body joint-pose array in SomaBodyJoint order. +struct SomaBodyJointPoseArray { + values: [SomaBodyJointPose:77]; } -// Evaluated SOMA full-body joint-pose transport schema v0. +// Evaluated SOMA full-body joint-pose transport schema. // // Defined and verified against SOMA-X v0.3.1 and its v0.3 pose interface. // Every position and orientation is global within the payload reference frame. // Positions are meters in a right-handed frame with +Y up and +Z forward. // Orientations are unit XYZW quaternions. The virtual Root is omitted. -table SomaBodyJointPosesV0 { - joint_poses: SomaBodyJointPoseArrayV0 (id: 0); +table SomaBodyJointPoses { + joint_poses: SomaBodyJointPoseArray (id: 0); } -table SomaBodyJointPosesV0Record { - data: SomaBodyJointPosesV0 (id: 0); +table SomaBodyJointPosesRecord { + data: SomaBodyJointPoses (id: 0); timestamp: DeviceDataTimestamp (id: 1); } -root_type SomaBodyJointPosesV0Record; +root_type SomaBodyJointPosesRecord; diff --git a/src/core/schema/fbs/soma_body_v0.fbs b/src/core/schema/fbs/soma_body_joint_rotations.fbs similarity index 68% rename from src/core/schema/fbs/soma_body_v0.fbs rename to src/core/schema/fbs/soma_body_joint_rotations.fbs index 619ed32f2f..38b9a2cda9 100644 --- a/src/core/schema/fbs/soma_body_v0.fbs +++ b/src/core/schema/fbs/soma_body_joint_rotations.fbs @@ -2,18 +2,18 @@ // SPDX-License-Identifier: Apache-2.0 include "point.fbs"; -include "soma_body_common_v0.fbs"; -include "soma_common_v0.fbs"; +include "soma_body_common.fbs"; +include "soma_common.fbs"; include "timestamp.fbs"; namespace core; -// Fixed-size SOMA body joint-rotation array v0 in SomaBodyJointV0 order. -struct SomaBodyJointRotationArrayV0 { - values: [SomaJointRotationV0:77]; +// Fixed-size SOMA body joint-rotation array in SomaBodyJoint order. +struct SomaBodyJointRotationArray { + values: [SomaJointRotation:77]; } -// SOMA full-body joint-rotation transport schema v0. +// SOMA full-body joint-rotation transport schema. // // Defined and verified against SOMA-X v0.3.1 and its v0.3 pose interface, // with reference_pose=None, absolute_pose=False, and its public joint order. @@ -23,8 +23,8 @@ struct SomaBodyJointRotationArrayV0 { // root transform must be composed into Hips before publishing. Rotations are // unit XYZW quaternions. Global translation is meters in a right-handed // reference frame with +Y up and +Z forward. -table SomaBodyJointRotationsV0 { - joint_rotations: SomaBodyJointRotationArrayV0 (id: 0); +table SomaBodyJointRotations { + joint_rotations: SomaBodyJointRotationArray (id: 0); // Translation applied to Hips in the reference frame. Ignore this value // when global_translation_is_valid is false. @@ -32,9 +32,9 @@ table SomaBodyJointRotationsV0 { global_translation_is_valid: bool (id: 2); } -table SomaBodyJointRotationsV0Record { - data: SomaBodyJointRotationsV0 (id: 0); +table SomaBodyJointRotationsRecord { + data: SomaBodyJointRotations (id: 0); timestamp: DeviceDataTimestamp (id: 1); } -root_type SomaBodyJointRotationsV0Record; +root_type SomaBodyJointRotationsRecord; diff --git a/src/core/schema/fbs/soma_common_v0.fbs b/src/core/schema/fbs/soma_common.fbs similarity index 80% rename from src/core/schema/fbs/soma_common_v0.fbs rename to src/core/schema/fbs/soma_common.fbs index 472ddadea4..ae094b25c2 100644 --- a/src/core/schema/fbs/soma_common_v0.fbs +++ b/src/core/schema/fbs/soma_common.fbs @@ -6,7 +6,7 @@ include "quaternion.fbs"; namespace core; // Hand side shared by the SOMA pose transport schemas. -enum SomaHandednessV0 : uint8 { +enum SomaHandedness : uint8 { UNSPECIFIED = 0, LEFT = 1, RIGHT = 2, @@ -14,11 +14,11 @@ enum SomaHandednessV0 : uint8 { // One SOMA rotation and its tracking validity. // -// Versioned body and hand profiles define the joint order and reference frame. +// Body and hand profiles define the joint order and reference frame. // Valid rotations are unit XYZW quaternions. q and -q represent the same // rotation; no quaternion-sign convention is required. Ignore rotation when // is_valid is false. -struct SomaJointRotationV0 { +struct SomaJointRotation { rotation: Quaternion; is_valid: bool; } diff --git a/src/core/schema/fbs/soma_hand_joint_poses_v0.fbs b/src/core/schema/fbs/soma_hand_joint_poses.fbs similarity index 57% rename from src/core/schema/fbs/soma_hand_joint_poses_v0.fbs rename to src/core/schema/fbs/soma_hand_joint_poses.fbs index a2d7d4133b..663117d149 100644 --- a/src/core/schema/fbs/soma_hand_joint_poses_v0.fbs +++ b/src/core/schema/fbs/soma_hand_joint_poses.fbs @@ -2,36 +2,36 @@ // SPDX-License-Identifier: Apache-2.0 include "pose.fbs"; -include "soma_common_v0.fbs"; -include "soma_hand_v0.fbs"; +include "soma_common.fbs"; +include "soma_hand_joint_rotations.fbs"; include "timestamp.fbs"; namespace core; -struct SomaHandJointPoseV0 { +struct SomaHandJointPose { pose: Pose; is_valid: bool; } -// Fixed-size evaluated SOMA hand joint-pose array in SomaHandJointV0 order. -struct SomaHandJointPoseArrayV0 { - values: [SomaHandJointPoseV0:25]; +// Fixed-size evaluated SOMA hand joint-pose array in SomaHandJoint order. +struct SomaHandJointPoseArray { + values: [SomaHandJointPose:25]; } -// Evaluated SOMA hand joint-pose transport schema v0. +// Evaluated SOMA hand joint-pose transport schema. // // Defined and verified against SOMA-X v0.3.1 and its v0.3 pose interface. // Every position and orientation is global within the payload reference frame. // Positions are meters in a right-handed frame with +Y up and +Z forward. // Orientations are unit XYZW quaternions. -table SomaHandJointPosesV0 { - joint_poses: SomaHandJointPoseArrayV0 (id: 0); - handedness: SomaHandednessV0 = UNSPECIFIED (id: 1); +table SomaHandJointPoses { + joint_poses: SomaHandJointPoseArray (id: 0); + handedness: SomaHandedness = UNSPECIFIED (id: 1); } -table SomaHandJointPosesV0Record { - data: SomaHandJointPosesV0 (id: 0); +table SomaHandJointPosesRecord { + data: SomaHandJointPoses (id: 0); timestamp: DeviceDataTimestamp (id: 1); } -root_type SomaHandJointPosesV0Record; +root_type SomaHandJointPosesRecord; diff --git a/src/core/schema/fbs/soma_hand_v0.fbs b/src/core/schema/fbs/soma_hand_joint_rotations.fbs similarity index 74% rename from src/core/schema/fbs/soma_hand_v0.fbs rename to src/core/schema/fbs/soma_hand_joint_rotations.fbs index d00a90e168..8bbe270598 100644 --- a/src/core/schema/fbs/soma_hand_v0.fbs +++ b/src/core/schema/fbs/soma_hand_joint_rotations.fbs @@ -2,14 +2,14 @@ // SPDX-License-Identifier: Apache-2.0 include "point.fbs"; -include "soma_common_v0.fbs"; +include "soma_common.fbs"; include "timestamp.fbs"; namespace core; // SOMAHandLayer's 25-entry public pose input order. These are the wrist and the // 24 finger joints that form a strict subset of the SOMA-X body skeleton. -enum SomaHandJointV0 : uint8 { +enum SomaHandJoint : uint8 { WRIST = 0, THUMB1 = 1, THUMB2 = 2, @@ -39,12 +39,12 @@ enum SomaHandJointV0 : uint8 { NUM_JOINTS = 25, } -// Fixed-size SOMA hand joint-rotation array v0 in SomaHandJointV0 order. -struct SomaHandJointRotationArrayV0 { - values: [SomaJointRotationV0:25]; +// Fixed-size SOMA hand joint-rotation array in SomaHandJoint order. +struct SomaHandJointRotationArray { + values: [SomaJointRotation:25]; } -// SOMA hand joint-rotation transport schema v0. +// SOMA hand joint-rotation transport schema. // // Defined and verified against SOMA-X v0.3.1 and its v0.3 pose interface, // with reference_pose=None, absolute_pose=False, and its public joint order. @@ -53,19 +53,19 @@ struct SomaHandJointRotationArrayV0 { // joint-local relative to that fixed reference pose. Rotations are unit XYZW // quaternions. Global translation is meters in a right-handed reference frame // with +Y up and +Z forward. -table SomaHandJointRotationsV0 { - joint_rotations: SomaHandJointRotationArrayV0 (id: 0); +table SomaHandJointRotations { + joint_rotations: SomaHandJointRotationArray (id: 0); // Translation applied to Wrist in the reference frame. Ignore this value // when global_translation_is_valid is false. global_translation: Point (id: 1); global_translation_is_valid: bool (id: 2); - handedness: SomaHandednessV0 = UNSPECIFIED (id: 3); + handedness: SomaHandedness = UNSPECIFIED (id: 3); } -table SomaHandJointRotationsV0Record { - data: SomaHandJointRotationsV0 (id: 0); +table SomaHandJointRotationsRecord { + data: SomaHandJointRotations (id: 0); timestamp: DeviceDataTimestamp (id: 1); } -root_type SomaHandJointRotationsV0Record; +root_type SomaHandJointRotationsRecord; diff --git a/src/core/schema/golden/soma_body_common_v0.bfbs b/src/core/schema/golden/soma_body_common.bfbs similarity index 90% rename from src/core/schema/golden/soma_body_common_v0.bfbs rename to src/core/schema/golden/soma_body_common.bfbs index 550cdd05a953d0c3b5343b365f84c71c58ea81fb..5a51d63c6aef3802b8e487d6b96d44704c305d06 100644 GIT binary patch delta 360 zcmWO2Axr`R7{&3&amWFO9&Rwg6*no&F~S9>g+MSE2nHf!j1dTI6oVTC;)P=p8N-cW zuxT(bW+WIc7zhL-kzgPY{D!NVabEt48($Weub z?}QZK0eNgK=@ngE;~3{LMySBV`b6?kgN-mo5Y*vfL**$NxWq0hy`cv$dP@?6BzQy> c4z^4-M+;>fnQaXpHMlr`zA&=yH^$8Q2j<8%6aWAK delta 385 zcmWO2AxHyp6vy$Oo4RwJ`X9_77_JP&Fb-L`V3LI)2EieQfv8g^Oi%O>1YrvX<9}dS zk_ih2gJ2Oc7#0^6E-V-pj7EdO;5U4DuRPxGy+Ll6vo>SLq)kZy#<9?Pn0IzB{ru7X zg<5aRjrWYyL=gtDkvga&fQ#Xr)I|*i{LV@}++qU;@r=|)1s=X!=@E7K_?ebo5h1`| zTKY(CeefQn_-mzAZj_31z z@80vBd(OFkde;+?k5!bzw^S`j#s5h5YSG@m_wmGCyBgxb(Z*OJv059NK*wT_$IiX7a}T zWP$DTh#eRLI)FNoTLaz$l0X#L?G$+c6oG!=@Jk{)z$%adVnF?iBJ02npc{A!$2WlY zfL`F>Rgo>=4v++1yn?=g4}laA1)h9CZPay(`$i=|65NdE}3f3-5VP{lkxI6h`OLY4`DQv2KSG0d2}BTUI&pU7XG9Lwe~ z^JCEWP=_>|u|;8|t~f}iOpjg3%=F{@v0sy~w%%Od{2hw*2ueog6PSgrl>^c6hEpzon%{Csy( zR~+VYJUyzzmz3g-Iugn#>RtTITfp9F~-doTsD&8+G@44b(3)k^Y zk7ciz)YKGj(DD+fb9rID{dsP3N*>c9yNdS<%e&c{$FxZ7FpTf#mbc!T$FxXF@qT7` zPg?Vs7Q^#UGL8YjNfh>6j5v~Xj1yz2)VSL}wpG7BwPgC@xZK}Veta%6bf|bgvAmUb zJY=Z%A7OreYx>_3^{To-je!Nq5+wkbus^aw-7Um-ATKP%8$n#J# zKbHU}8O3eLPcCzPV#MX=NcH<8OQy}2^lP|`Gx=GVpI*z`Zo^}oWySlU4qR#mlo_E{_Fji>upvSy?-|Dltr8wcaIyyO>&JR!HppU%>5_K*Q^rL}i zmfODKB`mMpnrD_<_rJn?e8=)OTJy{@%PHQsEpM+i&n&Yw#e2i@;;3`+T|iwJUvNwa zc^=vrKT9W5ii@AT4cw$WQoL_D63Y86dGE-P;=QhTJ1u!{OY+~LpWjrx7cF^jNm=pu zyVOZP>RoQQ&cocCI3{EtdhYk^H!PVp9rt__E+RS29iStPD@F`|wT=7n>r|^h4QapZ#QC1c224< z>#3}G952dYYo1w88;ZwRD7~n2d1>OOSxDjwG)KNe&QZ60*!8^0@@EGtg% zy%8P@lK5Yk4}Ql`HWerM9`KI^>4v@s9rlCIO6doF`Rejq>9dVeRr>gpmaD_RmH2TOFFwzW)RZ2%ne=eV{f>x1-$Nba z#pin`tPLPj0*>2`19lN zxd{-sA9NXeKASg3&K_-2KQlHp!&b)P;$z!m{G-3OfF6Kn%jfSkrYcLNrRfqHifH$+ zE>T1x7%}d(wEq$IA{1r`(k|yzb#A`2R4vTk!Sj9eE0K;s|C!ZiUYw7`LbZTb#B=iY zi_tMg_Qm{CzYG6Z4g%b_U*a?FYP#&`=-CB=oH6$}+NTviu1?M$+UENEJ+F20cNo9f z-`T|cF~5xa0RmlRC6EAkMmG0lcyY0C&+JW9XRiHm>w=v#@R9!U_EY41=Y+9M`>efN zD8G;HrNicZ2zC7c+dW&dUws#-{5BVY9OD#F1$bUH_Yn>9I{cy^XW>($yn~wa;u_ro$^iALYhp{4?#|7YQkYUXH4Wtv?4Lq(&gKf|C8jI;p)VZUB~00JDbJ4b zi-iyJMPn*ITdvIh0nHLMdvP?dvD09KInp-3E0pc+th3S$8$Hl-bHMGD{tY;L34MA4 zFNM2>xpLvQbE}z2bJjpL!nxrA%q!zz$ups^k6HHbhnf2%@FBppYI4S9j%VJya|T2W z3l)K!GxqR`wa;D5x4q@Z-^sCH4!LeBi*u!gYN0w;SpdhaK{T9@&|YIMI38|2;GvUt z3DzTuIb#^l7yZ5?LwKeQ#_HB4%f7xpzF(-878l@%AGxoSK-(Hn1h|KAZkZhXCQz3H z%-hq5lVe1?JTpU{YCFFfx~!7Hcpv_a=j&>4VE1Qwi5hwpP>=N@!=QRmtd4eiV9 z2U+CYIEUz{$C>Vz5=r9C1L!07N0uA|jyHC%V4cuT1|W~~!H&7Px86gaI*x}8J}?e{ nPVoFWYx>bKecYhWnCLIhB92)CV81-4++3#y#|RSSf!`_ujelM(;&MWNdtF295N{fSi#o2}=amy%Lo^v?3!w92fxlfW+$p4Ur5G0zOSR zwhMFtTb~y>=oLBqg2?_CVH1Y8fyc0QbV1}CbXI_y0PPL~P=`MvPyWzotb@;M$`0;p z;PU=7@Dw)hqOC$_A7f=)JKaOvKfrwMUjEo&XSg&TI(s zL5ZM8?gI%R0@RV<8c+s?fiUpATVw;c4a9+$pF=KyH6Q~-fcj@e)`1&96gYtM zkAN~T1RTAJd;wKp7&v|f>j10*DIg3y`;5r%f%kzvU?0wJ0*k;U1_JCLs5Kw~bOF0q zz=uE%hyeAo@B_F3M1eg-Tmy>00Pq3}_ZX-G!@w~XV;fijQa}%|kK}9sw}Chl1?-}E z2;_h$fHIKT%2Mg#43=OFD{!?kw@`g=2;H*x=SG9|im+vZ9grIOtSLdLlD$5WzdD)D z=CA{*hmA%QeMq&bK{Q6{ii4EOboxpvm(NWm^T})i_nV4$&7+wAVf4GXur!J{Hae3; ztQdFxUA4Rs#i4)RT;r3I2$=IY{j}9zY^gt!N@o$j(ob3an$pM8TKk#g6`qv=-F{{s}t~jhWZ%tC8*+hOSoxS#fEd$Ab@BACJ{0RD7 z4zR61!HuJDRq;kFFV%tf4)nGZ?_JBg-+}iw^o|to=a%=h1Me;94gM{N?`M{GsCamk z>-eVA*()YBMa8>hc?0NkdEtIjIVF#2k&hMcr z4K=S>DCpi++@^hHxb$yN@ytS@uhxM_|9bur#5sg}%JU99`jt_1B|Y?^t~{Ma2ou)$z&cWPWrS2Yp@5Ghlh^iW8iB zMqSEko^Pvpb`>X>XC^gyJ(EaI@e zipRQf5=Xy_kNZ5x&52_|HlgS0?JJhNsJPtc&0H4Ff#QAH@*Z{Iv2gnS6U@i=9LjzN z9t$U@czC5U(u+P9V=F&dI1dyLuVhB16z}AI3XX^!#lx$eku}9Txu5*wL5>xV=QCxy zJa5UcPvQX|API__X;N~ zisQd;1jmBBQ1kFThVoc({PzLxSddjU4_=jhIZzybp5R!J9C&V@@=T-jqR-_aIDe8K z@V$rgIDL*$QcB* zzSI@xjOB1}L+PLOwQ+8v-^I%QZ1VCo7DyTWE>7B^47Jc@QH1gJ=ZtxC=l3Y@JYX#{ zHYc>F=ee9p&g3vySM%UAf{}Hl%h+b{=9Yw0Ve^%TGkEhj|7e%;tQ8LqhVkdc;PH4K z^H~rv-jd0WJYD82b3U5frKe^%c0>8d@n`r)UvB~f0MD^ce%F|)ESHww8=5X*s3<+y zlTZtV@b!$(S=!&ka|n&uezePas?N=qmaB#NMO?39UJ0G?_4lkkb3%REC>E*(G>GTq z7cfT07&#X+M*Uv=V><%yjQCWaG1dpZmn~gg7?88(yQ221>c!Sge`ufk@7MgalV4;G zWX1D4#PwobP_Kqy*U(a*Ipo>g+Mm&-rNTY)k*EuMnH1`K>lcBov+$EX@=K`5Yuyv* z@#bLfokICTxPTwf9yfu@xE+t$mcJ+TcR}U5|8bD(O>PxnPFwp*H{XD7^yM7`R_!7Hb;-=y<9=nO+A1Pe}wXBN7BJNIxih(0&o z-!y)g^B{|?jq`|(daQJ>l~5daQGh=3S;>}b!1cyc%-<)plLE+NJ=ise3cg=pP94Xi rCLb7wR}(yo&Y5|1OwXJ2858~Gxx_W=2RJX!ELZE)u(j^zfU+ diff --git a/src/core/schema/golden/soma_body_v0.bfbs b/src/core/schema/golden/soma_body_joint_rotations.bfbs similarity index 69% rename from src/core/schema/golden/soma_body_v0.bfbs rename to src/core/schema/golden/soma_body_joint_rotations.bfbs index 5abf66cbefb785bb0cbf3a66b871a37ac538d779..c8475650ba38b528ec67efb635e80badd0f16cfd 100644 GIT binary patch delta 1054 zcmYk)O-R#W9KiA4f3EUn=1lh@DU)8nWWIoAg9?^NX$Z}igSYuIL&NzOXJAmcgMkMR zjmK^V9tt{mDD}``2M-+fB)a}OLpZhS2c=A z_s#Ah9#Sj8RF$W$m)8)2XoSDk42eWi*8{1D^OA%24NGK)=PAs1MKLp+~XSU zFzbr#e%e9S6x+>O>rVD*6e*WdUUF=bS1p#WGD$0fdeo=^%j=MJ6}NmXqd&AZ@fasC z*mtXUn5Xln)@HH`X3YFn)oZp&vw|-zoccvTGdw5k z`N`HHa@$R6Z`;$e3)@>ITkf(W*07Dl)P3*8ip1a~6JxYX)w9|T>#O=ud&gsnix^}D zP1wU(^~l>$laTZ;tsPVi^;NjUw`$rGwKD3RXUM8o4c_y8O?j;uw8H!quiDAy^Mw+? z@hiV)0*lVu3u6n@_H4r3eW8rZsR2$r=a4=1!h6-qsxRJX^(I?8#QpE^9W~(Vs6M3E zIu=q+J@s`|I)7_j3^JrX`L5i^)5E@2h(B|H38+}1)K@XE|7A#@NmUOv;8g?uIy$yz H{A-Rs)r1h3RUm1}TJ$T459_;=!1Iwoq_;rHvpc^ss|L zhmF_3qk#vB4wfFe1RgvXcqmxV!N5axvYUqyi?{EyHnG9S_kEt{_dL(<{XLJxE3;P{ zbrI>g+0)NM+~UtSdYU7=hTumFbhn6;XK%i{H9ulMyQGP%(LRifSLA&&nnX4|BJ;!+ zPKeCqpSR9z|FEn90_+5;-!AQG-@i6%NR~g0(c}izznk{2vM*K-^+(;&kRP1q}g zOdy2fF_8_-p%+dR8blT_3?1LecNtR%VDBi0h77_eIYibSG9y`DqNvBGBUA%JXu%Ez zeS>Lqsp4XTYcPih>abNSlEVO;C{Vlw3`55@C$)$;{MfA#SwRNj8Y*0}kqMq63O9Br zq`Gz!2tieTi$@OCBUPo}Khhq`d&iheWz>nTzOH%abQPJyQW8fy+RUW;lw&Z^mta<0 z?B+{%P}?y-y6<_^1tcALQ3>7Vc~XUY%NJz*w`JM*c6ELE0@*Ylcw*X?`N7k#>88uu z*4e^+VVyP^Og5H3xD&*T+1wJ56&R7j*5?s~t#X&&>OR4f4mbs&OInv+vqSksj99x+K}knbFLoks1uF#+&ACe*u=k+Kd1I diff --git a/src/core/schema/golden/soma_common_v0.bfbs b/src/core/schema/golden/soma_common.bfbs similarity index 50% rename from src/core/schema/golden/soma_common_v0.bfbs rename to src/core/schema/golden/soma_common.bfbs index a9326fca3b2a8ab9209e8337005dd2aa6cc22ad8..5ac03e2f95f3158d2aed0d9d2ba255074beb543e 100644 GIT binary patch delta 268 zcmZqRc)>m)Lbrv9fgymAfdNP{$N(`55byc(|9=Ef48#Uf3^hQ>3qY}wiLLsKA``c2 zXHEf%MgTELhcFN)=NF~w1?T4`dL-thq^9Jh78f&I`TzevNS+6XL8gPi44@duL=a{L z;vi3Vj}Rcu!yp2*4WyC-Ov3arPL5?vj(PI$|9_wqqXm%A0Ai54M4+@Fkk;2P23j4T woS&PUpQo3WR16dY+2{hq3xHS*h~X~u%FoOz3Cb@?EXmBzV_?{Es|7^VOvcL2qDCbsG`YE0a! zt-1mzngPT*Kr9Nx$@xX8dcpa*i5`i0DXA%Wsl~-%29tFerENjxg20sj|Nn!`0%2Al z4)S#O2m#VO3=%+lcz`SpFsT4!2>>xj9m8Z##$?wA|Nj363D^J$kbNLmi6GJX`o%zt p~^v=>ec#>xKKp#` zzLznxLx&HYptBwuw07&XE=%*=YZ>dKHyZ~dU=U=$tu5l36+sHTs%>I&4P5Rp zb1Q9j_jR+CZnOTaX1{bY2a3pjy4~#BHnaPTFTZBSklg@Fpbky}>Cy|@!Rl}8>)#_& z&#wKxzJ3Q8dT#?2_F6*D44gjZ?z4~PgjsZ`Fn1mP2=k-NpW8OLo2?}xwF*RL_30c>e|*<Ka?H4v4Y;y5H-7T?q1fK#Kt4xM(k=#TB;Tz;|HlEJ<0)x3Tk+AqXT4#s^!4pQ zD?a*n!dKl&_vZmewtqG~aU#b_*Tbx06XjRhoqejc4A6L;KwPH2cx?ARQZM^%fFc+G zs`m|NW~sPPs@0II9VA`%Wpuu(_x4^#M=v+3#UtZ6=DWDL!gU$ORKw!>c0X7v8;^}0 z)>77Xu~seGv!5sVA?-BKxO~!?$dgRf%gMp^kmAjiv;9c}NQ@_UE7@0?6zLEjK#oe5{S{^C&DgC^b5 zf4Mc?ikozk55Lw`nLjH(SZ{2YoDQ^8W(zACglS?_k4M}=3*;-lWY=HH|1I$waP^vt;l^$m)I~Ky>~00#Fw7(7@f_K(_u_Aki?Uq zL9iRVsM~b4UZ|I=3ys_xh;-VJd$;3#5#jc2&b<}Rh%on7W4?G^PoH}|O(%yUtQwOK zRo~(l`H^kQK)J2D(M0Q*CpOqUV%}!koV}7M$+{2ZSABCUmuRod)ikWP%|#zmn~h_6khx(XpRPex{{eV0uamVdwQA+<{logxcNIga5q|oCZ1Vn>T|-03N$2#^9irT*0P)3j`ejS!%X6hH z$6PKr8r}EeCN%Ieb>FgNtM{Girp7T^Ddn|Mez9;NUv#4KbCv4cH%!|AI;POf%Xro# zV^@<5){!D})%p4Af|sH*+OHp>URjdpu$z~{`9ir;IPKMFCBl8HK~0VDBVWsx^0#_@ z+r}}CHg?{M&Vej*SHOld%H2J_ewegf$Wp92TnseQXxo@oOEKT(_V#i1-(1l@TSrRj zMLtf7gXh@p}{LrnKD&NHSS;(XNR-;SlT+RU5sd%Z^Ju@_<6EHGV=iM;440QIm zoY2+Ev;JwzvQD>(fuwDTYKuKmFnoNLoAgrNRu{{qh3LR!jjz>2Q2fx4r}%_El5M^g zcu39zjblYwC$Sj4(cYg?edm0e&1}H`fp6Dky?cpxSXh_%xy0 zDtX(6ojzyutqN_sd@o!8`d-u-=sAy?IN!lh98>J-Iqi|oiJVJt1{w47{Y~>1wGUoY z&TPRt@l?rCZ>eG4GC(%#L{zUBDBgT2Bu~#oHa%$K%O>foZ$QON{?@+wKJ+ypMtsjCzL1Xp1Db6-(f|Me literal 0 HcmV?d00001 diff --git a/src/core/schema/golden/soma_hand_joint_poses_v0.bfbs b/src/core/schema/golden/soma_hand_joint_poses_v0.bfbs deleted file mode 100644 index 36ddf85e290a045341fd7783ae5d490273a53ac5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 4632 zcmaKvKX6n<9LM*PBSeTM;Hg1T&+&>YG93N)gAXX#9Nly?wVi4tM5z``+$%fBXBp z`z~pWIey~!1f5Bkw27Ne(`9;i?lnnsklvUq$bd9B2=4ASFpN15V&FxsjfqXL*kO=q z%ql+*Sh)U*F>Bq(=`^PAC1lZ0BQwtYtv$wUFt+-#F$|eo;5w*-b6^nkf;d?Jb$fe> zHF{S2h4=Nn!FxOJi|nDT%abY3JicO~cFp4xK67Sd?4!>M z!(&;VHxDydVKlC zwOlLyVe*P`=bu+Ya^2%9#^WPnCqr}1h9`f&6HdS4tx?#u26Ii~S;O9a9{*7bKIWRN z$CpoC>tx*7>*Qmuse63VHEM$`_?TWTAFd^pN2k%%^Ks*UgfS@-s5U) zWGp*883uUOldssjcHLW}e6wo^@TSMtzOLPA#V5e2e;fXh9M_(-;uGM!$5-54>t)>8 z8|g!U?=tS}<*2bO>v3goK6m!?@zCDeo_vm>s9o{aNWNWz{u>^jBPwbut@!9ay4}!6 z`l>x_#YOixT-B`f-tsuI`?K+piM-j*S*KepIL~zIs0LMMNucp-1Yw%m;xX?0qh7YH zfN9VNbe8NmGs~sf^8CC#G`s9B%3j@cX4~g?TTPsk}=h= zxUN0$*2>P&;S*ZQs;=My3HMAfCSUYed1A;fosm4rRK1)Xn>aK4;mC=R;jDQ;&A8ZW z{ysm_`8-I2cfk&u?LBd(Hebf>@iJ2-^W&EFT`XogqPsW~jM?Kwa+O2%%1n8_UYxne zbDDikEFSSoAzyhTzU(X&>qRd>cR(FTeVG-o%oQta|h4EB=tA^xPedDT^4t|ciyp}Ba{5|mE zoN3ftS8yK0BtP(Z5At_mhis77jZqD%=C!Y1gP~tygN&zuY;k^%cqbO1GHKuAaGqqVKMc;;QeTG_PNu-1PTwEwts>KUVjaDMe zK*^s|OyUvq0lmQPD{(MsMh&V?0ZSY*h`pzQ6z5OT~J5ZWexEc`%oP zvjj>&IimWpKKLY}o2TS%J9bJh$4{vNPySEW=w8NXM?QR}2(yz8a#lKyq?G>y!^+%%SkV xi#>fiR+@B>9YId$JFvs<U+`EfEbDuE%JqQ{12bYKC%D+ diff --git a/src/core/schema/golden/soma_hand_joint_rotations.bfbs b/src/core/schema/golden/soma_hand_joint_rotations.bfbs new file mode 100644 index 0000000000000000000000000000000000000000..d5682a770fa394b3367af77d8a219f06bd4f9d32 GIT binary patch literal 3712 zcmaKvJ#Z687=~B2Fb0&^Bu)Z}hlm*!E*KQRg$s&=$Y2pr99vABnaqg8Qi2j$B9iz^ zk)fbSK|w)5;esLsMM??^3JMn#6fP(zC{j>RP%s%6o`ghs-o4w?N*4KMUhUoPyWf6) zyL(C-V~!m^wn$?p%#=x*QIj&`Jnu1SGf8Vq4orh7FbS5n8yLo%0txUY8DiiL_+-Qw zrkKqg#>~70Qt0IQF`X%4moeKIFN1k71NMLp;FMzVCqLMzeBov9S`& z1@-dDMz!8HW))jCj`?OUkesCtHP%L4H-$5&ODDg~7m8%`vBy_DB{w}jAv5#&!WZ9^ zW(zr1K?!Mn?owYITS?BSu?M~JJ(f6wD*{_gq^>pyJFZysND z=%hG^e~8%9@8a`rM84*6mEVPY;Y8%FN&eIIS21^T&zqyV)Ew3Sw8!7=$h6M~@Nw6a zJ-%Y`e{?nH6k z!Y9Eu=y(2d%vhQCxbn9+cjol5$lnJZpCic1HIFYFEg#!coUloIlIXnWECQ-gj;UCr z&$r9Y#g7brIJ-11c`=BK4}0Kh@5=rZ&rYBG|9T<6Smd;uU{v`DYi!(}f7)+pp#D+F zqv+=`>E^4IkGH@QkUiS-J?CbnT(7iR=80#o^Y3<_EerV*b4BK-tTnPEj7z_t>)t&1 zbZYjvrZF#NeRT6O^i6>A$SZN=rDE)S&>lHcSe&2zJbyetn=?zEo#GtDSlL?!2f%*N zoKU2W7`3l#>%eHg|nkuW3iJzC?8V#ppV(yiKQS63>ncCd1R=T*%~BxC++ z#Fvlat1SoZfCj&MGbS(PYX&HC;$P!ObJl_MPk~ALKBD~}AIuf*j$yOY`Wwb?!hSo1 z^)YVahl-v$(AjaGYcfi=d4P@~V(q>m);O=A49PA*Hhf=eoC|8Db~C8AYBqkQYO8c9 zs8!4Mt4H5Jqu9|a+ijiE*Av@Eq5d(aUw6q?{i;#vRD}G~LMv-5H?co+>UaOR}P^lSQE|smS(sHe_ z{3FBW6*eZY%<0(d)3L8l2lFVO<;KcNqwcimo^$hO*vm|bZP?FCa4D$Pf^*J{K`PAa zD9$tR6>G&(@m8yEjwq+$!Zupr8Ifb`4$ya4oXei`fdR7*UCPyn&4GHlvXfCsg7IOy zw#&1-3&Vc~mK5(L#W>zLJa=o{^1Pq!-jQFvZs~9}#KV}*JmpROuG)FDcL|NHcj-_} zRClh;W`DXEv@6Xzr(qNtJC=cS1C+s0pqjHW`YNWISLAz`*tl5h?t`{Vb_kb&z7KS+ zIL;q^oS)$+?+I3QoMc!Bv7W1N4$$Yub(hi=k6+O`L{YKcj(6f|2ZXIjO!Jlo@>wT| zTHPbr>;;_;@&1vXIiPtu#jL;c;s1zdtm*r4x6cOo5uSg#(?;w%zE4m4`0`2i>V8ws S6mPAoyU*2*7|IpW#aUTu=}qgG6k_u|?v{c;<4pl%PbGh$IYT zC@CmhP*Av_prD{gk%H0%1qB5~3KtX66O1L@10hP!p?l&)9&})zQ22W zPKwBdix(C!$uUVwRALgB1fItwDHE7Q^1u|329m(lQF2A*fC%uWF@(*Bz|@Ec#6%8> zbv38SO zJd;b?_MvwjYrB|L{sDf!!uk4p?axl<9=J&$2E4?d@_wg-M?;|=;sf9ujEn&sjfkhw%>_YD*a2F^6Nr3TPX*4UR78&maSCd$l`>=NcGhC44T)@$bYm~z~8_z0WK z6^fsK<4qTHc;5c2(@A1Z0Ph1jHXRwH)5>IF>+3H6Jtbq_8N@$}+&lbt9sUH)RUbnA zXGGc#pK;MKGl+j$W8~hcKBmT#(Zu7AMVN{tgF9o2mGrJm-R4P$mM3Ht;f$C z{djf4@xYPcyJ}^!vghzwb2?rO7-g<@`cF-Y(eM?Z6~W8INqy%`xKc-!IgTpgQ(_(<@vPS;=R(Xlg#j|Ar& zKJ%_)0_Uo~p*=|O9jvRrXfgBI=tmn0$8AT3`i%_ir@yKnEy|3? zgSfCgj;-G4m~9>m*@fh^?M_U>~`PYtyD3cP?}cA>R5;@f^Jflp)$idAnv%qU=&LwTnjbyQR5@5{v&B2*AWHK#$PEj| zyXBVE#n^rf{C_S!Z8rT|_WiMTM5y29BLV#f;0JwRlthk<9XG#tE>{u7>lK_8#3eF? zwG_Y{YEI}+CKFbnF^;}I0crsGZ=Hi&1_x_4ZI8%-_Yer<#qY?T{=ih8fIjAH z#N+{VXB@&VM+E!Brj|6 -#include +#include #include #include @@ -18,35 +18,35 @@ namespace py = pybind11; namespace core { -inline const SomaBodyJointPoseV0& first_soma_body_joint_pose_v0(const py::object& self) +inline const SomaBodyJointPose& first_soma_body_joint_pose(const py::object& self) { - return *(*self.cast().values())[0]; + return *(*self.cast().values())[0]; } -constexpr py::ssize_t SOMA_BODY_JOINT_POSE_STRIDE = static_cast(sizeof(SomaBodyJointPoseV0)); -constexpr py::ssize_t SOMA_BODY_JOINT_POSE_COUNT = static_cast(SomaBodyJointV0_NUM_JOINTS); +constexpr py::ssize_t SOMA_BODY_JOINT_POSE_STRIDE = static_cast(sizeof(SomaBodyJointPose)); +constexpr py::ssize_t SOMA_BODY_JOINT_POSE_COUNT = static_cast(SomaBodyJoint_NUM_JOINTS); -static_assert(sizeof(SomaBodyJointPoseArrayV0) == - sizeof(SomaBodyJointPoseV0) * static_cast(SomaBodyJointV0_NUM_JOINTS), - "SomaBodyJointPoseArrayV0.values length must equal SomaBodyJointV0::NUM_JOINTS"); +static_assert(sizeof(SomaBodyJointPoseArray) == + sizeof(SomaBodyJointPose) * static_cast(SomaBodyJoint_NUM_JOINTS), + "SomaBodyJointPoseArray.values length must equal SomaBodyJoint::NUM_JOINTS"); -inline void bind_soma_body_joint_poses_v0(py::module& m) +inline void bind_soma_body_joint_poses(py::module& m) { - py::class_(m, "SomaBodyJointPoseV0") + py::class_(m, "SomaBodyJointPose") .def(py::init<>()) .def(py::init(), py::arg("pose"), py::arg("is_valid") = false) - .def_property_readonly("pose", &SomaBodyJointPoseV0::pose, py::return_value_policy::reference_internal) - .def_property_readonly("is_valid", &SomaBodyJointPoseV0::is_valid); + .def_property_readonly("pose", &SomaBodyJointPose::pose, py::return_value_policy::reference_internal) + .def_property_readonly("is_valid", &SomaBodyJointPose::is_valid); - py::class_(m, "SomaBodyJointPoseArrayV0") + py::class_(m, "SomaBodyJointPoseArray") .def(py::init<>()) .def( "values", - [](const SomaBodyJointPoseArrayV0& self, size_t index) -> const SomaBodyJointPoseV0* + [](const SomaBodyJointPoseArray& self, size_t index) -> const SomaBodyJointPose* { - if (index >= static_cast(SomaBodyJointV0_NUM_JOINTS)) + if (index >= static_cast(SomaBodyJoint_NUM_JOINTS)) { - throw py::index_error("SomaBodyJointPoseArrayV0 index out of range"); + throw py::index_error("SomaBodyJointPoseArray index out of range"); } return (*self.values())[index]; }, @@ -56,43 +56,43 @@ inline void bind_soma_body_joint_poses_v0(py::module& m) [](py::object self) { const auto* first = - reinterpret_cast(&first_soma_body_joint_pose_v0(self).pose().position()); + reinterpret_cast(&first_soma_body_joint_pose(self).pose().position()); return strided_field_view(self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 3); }, - "Joint positions as a writable (77, 3) float32 view in SomaBodyJointV0 order.") + "Joint positions as a writable (77, 3) float32 view in SomaBodyJoint order.") .def_property_readonly( "orientations", [](py::object self) { const auto* first = - reinterpret_cast(&first_soma_body_joint_pose_v0(self).pose().orientation()); + reinterpret_cast(&first_soma_body_joint_pose(self).pose().orientation()); return strided_field_view(self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 4); }, "Unit XYZW quaternions as a writable (77, 4) float32 view.") .def_property_readonly( "is_valid", - [offset = FBS_FIELD_OFFSET(SomaBodyJointPoseV0, is_valid)](py::object self) + [offset = FBS_FIELD_OFFSET(SomaBodyJointPose, is_valid)](py::object self) { - const auto* first = fbs_field_address(first_soma_body_joint_pose_v0(self), offset); + const auto* first = fbs_field_address(first_soma_body_joint_pose(self), offset); return strided_field_view( self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 0); }, "Per-joint validity as a writable (77,) uint8 view."); - serialized_class(m, "SomaBodyJointPosesV0", "Encoded evaluated SOMA body poses v0.") + serialized_class(m, "SomaBodyJointPoses", "Encoded evaluated SOMA body poses.") .def(py::init( - [](const SomaBodyJointPoseArrayV0& joint_poses) + [](const SomaBodyJointPoseArray& joint_poses) { - SomaBodyJointPosesV0T native; - native.joint_poses = std::make_shared(joint_poses); - return pack(native); + SomaBodyJointPosesT native; + native.joint_poses = std::make_shared(joint_poses); + return pack(native); }), - py::arg("joint_poses") = SomaBodyJointPoseArrayV0()) + py::arg("joint_poses") = SomaBodyJointPoseArray()) .def_property_readonly( - "joint_poses", field(&SomaBodyJointPosesV0::joint_poses), py::return_value_policy::reference_internal); + "joint_poses", field(&SomaBodyJointPoses::joint_poses), py::return_value_policy::reference_internal); - bind_record( - m, "SomaBodyJointPosesV0Record", "SomaBodyJointPosesV0"); + bind_record( + m, "SomaBodyJointPosesRecord", "SomaBodyJointPoses"); } } // namespace core diff --git a/src/core/schema/python/soma_body_joint_rotations_bindings.h b/src/core/schema/python/soma_body_joint_rotations_bindings.h new file mode 100644 index 0000000000..81cc87242f --- /dev/null +++ b/src/core/schema/python/soma_body_joint_rotations_bindings.h @@ -0,0 +1,169 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline const SomaJointRotation& first_soma_body_joint_rotations_rotation(const py::object& self) +{ + return *(*self.cast().values())[0]; +} + +constexpr py::ssize_t SOMA_BODY_JOINT_STRIDE = static_cast(sizeof(SomaJointRotation)); +constexpr py::ssize_t SOMA_BODY_JOINT_COUNT = static_cast(SomaBodyJoint_NUM_JOINTS); + +static_assert(sizeof(SomaBodyJointRotationArray) == + sizeof(SomaJointRotation) * static_cast(SomaBodyJoint_NUM_JOINTS), + "SomaBodyJointRotationArray.values length must equal SomaBodyJoint::NUM_JOINTS"); + +inline void bind_soma_body_joint_rotations(py::module& m) +{ + py::enum_(m, "SomaBodyJoint") + .value("HIPS", SomaBodyJoint_HIPS) + .value("SPINE1", SomaBodyJoint_SPINE1) + .value("SPINE2", SomaBodyJoint_SPINE2) + .value("CHEST", SomaBodyJoint_CHEST) + .value("NECK1", SomaBodyJoint_NECK1) + .value("NECK2", SomaBodyJoint_NECK2) + .value("HEAD", SomaBodyJoint_HEAD) + .value("HEAD_END", SomaBodyJoint_HEAD_END) + .value("JAW", SomaBodyJoint_JAW) + .value("LEFT_EYE", SomaBodyJoint_LEFT_EYE) + .value("RIGHT_EYE", SomaBodyJoint_RIGHT_EYE) + .value("LEFT_SHOULDER", SomaBodyJoint_LEFT_SHOULDER) + .value("LEFT_ARM", SomaBodyJoint_LEFT_ARM) + .value("LEFT_FORE_ARM", SomaBodyJoint_LEFT_FORE_ARM) + .value("LEFT_HAND", SomaBodyJoint_LEFT_HAND) + .value("LEFT_HAND_THUMB1", SomaBodyJoint_LEFT_HAND_THUMB1) + .value("LEFT_HAND_THUMB2", SomaBodyJoint_LEFT_HAND_THUMB2) + .value("LEFT_HAND_THUMB3", SomaBodyJoint_LEFT_HAND_THUMB3) + .value("LEFT_HAND_THUMB_END", SomaBodyJoint_LEFT_HAND_THUMB_END) + .value("LEFT_HAND_INDEX1", SomaBodyJoint_LEFT_HAND_INDEX1) + .value("LEFT_HAND_INDEX2", SomaBodyJoint_LEFT_HAND_INDEX2) + .value("LEFT_HAND_INDEX3", SomaBodyJoint_LEFT_HAND_INDEX3) + .value("LEFT_HAND_INDEX4", SomaBodyJoint_LEFT_HAND_INDEX4) + .value("LEFT_HAND_INDEX_END", SomaBodyJoint_LEFT_HAND_INDEX_END) + .value("LEFT_HAND_MIDDLE1", SomaBodyJoint_LEFT_HAND_MIDDLE1) + .value("LEFT_HAND_MIDDLE2", SomaBodyJoint_LEFT_HAND_MIDDLE2) + .value("LEFT_HAND_MIDDLE3", SomaBodyJoint_LEFT_HAND_MIDDLE3) + .value("LEFT_HAND_MIDDLE4", SomaBodyJoint_LEFT_HAND_MIDDLE4) + .value("LEFT_HAND_MIDDLE_END", SomaBodyJoint_LEFT_HAND_MIDDLE_END) + .value("LEFT_HAND_RING1", SomaBodyJoint_LEFT_HAND_RING1) + .value("LEFT_HAND_RING2", SomaBodyJoint_LEFT_HAND_RING2) + .value("LEFT_HAND_RING3", SomaBodyJoint_LEFT_HAND_RING3) + .value("LEFT_HAND_RING4", SomaBodyJoint_LEFT_HAND_RING4) + .value("LEFT_HAND_RING_END", SomaBodyJoint_LEFT_HAND_RING_END) + .value("LEFT_HAND_PINKY1", SomaBodyJoint_LEFT_HAND_PINKY1) + .value("LEFT_HAND_PINKY2", SomaBodyJoint_LEFT_HAND_PINKY2) + .value("LEFT_HAND_PINKY3", SomaBodyJoint_LEFT_HAND_PINKY3) + .value("LEFT_HAND_PINKY4", SomaBodyJoint_LEFT_HAND_PINKY4) + .value("LEFT_HAND_PINKY_END", SomaBodyJoint_LEFT_HAND_PINKY_END) + .value("RIGHT_SHOULDER", SomaBodyJoint_RIGHT_SHOULDER) + .value("RIGHT_ARM", SomaBodyJoint_RIGHT_ARM) + .value("RIGHT_FORE_ARM", SomaBodyJoint_RIGHT_FORE_ARM) + .value("RIGHT_HAND", SomaBodyJoint_RIGHT_HAND) + .value("RIGHT_HAND_THUMB1", SomaBodyJoint_RIGHT_HAND_THUMB1) + .value("RIGHT_HAND_THUMB2", SomaBodyJoint_RIGHT_HAND_THUMB2) + .value("RIGHT_HAND_THUMB3", SomaBodyJoint_RIGHT_HAND_THUMB3) + .value("RIGHT_HAND_THUMB_END", SomaBodyJoint_RIGHT_HAND_THUMB_END) + .value("RIGHT_HAND_INDEX1", SomaBodyJoint_RIGHT_HAND_INDEX1) + .value("RIGHT_HAND_INDEX2", SomaBodyJoint_RIGHT_HAND_INDEX2) + .value("RIGHT_HAND_INDEX3", SomaBodyJoint_RIGHT_HAND_INDEX3) + .value("RIGHT_HAND_INDEX4", SomaBodyJoint_RIGHT_HAND_INDEX4) + .value("RIGHT_HAND_INDEX_END", SomaBodyJoint_RIGHT_HAND_INDEX_END) + .value("RIGHT_HAND_MIDDLE1", SomaBodyJoint_RIGHT_HAND_MIDDLE1) + .value("RIGHT_HAND_MIDDLE2", SomaBodyJoint_RIGHT_HAND_MIDDLE2) + .value("RIGHT_HAND_MIDDLE3", SomaBodyJoint_RIGHT_HAND_MIDDLE3) + .value("RIGHT_HAND_MIDDLE4", SomaBodyJoint_RIGHT_HAND_MIDDLE4) + .value("RIGHT_HAND_MIDDLE_END", SomaBodyJoint_RIGHT_HAND_MIDDLE_END) + .value("RIGHT_HAND_RING1", SomaBodyJoint_RIGHT_HAND_RING1) + .value("RIGHT_HAND_RING2", SomaBodyJoint_RIGHT_HAND_RING2) + .value("RIGHT_HAND_RING3", SomaBodyJoint_RIGHT_HAND_RING3) + .value("RIGHT_HAND_RING4", SomaBodyJoint_RIGHT_HAND_RING4) + .value("RIGHT_HAND_RING_END", SomaBodyJoint_RIGHT_HAND_RING_END) + .value("RIGHT_HAND_PINKY1", SomaBodyJoint_RIGHT_HAND_PINKY1) + .value("RIGHT_HAND_PINKY2", SomaBodyJoint_RIGHT_HAND_PINKY2) + .value("RIGHT_HAND_PINKY3", SomaBodyJoint_RIGHT_HAND_PINKY3) + .value("RIGHT_HAND_PINKY4", SomaBodyJoint_RIGHT_HAND_PINKY4) + .value("RIGHT_HAND_PINKY_END", SomaBodyJoint_RIGHT_HAND_PINKY_END) + .value("LEFT_LEG", SomaBodyJoint_LEFT_LEG) + .value("LEFT_SHIN", SomaBodyJoint_LEFT_SHIN) + .value("LEFT_FOOT", SomaBodyJoint_LEFT_FOOT) + .value("LEFT_TOE_BASE", SomaBodyJoint_LEFT_TOE_BASE) + .value("LEFT_TOE_END", SomaBodyJoint_LEFT_TOE_END) + .value("RIGHT_LEG", SomaBodyJoint_RIGHT_LEG) + .value("RIGHT_SHIN", SomaBodyJoint_RIGHT_SHIN) + .value("RIGHT_FOOT", SomaBodyJoint_RIGHT_FOOT) + .value("RIGHT_TOE_BASE", SomaBodyJoint_RIGHT_TOE_BASE) + .value("RIGHT_TOE_END", SomaBodyJoint_RIGHT_TOE_END) + .value("NUM_JOINTS", SomaBodyJoint_NUM_JOINTS); + + py::class_(m, "SomaBodyJointRotationArray") + .def(py::init<>()) + .def( + "values", + [](const SomaBodyJointRotationArray& self, size_t index) -> const SomaJointRotation* + { + if (index >= static_cast(SomaBodyJoint_NUM_JOINTS)) + { + throw py::index_error("SomaBodyJointRotationArray index out of range"); + } + return (*self.values())[index]; + }, + py::arg("index"), py::return_value_policy::reference_internal) + .def_property_readonly( + "rotations", + [](py::object self) + { + const auto* first = reinterpret_cast(&first_soma_body_joint_rotations_rotation(self).rotation()); + return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 4); + }, + "Unit XYZW quaternions as a writable (77, 4) float32 view.") + .def_property_readonly( + "is_valid", + [offset = FBS_FIELD_OFFSET(SomaJointRotation, is_valid)](py::object self) + { + const auto* first = fbs_field_address(first_soma_body_joint_rotations_rotation(self), offset); + return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 0); + }, + "Per-joint validity as a writable (77,) uint8 view."); + + serialized_class(m, "SomaBodyJointRotations", "Encoded SOMA body joint rotations.") + .def(py::init( + [](const SomaBodyJointRotationArray& joint_rotations, const Point& global_translation, + bool global_translation_is_valid) + { + SomaBodyJointRotationsT native; + native.joint_rotations = std::make_shared(joint_rotations); + native.global_translation = std::make_shared(global_translation); + native.global_translation_is_valid = global_translation_is_valid; + return pack(native); + }), + py::arg("joint_rotations") = SomaBodyJointRotationArray(), py::arg("global_translation") = Point(), + py::arg("global_translation_is_valid") = false) + .def_property_readonly("joint_rotations", field(&SomaBodyJointRotations::joint_rotations), + py::return_value_policy::reference_internal) + .def_property_readonly("global_translation", field(&SomaBodyJointRotations::global_translation), + py::return_value_policy::reference_internal) + .def_property_readonly( + "global_translation_is_valid", field(&SomaBodyJointRotations::global_translation_is_valid)); + + bind_record( + m, "SomaBodyJointRotationsRecord", "SomaBodyJointRotations"); +} + +} // namespace core diff --git a/src/core/schema/python/soma_body_v0_bindings.h b/src/core/schema/python/soma_body_v0_bindings.h deleted file mode 100644 index bec4181688..0000000000 --- a/src/core/schema/python/soma_body_v0_bindings.h +++ /dev/null @@ -1,169 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -#include "schema_array_views.h" -#include "schema_serialized.h" - -#include -#include - -#include -#include -#include - -namespace py = pybind11; - -namespace core -{ - -inline const SomaJointRotationV0& first_soma_body_v0_rotation(const py::object& self) -{ - return *(*self.cast().values())[0]; -} - -constexpr py::ssize_t SOMA_BODY_JOINT_STRIDE = static_cast(sizeof(SomaJointRotationV0)); -constexpr py::ssize_t SOMA_BODY_JOINT_COUNT = static_cast(SomaBodyJointV0_NUM_JOINTS); - -static_assert(sizeof(SomaBodyJointRotationArrayV0) == - sizeof(SomaJointRotationV0) * static_cast(SomaBodyJointV0_NUM_JOINTS), - "SomaBodyJointRotationArrayV0.values length must equal SomaBodyJointV0::NUM_JOINTS"); - -inline void bind_soma_body_v0(py::module& m) -{ - py::enum_(m, "SomaBodyJointV0") - .value("HIPS", SomaBodyJointV0_HIPS) - .value("SPINE1", SomaBodyJointV0_SPINE1) - .value("SPINE2", SomaBodyJointV0_SPINE2) - .value("CHEST", SomaBodyJointV0_CHEST) - .value("NECK1", SomaBodyJointV0_NECK1) - .value("NECK2", SomaBodyJointV0_NECK2) - .value("HEAD", SomaBodyJointV0_HEAD) - .value("HEAD_END", SomaBodyJointV0_HEAD_END) - .value("JAW", SomaBodyJointV0_JAW) - .value("LEFT_EYE", SomaBodyJointV0_LEFT_EYE) - .value("RIGHT_EYE", SomaBodyJointV0_RIGHT_EYE) - .value("LEFT_SHOULDER", SomaBodyJointV0_LEFT_SHOULDER) - .value("LEFT_ARM", SomaBodyJointV0_LEFT_ARM) - .value("LEFT_FORE_ARM", SomaBodyJointV0_LEFT_FORE_ARM) - .value("LEFT_HAND", SomaBodyJointV0_LEFT_HAND) - .value("LEFT_HAND_THUMB1", SomaBodyJointV0_LEFT_HAND_THUMB1) - .value("LEFT_HAND_THUMB2", SomaBodyJointV0_LEFT_HAND_THUMB2) - .value("LEFT_HAND_THUMB3", SomaBodyJointV0_LEFT_HAND_THUMB3) - .value("LEFT_HAND_THUMB_END", SomaBodyJointV0_LEFT_HAND_THUMB_END) - .value("LEFT_HAND_INDEX1", SomaBodyJointV0_LEFT_HAND_INDEX1) - .value("LEFT_HAND_INDEX2", SomaBodyJointV0_LEFT_HAND_INDEX2) - .value("LEFT_HAND_INDEX3", SomaBodyJointV0_LEFT_HAND_INDEX3) - .value("LEFT_HAND_INDEX4", SomaBodyJointV0_LEFT_HAND_INDEX4) - .value("LEFT_HAND_INDEX_END", SomaBodyJointV0_LEFT_HAND_INDEX_END) - .value("LEFT_HAND_MIDDLE1", SomaBodyJointV0_LEFT_HAND_MIDDLE1) - .value("LEFT_HAND_MIDDLE2", SomaBodyJointV0_LEFT_HAND_MIDDLE2) - .value("LEFT_HAND_MIDDLE3", SomaBodyJointV0_LEFT_HAND_MIDDLE3) - .value("LEFT_HAND_MIDDLE4", SomaBodyJointV0_LEFT_HAND_MIDDLE4) - .value("LEFT_HAND_MIDDLE_END", SomaBodyJointV0_LEFT_HAND_MIDDLE_END) - .value("LEFT_HAND_RING1", SomaBodyJointV0_LEFT_HAND_RING1) - .value("LEFT_HAND_RING2", SomaBodyJointV0_LEFT_HAND_RING2) - .value("LEFT_HAND_RING3", SomaBodyJointV0_LEFT_HAND_RING3) - .value("LEFT_HAND_RING4", SomaBodyJointV0_LEFT_HAND_RING4) - .value("LEFT_HAND_RING_END", SomaBodyJointV0_LEFT_HAND_RING_END) - .value("LEFT_HAND_PINKY1", SomaBodyJointV0_LEFT_HAND_PINKY1) - .value("LEFT_HAND_PINKY2", SomaBodyJointV0_LEFT_HAND_PINKY2) - .value("LEFT_HAND_PINKY3", SomaBodyJointV0_LEFT_HAND_PINKY3) - .value("LEFT_HAND_PINKY4", SomaBodyJointV0_LEFT_HAND_PINKY4) - .value("LEFT_HAND_PINKY_END", SomaBodyJointV0_LEFT_HAND_PINKY_END) - .value("RIGHT_SHOULDER", SomaBodyJointV0_RIGHT_SHOULDER) - .value("RIGHT_ARM", SomaBodyJointV0_RIGHT_ARM) - .value("RIGHT_FORE_ARM", SomaBodyJointV0_RIGHT_FORE_ARM) - .value("RIGHT_HAND", SomaBodyJointV0_RIGHT_HAND) - .value("RIGHT_HAND_THUMB1", SomaBodyJointV0_RIGHT_HAND_THUMB1) - .value("RIGHT_HAND_THUMB2", SomaBodyJointV0_RIGHT_HAND_THUMB2) - .value("RIGHT_HAND_THUMB3", SomaBodyJointV0_RIGHT_HAND_THUMB3) - .value("RIGHT_HAND_THUMB_END", SomaBodyJointV0_RIGHT_HAND_THUMB_END) - .value("RIGHT_HAND_INDEX1", SomaBodyJointV0_RIGHT_HAND_INDEX1) - .value("RIGHT_HAND_INDEX2", SomaBodyJointV0_RIGHT_HAND_INDEX2) - .value("RIGHT_HAND_INDEX3", SomaBodyJointV0_RIGHT_HAND_INDEX3) - .value("RIGHT_HAND_INDEX4", SomaBodyJointV0_RIGHT_HAND_INDEX4) - .value("RIGHT_HAND_INDEX_END", SomaBodyJointV0_RIGHT_HAND_INDEX_END) - .value("RIGHT_HAND_MIDDLE1", SomaBodyJointV0_RIGHT_HAND_MIDDLE1) - .value("RIGHT_HAND_MIDDLE2", SomaBodyJointV0_RIGHT_HAND_MIDDLE2) - .value("RIGHT_HAND_MIDDLE3", SomaBodyJointV0_RIGHT_HAND_MIDDLE3) - .value("RIGHT_HAND_MIDDLE4", SomaBodyJointV0_RIGHT_HAND_MIDDLE4) - .value("RIGHT_HAND_MIDDLE_END", SomaBodyJointV0_RIGHT_HAND_MIDDLE_END) - .value("RIGHT_HAND_RING1", SomaBodyJointV0_RIGHT_HAND_RING1) - .value("RIGHT_HAND_RING2", SomaBodyJointV0_RIGHT_HAND_RING2) - .value("RIGHT_HAND_RING3", SomaBodyJointV0_RIGHT_HAND_RING3) - .value("RIGHT_HAND_RING4", SomaBodyJointV0_RIGHT_HAND_RING4) - .value("RIGHT_HAND_RING_END", SomaBodyJointV0_RIGHT_HAND_RING_END) - .value("RIGHT_HAND_PINKY1", SomaBodyJointV0_RIGHT_HAND_PINKY1) - .value("RIGHT_HAND_PINKY2", SomaBodyJointV0_RIGHT_HAND_PINKY2) - .value("RIGHT_HAND_PINKY3", SomaBodyJointV0_RIGHT_HAND_PINKY3) - .value("RIGHT_HAND_PINKY4", SomaBodyJointV0_RIGHT_HAND_PINKY4) - .value("RIGHT_HAND_PINKY_END", SomaBodyJointV0_RIGHT_HAND_PINKY_END) - .value("LEFT_LEG", SomaBodyJointV0_LEFT_LEG) - .value("LEFT_SHIN", SomaBodyJointV0_LEFT_SHIN) - .value("LEFT_FOOT", SomaBodyJointV0_LEFT_FOOT) - .value("LEFT_TOE_BASE", SomaBodyJointV0_LEFT_TOE_BASE) - .value("LEFT_TOE_END", SomaBodyJointV0_LEFT_TOE_END) - .value("RIGHT_LEG", SomaBodyJointV0_RIGHT_LEG) - .value("RIGHT_SHIN", SomaBodyJointV0_RIGHT_SHIN) - .value("RIGHT_FOOT", SomaBodyJointV0_RIGHT_FOOT) - .value("RIGHT_TOE_BASE", SomaBodyJointV0_RIGHT_TOE_BASE) - .value("RIGHT_TOE_END", SomaBodyJointV0_RIGHT_TOE_END) - .value("NUM_JOINTS", SomaBodyJointV0_NUM_JOINTS); - - py::class_(m, "SomaBodyJointRotationArrayV0") - .def(py::init<>()) - .def( - "values", - [](const SomaBodyJointRotationArrayV0& self, size_t index) -> const SomaJointRotationV0* - { - if (index >= static_cast(SomaBodyJointV0_NUM_JOINTS)) - { - throw py::index_error("SomaBodyJointRotationArrayV0 index out of range"); - } - return (*self.values())[index]; - }, - py::arg("index"), py::return_value_policy::reference_internal) - .def_property_readonly( - "rotations", - [](py::object self) - { - const auto* first = reinterpret_cast(&first_soma_body_v0_rotation(self).rotation()); - return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 4); - }, - "Unit XYZW quaternions as a writable (77, 4) float32 view.") - .def_property_readonly( - "is_valid", - [offset = FBS_FIELD_OFFSET(SomaJointRotationV0, is_valid)](py::object self) - { - const auto* first = fbs_field_address(first_soma_body_v0_rotation(self), offset); - return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 0); - }, - "Per-joint validity as a writable (77,) uint8 view."); - - serialized_class(m, "SomaBodyJointRotationsV0", "Encoded SOMA body joint rotations v0.") - .def(py::init( - [](const SomaBodyJointRotationArrayV0& joint_rotations, const Point& global_translation, - bool global_translation_is_valid) - { - SomaBodyJointRotationsV0T native; - native.joint_rotations = std::make_shared(joint_rotations); - native.global_translation = std::make_shared(global_translation); - native.global_translation_is_valid = global_translation_is_valid; - return pack(native); - }), - py::arg("joint_rotations") = SomaBodyJointRotationArrayV0(), py::arg("global_translation") = Point(), - py::arg("global_translation_is_valid") = false) - .def_property_readonly("joint_rotations", field(&SomaBodyJointRotationsV0::joint_rotations), - py::return_value_policy::reference_internal) - .def_property_readonly("global_translation", field(&SomaBodyJointRotationsV0::global_translation), - py::return_value_policy::reference_internal) - .def_property_readonly( - "global_translation_is_valid", field(&SomaBodyJointRotationsV0::global_translation_is_valid)); - - bind_record( - m, "SomaBodyJointRotationsV0Record", "SomaBodyJointRotationsV0"); -} - -} // namespace core diff --git a/src/core/schema/python/soma_common_bindings.h b/src/core/schema/python/soma_common_bindings.h new file mode 100644 index 0000000000..c8b76f91a9 --- /dev/null +++ b/src/core/schema/python/soma_common_bindings.h @@ -0,0 +1,28 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_soma_common(py::module& m) +{ + py::enum_(m, "SomaHandedness") + .value("UNSPECIFIED", SomaHandedness_UNSPECIFIED) + .value("LEFT", SomaHandedness_LEFT) + .value("RIGHT", SomaHandedness_RIGHT); + + py::class_(m, "SomaJointRotation") + .def(py::init<>()) + .def(py::init(), py::arg("rotation"), py::arg("is_valid") = false) + .def_property_readonly("rotation", &SomaJointRotation::rotation, py::return_value_policy::reference_internal) + .def_property_readonly("is_valid", &SomaJointRotation::is_valid); +} + +} // namespace core diff --git a/src/core/schema/python/soma_common_v0_bindings.h b/src/core/schema/python/soma_common_v0_bindings.h deleted file mode 100644 index 8d8591a3dc..0000000000 --- a/src/core/schema/python/soma_common_v0_bindings.h +++ /dev/null @@ -1,28 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -#include -#include - -namespace py = pybind11; - -namespace core -{ - -inline void bind_soma_common_v0(py::module& m) -{ - py::enum_(m, "SomaHandednessV0") - .value("UNSPECIFIED", SomaHandednessV0_UNSPECIFIED) - .value("LEFT", SomaHandednessV0_LEFT) - .value("RIGHT", SomaHandednessV0_RIGHT); - - py::class_(m, "SomaJointRotationV0") - .def(py::init<>()) - .def(py::init(), py::arg("rotation"), py::arg("is_valid") = false) - .def_property_readonly("rotation", &SomaJointRotationV0::rotation, py::return_value_policy::reference_internal) - .def_property_readonly("is_valid", &SomaJointRotationV0::is_valid); -} - -} // namespace core diff --git a/src/core/schema/python/soma_hand_joint_poses_v0_bindings.h b/src/core/schema/python/soma_hand_joint_poses_bindings.h similarity index 53% rename from src/core/schema/python/soma_hand_joint_poses_v0_bindings.h rename to src/core/schema/python/soma_hand_joint_poses_bindings.h index d0ab4d540f..51650a159e 100644 --- a/src/core/schema/python/soma_hand_joint_poses_v0_bindings.h +++ b/src/core/schema/python/soma_hand_joint_poses_bindings.h @@ -7,7 +7,7 @@ #include "schema_serialized.h" #include -#include +#include #include #include @@ -18,35 +18,35 @@ namespace py = pybind11; namespace core { -inline const SomaHandJointPoseV0& first_soma_hand_joint_pose_v0(const py::object& self) +inline const SomaHandJointPose& first_soma_hand_joint_pose(const py::object& self) { - return *(*self.cast().values())[0]; + return *(*self.cast().values())[0]; } -constexpr py::ssize_t SOMA_HAND_JOINT_POSE_STRIDE = static_cast(sizeof(SomaHandJointPoseV0)); -constexpr py::ssize_t SOMA_HAND_JOINT_POSE_COUNT = static_cast(SomaHandJointV0_NUM_JOINTS); +constexpr py::ssize_t SOMA_HAND_JOINT_POSE_STRIDE = static_cast(sizeof(SomaHandJointPose)); +constexpr py::ssize_t SOMA_HAND_JOINT_POSE_COUNT = static_cast(SomaHandJoint_NUM_JOINTS); -static_assert(sizeof(SomaHandJointPoseArrayV0) == - sizeof(SomaHandJointPoseV0) * static_cast(SomaHandJointV0_NUM_JOINTS), - "SomaHandJointPoseArrayV0.values length must equal SomaHandJointV0::NUM_JOINTS"); +static_assert(sizeof(SomaHandJointPoseArray) == + sizeof(SomaHandJointPose) * static_cast(SomaHandJoint_NUM_JOINTS), + "SomaHandJointPoseArray.values length must equal SomaHandJoint::NUM_JOINTS"); -inline void bind_soma_hand_joint_poses_v0(py::module& m) +inline void bind_soma_hand_joint_poses(py::module& m) { - py::class_(m, "SomaHandJointPoseV0") + py::class_(m, "SomaHandJointPose") .def(py::init<>()) .def(py::init(), py::arg("pose"), py::arg("is_valid") = false) - .def_property_readonly("pose", &SomaHandJointPoseV0::pose, py::return_value_policy::reference_internal) - .def_property_readonly("is_valid", &SomaHandJointPoseV0::is_valid); + .def_property_readonly("pose", &SomaHandJointPose::pose, py::return_value_policy::reference_internal) + .def_property_readonly("is_valid", &SomaHandJointPose::is_valid); - py::class_(m, "SomaHandJointPoseArrayV0") + py::class_(m, "SomaHandJointPoseArray") .def(py::init<>()) .def( "values", - [](const SomaHandJointPoseArrayV0& self, size_t index) -> const SomaHandJointPoseV0* + [](const SomaHandJointPoseArray& self, size_t index) -> const SomaHandJointPose* { - if (index >= static_cast(SomaHandJointV0_NUM_JOINTS)) + if (index >= static_cast(SomaHandJoint_NUM_JOINTS)) { - throw py::index_error("SomaHandJointPoseArrayV0 index out of range"); + throw py::index_error("SomaHandJointPoseArray index out of range"); } return (*self.values())[index]; }, @@ -56,45 +56,45 @@ inline void bind_soma_hand_joint_poses_v0(py::module& m) [](py::object self) { const auto* first = - reinterpret_cast(&first_soma_hand_joint_pose_v0(self).pose().position()); + reinterpret_cast(&first_soma_hand_joint_pose(self).pose().position()); return strided_field_view(self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 3); }, - "Joint positions as a writable (25, 3) float32 view in SomaHandJointV0 order.") + "Joint positions as a writable (25, 3) float32 view in SomaHandJoint order.") .def_property_readonly( "orientations", [](py::object self) { const auto* first = - reinterpret_cast(&first_soma_hand_joint_pose_v0(self).pose().orientation()); + reinterpret_cast(&first_soma_hand_joint_pose(self).pose().orientation()); return strided_field_view(self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 4); }, "Unit XYZW quaternions as a writable (25, 4) float32 view.") .def_property_readonly( "is_valid", - [offset = FBS_FIELD_OFFSET(SomaHandJointPoseV0, is_valid)](py::object self) + [offset = FBS_FIELD_OFFSET(SomaHandJointPose, is_valid)](py::object self) { - const auto* first = fbs_field_address(first_soma_hand_joint_pose_v0(self), offset); + const auto* first = fbs_field_address(first_soma_hand_joint_pose(self), offset); return strided_field_view( self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 0); }, "Per-joint validity as a writable (25,) uint8 view."); - serialized_class(m, "SomaHandJointPosesV0", "Encoded evaluated SOMA hand poses v0.") + serialized_class(m, "SomaHandJointPoses", "Encoded evaluated SOMA hand poses.") .def(py::init( - [](const SomaHandJointPoseArrayV0& joint_poses, SomaHandednessV0 handedness) + [](const SomaHandJointPoseArray& joint_poses, SomaHandedness handedness) { - SomaHandJointPosesV0T native; - native.joint_poses = std::make_shared(joint_poses); + SomaHandJointPosesT native; + native.joint_poses = std::make_shared(joint_poses); native.handedness = handedness; - return pack(native); + return pack(native); }), - py::arg("joint_poses") = SomaHandJointPoseArrayV0(), py::arg("handedness") = SomaHandednessV0_UNSPECIFIED) + py::arg("joint_poses") = SomaHandJointPoseArray(), py::arg("handedness") = SomaHandedness_UNSPECIFIED) .def_property_readonly( - "joint_poses", field(&SomaHandJointPosesV0::joint_poses), py::return_value_policy::reference_internal) - .def_property_readonly("handedness", field(&SomaHandJointPosesV0::handedness)); + "joint_poses", field(&SomaHandJointPoses::joint_poses), py::return_value_policy::reference_internal) + .def_property_readonly("handedness", field(&SomaHandJointPoses::handedness)); - bind_record( - m, "SomaHandJointPosesV0Record", "SomaHandJointPosesV0"); + bind_record( + m, "SomaHandJointPosesRecord", "SomaHandJointPoses"); } } // namespace core diff --git a/src/core/schema/python/soma_hand_joint_rotations_bindings.h b/src/core/schema/python/soma_hand_joint_rotations_bindings.h new file mode 100644 index 0000000000..6243696d2e --- /dev/null +++ b/src/core/schema/python/soma_hand_joint_rotations_bindings.h @@ -0,0 +1,119 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline const SomaJointRotation& first_soma_hand_rotation(const py::object& self) +{ + return *(*self.cast().values())[0]; +} + +constexpr py::ssize_t SOMA_HAND_JOINT_STRIDE = static_cast(sizeof(SomaJointRotation)); +constexpr py::ssize_t SOMA_HAND_JOINT_COUNT = static_cast(SomaHandJoint_NUM_JOINTS); + +static_assert(sizeof(SomaHandJointRotationArray) == + sizeof(SomaJointRotation) * static_cast(SomaHandJoint_NUM_JOINTS), + "SomaHandJointRotationArray.values length must equal SomaHandJoint::NUM_JOINTS"); + +inline void bind_soma_hand_joint_rotations(py::module& m) +{ + py::enum_(m, "SomaHandJoint") + .value("WRIST", SomaHandJoint_WRIST) + .value("THUMB1", SomaHandJoint_THUMB1) + .value("THUMB2", SomaHandJoint_THUMB2) + .value("THUMB3", SomaHandJoint_THUMB3) + .value("THUMB_END", SomaHandJoint_THUMB_END) + .value("INDEX1", SomaHandJoint_INDEX1) + .value("INDEX2", SomaHandJoint_INDEX2) + .value("INDEX3", SomaHandJoint_INDEX3) + .value("INDEX4", SomaHandJoint_INDEX4) + .value("INDEX_END", SomaHandJoint_INDEX_END) + .value("MIDDLE1", SomaHandJoint_MIDDLE1) + .value("MIDDLE2", SomaHandJoint_MIDDLE2) + .value("MIDDLE3", SomaHandJoint_MIDDLE3) + .value("MIDDLE4", SomaHandJoint_MIDDLE4) + .value("MIDDLE_END", SomaHandJoint_MIDDLE_END) + .value("RING1", SomaHandJoint_RING1) + .value("RING2", SomaHandJoint_RING2) + .value("RING3", SomaHandJoint_RING3) + .value("RING4", SomaHandJoint_RING4) + .value("RING_END", SomaHandJoint_RING_END) + .value("PINKY1", SomaHandJoint_PINKY1) + .value("PINKY2", SomaHandJoint_PINKY2) + .value("PINKY3", SomaHandJoint_PINKY3) + .value("PINKY4", SomaHandJoint_PINKY4) + .value("PINKY_END", SomaHandJoint_PINKY_END) + .value("NUM_JOINTS", SomaHandJoint_NUM_JOINTS); + + py::class_(m, "SomaHandJointRotationArray") + .def(py::init<>()) + .def( + "values", + [](const SomaHandJointRotationArray& self, size_t index) -> const SomaJointRotation* + { + if (index >= static_cast(SomaHandJoint_NUM_JOINTS)) + { + throw py::index_error("SomaHandJointRotationArray index out of range"); + } + return (*self.values())[index]; + }, + py::arg("index"), py::return_value_policy::reference_internal) + .def_property_readonly( + "rotations", + [](py::object self) + { + const auto* first = reinterpret_cast(&first_soma_hand_rotation(self).rotation()); + return strided_field_view(self, first, SOMA_HAND_JOINT_STRIDE, SOMA_HAND_JOINT_COUNT, 4); + }, + "Unit XYZW quaternions as a writable (25, 4) float32 view.") + .def_property_readonly( + "is_valid", + [offset = FBS_FIELD_OFFSET(SomaJointRotation, is_valid)](py::object self) + { + const auto* first = fbs_field_address(first_soma_hand_rotation(self), offset); + return strided_field_view(self, first, SOMA_HAND_JOINT_STRIDE, SOMA_HAND_JOINT_COUNT, 0); + }, + "Per-joint validity as a writable (25,) uint8 view."); + + serialized_class(m, "SomaHandJointRotations", "Encoded SOMA hand joint rotations.") + .def(py::init( + [](const SomaHandJointRotationArray& joint_rotations, const Point& global_translation, + bool global_translation_is_valid, SomaHandedness handedness) + { + SomaHandJointRotationsT native; + native.joint_rotations = std::make_shared(joint_rotations); + native.global_translation = std::make_shared(global_translation); + native.global_translation_is_valid = global_translation_is_valid; + native.handedness = handedness; + return pack(native); + }), + py::arg("joint_rotations") = SomaHandJointRotationArray(), py::arg("global_translation") = Point(), + py::arg("global_translation_is_valid") = false, py::arg("handedness") = SomaHandedness_UNSPECIFIED) + .def_property_readonly("joint_rotations", field(&SomaHandJointRotations::joint_rotations), + py::return_value_policy::reference_internal) + .def_property_readonly("global_translation", field(&SomaHandJointRotations::global_translation), + py::return_value_policy::reference_internal) + .def_property_readonly( + "global_translation_is_valid", field(&SomaHandJointRotations::global_translation_is_valid)) + .def_property_readonly("handedness", field(&SomaHandJointRotations::handedness)); + + bind_record( + m, "SomaHandJointRotationsRecord", "SomaHandJointRotations"); +} + +} // namespace core diff --git a/src/core/schema/python/soma_hand_v0_bindings.h b/src/core/schema/python/soma_hand_v0_bindings.h deleted file mode 100644 index 90137cd765..0000000000 --- a/src/core/schema/python/soma_hand_v0_bindings.h +++ /dev/null @@ -1,119 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#pragma once - -#include "schema_array_views.h" -#include "schema_serialized.h" - -#include -#include - -#include -#include -#include - -namespace py = pybind11; - -namespace core -{ - -inline const SomaJointRotationV0& first_soma_hand_rotation(const py::object& self) -{ - return *(*self.cast().values())[0]; -} - -constexpr py::ssize_t SOMA_HAND_JOINT_STRIDE = static_cast(sizeof(SomaJointRotationV0)); -constexpr py::ssize_t SOMA_HAND_JOINT_COUNT = static_cast(SomaHandJointV0_NUM_JOINTS); - -static_assert(sizeof(SomaHandJointRotationArrayV0) == - sizeof(SomaJointRotationV0) * static_cast(SomaHandJointV0_NUM_JOINTS), - "SomaHandJointRotationArrayV0.values length must equal SomaHandJointV0::NUM_JOINTS"); - -inline void bind_soma_hand_v0(py::module& m) -{ - py::enum_(m, "SomaHandJointV0") - .value("WRIST", SomaHandJointV0_WRIST) - .value("THUMB1", SomaHandJointV0_THUMB1) - .value("THUMB2", SomaHandJointV0_THUMB2) - .value("THUMB3", SomaHandJointV0_THUMB3) - .value("THUMB_END", SomaHandJointV0_THUMB_END) - .value("INDEX1", SomaHandJointV0_INDEX1) - .value("INDEX2", SomaHandJointV0_INDEX2) - .value("INDEX3", SomaHandJointV0_INDEX3) - .value("INDEX4", SomaHandJointV0_INDEX4) - .value("INDEX_END", SomaHandJointV0_INDEX_END) - .value("MIDDLE1", SomaHandJointV0_MIDDLE1) - .value("MIDDLE2", SomaHandJointV0_MIDDLE2) - .value("MIDDLE3", SomaHandJointV0_MIDDLE3) - .value("MIDDLE4", SomaHandJointV0_MIDDLE4) - .value("MIDDLE_END", SomaHandJointV0_MIDDLE_END) - .value("RING1", SomaHandJointV0_RING1) - .value("RING2", SomaHandJointV0_RING2) - .value("RING3", SomaHandJointV0_RING3) - .value("RING4", SomaHandJointV0_RING4) - .value("RING_END", SomaHandJointV0_RING_END) - .value("PINKY1", SomaHandJointV0_PINKY1) - .value("PINKY2", SomaHandJointV0_PINKY2) - .value("PINKY3", SomaHandJointV0_PINKY3) - .value("PINKY4", SomaHandJointV0_PINKY4) - .value("PINKY_END", SomaHandJointV0_PINKY_END) - .value("NUM_JOINTS", SomaHandJointV0_NUM_JOINTS); - - py::class_(m, "SomaHandJointRotationArrayV0") - .def(py::init<>()) - .def( - "values", - [](const SomaHandJointRotationArrayV0& self, size_t index) -> const SomaJointRotationV0* - { - if (index >= static_cast(SomaHandJointV0_NUM_JOINTS)) - { - throw py::index_error("SomaHandJointRotationArrayV0 index out of range"); - } - return (*self.values())[index]; - }, - py::arg("index"), py::return_value_policy::reference_internal) - .def_property_readonly( - "rotations", - [](py::object self) - { - const auto* first = reinterpret_cast(&first_soma_hand_rotation(self).rotation()); - return strided_field_view(self, first, SOMA_HAND_JOINT_STRIDE, SOMA_HAND_JOINT_COUNT, 4); - }, - "Unit XYZW quaternions as a writable (25, 4) float32 view.") - .def_property_readonly( - "is_valid", - [offset = FBS_FIELD_OFFSET(SomaJointRotationV0, is_valid)](py::object self) - { - const auto* first = fbs_field_address(first_soma_hand_rotation(self), offset); - return strided_field_view(self, first, SOMA_HAND_JOINT_STRIDE, SOMA_HAND_JOINT_COUNT, 0); - }, - "Per-joint validity as a writable (25,) uint8 view."); - - serialized_class(m, "SomaHandJointRotationsV0", "Encoded SOMA hand joint rotations v0.") - .def(py::init( - [](const SomaHandJointRotationArrayV0& joint_rotations, const Point& global_translation, - bool global_translation_is_valid, SomaHandednessV0 handedness) - { - SomaHandJointRotationsV0T native; - native.joint_rotations = std::make_shared(joint_rotations); - native.global_translation = std::make_shared(global_translation); - native.global_translation_is_valid = global_translation_is_valid; - native.handedness = handedness; - return pack(native); - }), - py::arg("joint_rotations") = SomaHandJointRotationArrayV0(), py::arg("global_translation") = Point(), - py::arg("global_translation_is_valid") = false, py::arg("handedness") = SomaHandednessV0_UNSPECIFIED) - .def_property_readonly("joint_rotations", field(&SomaHandJointRotationsV0::joint_rotations), - py::return_value_policy::reference_internal) - .def_property_readonly("global_translation", field(&SomaHandJointRotationsV0::global_translation), - py::return_value_policy::reference_internal) - .def_property_readonly( - "global_translation_is_valid", field(&SomaHandJointRotationsV0::global_translation_is_valid)) - .def_property_readonly("handedness", field(&SomaHandJointRotationsV0::handedness)); - - bind_record( - m, "SomaHandJointRotationsV0Record", "SomaHandJointRotationsV0"); -} - -} // namespace core diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py index ab3235d23c..203860a39f 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py @@ -43,10 +43,10 @@ KeyboardOutputTrackedType, JointStateOutputTrackedType, FullBodyPoseTrackedType, - SomaBodyJointRotationsV0TrackedType, - SomaBodyJointPosesV0TrackedType, - SomaHandJointRotationsV0TrackedType, - SomaHandJointPosesV0TrackedType, + SomaBodyJointRotationsTrackedType, + SomaBodyJointPosesTrackedType, + SomaHandJointRotationsTrackedType, + SomaHandJointPosesTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, @@ -54,10 +54,10 @@ DeviceIOKeyboardOutputTracked, DeviceIOJointStateOutputTracked, DeviceIOFullBodyPoseTracked, - DeviceIOSomaBodyJointRotationsV0Tracked, - DeviceIOSomaBodyJointPosesV0Tracked, - DeviceIOSomaHandJointRotationsV0Tracked, - DeviceIOSomaHandJointPosesV0Tracked, + DeviceIOSomaBodyJointRotationsTracked, + DeviceIOSomaBodyJointPosesTracked, + DeviceIOSomaHandJointRotationsTracked, + DeviceIOSomaHandJointPosesTracked, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, MessageChannelStatusType, @@ -101,10 +101,10 @@ "KeyboardOutputTrackedType", "JointStateOutputTrackedType", "FullBodyPoseTrackedType", - "SomaBodyJointRotationsV0TrackedType", - "SomaBodyJointPosesV0TrackedType", - "SomaHandJointRotationsV0TrackedType", - "SomaHandJointPosesV0TrackedType", + "SomaBodyJointRotationsTrackedType", + "SomaBodyJointPosesTrackedType", + "SomaHandJointRotationsTrackedType", + "SomaHandJointPosesTrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", "MessageChannelStatusType", @@ -115,10 +115,10 @@ "DeviceIOKeyboardOutputTracked", "DeviceIOJointStateOutputTracked", "DeviceIOFullBodyPoseTracked", - "DeviceIOSomaBodyJointRotationsV0Tracked", - "DeviceIOSomaBodyJointPosesV0Tracked", - "DeviceIOSomaHandJointRotationsV0Tracked", - "DeviceIOSomaHandJointPosesV0Tracked", + "DeviceIOSomaBodyJointRotationsTracked", + "DeviceIOSomaBodyJointPosesTracked", + "DeviceIOSomaHandJointRotationsTracked", + "DeviceIOSomaHandJointPosesTracked", "DeviceIOMessageChannelMessagesTracked", "MessageChannelMessagesTrackedGroup", "MessageChannelStatusGroup", diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py index dfa90e3a91..0f45c294c2 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py @@ -22,10 +22,10 @@ KeyboardOutput, JointStateOutput, FullBodyPose, - SomaBodyJointPosesV0, - SomaBodyJointRotationsV0, - SomaHandJointPosesV0, - SomaHandJointRotationsV0, + SomaBodyJointPoses, + SomaBodyJointRotations, + SomaHandJointPoses, + SomaHandJointRotations, MessageChannelMessagesTracked, ) @@ -119,28 +119,28 @@ class FullBodyPoseTrackedType(_PayloadTensorType): _payload_cls = FullBodyPose -class SomaBodyJointRotationsV0TrackedType(_PayloadTensorType): - """SomaBodyJointRotationsV0 payload from a generated SOMA body tracker.""" +class SomaBodyJointRotationsTrackedType(_PayloadTensorType): + """SomaBodyJointRotations payload from a generated SOMA body tracker.""" - _payload_cls = SomaBodyJointRotationsV0 + _payload_cls = SomaBodyJointRotations -class SomaBodyJointPosesV0TrackedType(_PayloadTensorType): - """SomaBodyJointPosesV0 payload from a generated SOMA body tracker.""" +class SomaBodyJointPosesTrackedType(_PayloadTensorType): + """SomaBodyJointPoses payload from a generated SOMA body tracker.""" - _payload_cls = SomaBodyJointPosesV0 + _payload_cls = SomaBodyJointPoses -class SomaHandJointRotationsV0TrackedType(_PayloadTensorType): - """SomaHandJointRotationsV0 payload from a generated SOMA hand tracker.""" +class SomaHandJointRotationsTrackedType(_PayloadTensorType): + """SomaHandJointRotations payload from a generated SOMA hand tracker.""" - _payload_cls = SomaHandJointRotationsV0 + _payload_cls = SomaHandJointRotations -class SomaHandJointPosesV0TrackedType(_PayloadTensorType): - """SomaHandJointPosesV0 payload from a generated SOMA hand tracker.""" +class SomaHandJointPosesTrackedType(_PayloadTensorType): + """SomaHandJointPoses payload from a generated SOMA hand tracker.""" - _payload_cls = SomaHandJointPosesV0 + _payload_cls = SomaHandJointPoses class MessageChannelMessagesTrackedType(_RequiredPayloadTensorType): @@ -243,35 +243,35 @@ def DeviceIOFullBodyPoseTracked() -> TensorGroupType: ) -def DeviceIOSomaBodyJointRotationsV0Tracked() -> TensorGroupType: - """SOMA V0 body controls, or None when the tracker is inactive.""" +def DeviceIOSomaBodyJointRotationsTracked() -> TensorGroupType: + """SOMA body controls, or None when the tracker is inactive.""" return TensorGroupType( - "deviceio_soma_body_joint_rotations_v0", - [SomaBodyJointRotationsV0TrackedType("soma_body_tracked")], + "deviceio_soma_body_joint_rotations", + [SomaBodyJointRotationsTrackedType("soma_body_tracked")], ) -def DeviceIOSomaBodyJointPosesV0Tracked() -> TensorGroupType: - """Evaluated SOMA V0 body poses, or None when the tracker is inactive.""" +def DeviceIOSomaBodyJointPosesTracked() -> TensorGroupType: + """Evaluated SOMA body poses, or None when the tracker is inactive.""" return TensorGroupType( - "deviceio_soma_body_joint_poses_v0", - [SomaBodyJointPosesV0TrackedType("soma_body_tracked")], + "deviceio_soma_body_joint_poses", + [SomaBodyJointPosesTrackedType("soma_body_tracked")], ) -def DeviceIOSomaHandJointRotationsV0Tracked() -> TensorGroupType: - """SOMA V0 hand controls, or None when the tracker is inactive.""" +def DeviceIOSomaHandJointRotationsTracked() -> TensorGroupType: + """SOMA hand controls, or None when the tracker is inactive.""" return TensorGroupType( - "deviceio_soma_hand_joint_rotations_v0", - [SomaHandJointRotationsV0TrackedType("soma_hand_tracked")], + "deviceio_soma_hand_joint_rotations", + [SomaHandJointRotationsTrackedType("soma_hand_tracked")], ) -def DeviceIOSomaHandJointPosesV0Tracked() -> TensorGroupType: - """Evaluated SOMA V0 hand poses, or None when the tracker is inactive.""" +def DeviceIOSomaHandJointPosesTracked() -> TensorGroupType: + """Evaluated SOMA hand poses, or None when the tracker is inactive.""" return TensorGroupType( - "deviceio_soma_hand_joint_poses_v0", - [SomaHandJointPosesV0TrackedType("soma_hand_tracked")], + "deviceio_soma_hand_joint_poses", + [SomaHandJointPosesTrackedType("soma_hand_tracked")], ) diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py index 70b9575308..5d5b7d364c 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py @@ -9,13 +9,13 @@ import numpy as np from isaaccapture.deviceio_trackers import ( - SomaBodyJointPosesV0Tracker, - SomaBodyJointRotationsV0Tracker, + SomaBodyJointPosesTracker, + SomaBodyJointRotationsTracker, ) from .deviceio_tensor_types import ( - DeviceIOSomaBodyJointPosesV0Tracked, - DeviceIOSomaBodyJointRotationsV0Tracked, + DeviceIOSomaBodyJointPosesTracked, + DeviceIOSomaBodyJointRotationsTracked, ) from .interface import IDeviceIOSource from ..interface.retargeter_core_types import RetargeterIO, RetargeterIOType @@ -34,7 +34,7 @@ class SomaBodyRepresentation(str, Enum): class SomaBodySource(IDeviceIOSource): - """Normalize either SOMA V0 wire profile into evaluated joint poses. + """Normalize either SOMA wire profile into evaluated joint poses. The generated tracker remains the raw transport and recording boundary. The rotation profile runs FK once per graph step. The evaluated-pose profile maps @@ -58,11 +58,11 @@ def __init__( if self.representation is SomaBodyRepresentation.JOINT_ROTATIONS: if layer is None: raise ValueError("joint-rotations requires a prepared SOMA layer") - self._tracker = SomaBodyJointRotationsV0Tracker(collection_id) - self._input_type = DeviceIOSomaBodyJointRotationsV0Tracked() + self._tracker = SomaBodyJointRotationsTracker(collection_id) + self._input_type = DeviceIOSomaBodyJointRotationsTracked() else: - self._tracker = SomaBodyJointPosesV0Tracker(collection_id) - self._input_type = DeviceIOSomaBodyJointPosesV0Tracked() + self._tracker = SomaBodyJointPosesTracker(collection_id) + self._input_type = DeviceIOSomaBodyJointPosesTracked() self._evaluator = _SomaBodyEvaluator(layer) if layer is not None else None self.joint_names = self._evaluator.joint_names if self._evaluator else () self.bones = self._evaluator.bones if self._evaluator else () diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py index 2d92ba03fd..0f325ab9cf 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py @@ -9,14 +9,14 @@ import numpy as np from isaaccapture.deviceio_trackers import ( - SomaHandJointPosesV0Tracker, - SomaHandJointRotationsV0Tracker, + SomaHandJointPosesTracker, + SomaHandJointRotationsTracker, ) -from isaaccapture.schema import SomaHandednessV0 +from isaaccapture.schema import SomaHandedness from .deviceio_tensor_types import ( - DeviceIOSomaHandJointPosesV0Tracked, - DeviceIOSomaHandJointRotationsV0Tracked, + DeviceIOSomaHandJointPosesTracked, + DeviceIOSomaHandJointRotationsTracked, ) from .interface import IDeviceIOSource from ..interface.retargeter_core_types import RetargeterIO, RetargeterIOType @@ -35,7 +35,7 @@ class SomaHandRepresentation(str, Enum): class SomaHandSource(IDeviceIOSource): - """Normalize one SOMA V0 hand collection into evaluated joint poses.""" + """Normalize one SOMA hand collection into evaluated joint poses.""" HAND = "soma_hand" @@ -43,7 +43,7 @@ def __init__( self, name: str, collection_id: str, - handedness: SomaHandednessV0, + handedness: SomaHandedness, layer: Any | None = None, *, representation: SomaHandRepresentation @@ -51,18 +51,18 @@ def __init__( ) -> None: if not collection_id: raise ValueError("SOMA hand collection_id must not be empty") - if handedness not in (SomaHandednessV0.LEFT, SomaHandednessV0.RIGHT): + if handedness not in (SomaHandedness.LEFT, SomaHandedness.RIGHT): raise ValueError("SOMA hand source requires LEFT or RIGHT handedness") self.handedness = handedness self.representation = SomaHandRepresentation(representation) if self.representation is SomaHandRepresentation.JOINT_ROTATIONS: if layer is None: raise ValueError("joint-rotations requires a prepared SOMA hand layer") - self._tracker = SomaHandJointRotationsV0Tracker(collection_id) - self._input_type = DeviceIOSomaHandJointRotationsV0Tracked() + self._tracker = SomaHandJointRotationsTracker(collection_id) + self._input_type = DeviceIOSomaHandJointRotationsTracked() else: - self._tracker = SomaHandJointPosesV0Tracker(collection_id) - self._input_type = DeviceIOSomaHandJointPosesV0Tracked() + self._tracker = SomaHandJointPosesTracker(collection_id) + self._input_type = DeviceIOSomaHandJointPosesTracked() self._evaluator = _SomaHandEvaluator(layer) if layer is not None else None super().__init__(name) diff --git a/src/python/isaaccapture/schema/__init__.py b/src/python/isaaccapture/schema/__init__.py index d5ef0097dc..d35bde981c 100644 --- a/src/python/isaaccapture/schema/__init__.py +++ b/src/python/isaaccapture/schema/__init__.py @@ -85,28 +85,28 @@ FullBodyPose, FullBodyPoseRecord, # Shared SOMA types. - SomaHandednessV0, - SomaJointRotationV0, - # SOMA body joint-rotation v0 types. - SomaBodyJointV0, - SomaBodyJointRotationArrayV0, - SomaBodyJointRotationsV0, - SomaBodyJointRotationsV0Record, - # SOMA evaluated body joint-pose v0 types. - SomaBodyJointPoseV0, - SomaBodyJointPoseArrayV0, - SomaBodyJointPosesV0, - SomaBodyJointPosesV0Record, - # SOMA hand joint-rotation v0 types. - SomaHandJointV0, - SomaHandJointRotationArrayV0, - SomaHandJointRotationsV0, - SomaHandJointRotationsV0Record, - # SOMA evaluated hand joint-pose v0 types. - SomaHandJointPoseV0, - SomaHandJointPoseArrayV0, - SomaHandJointPosesV0, - SomaHandJointPosesV0Record, + SomaHandedness, + SomaJointRotation, + # SOMA body joint-rotation types. + SomaBodyJoint, + SomaBodyJointRotationArray, + SomaBodyJointRotations, + SomaBodyJointRotationsRecord, + # SOMA evaluated body joint-pose types. + SomaBodyJointPose, + SomaBodyJointPoseArray, + SomaBodyJointPoses, + SomaBodyJointPosesRecord, + # SOMA hand joint-rotation types. + SomaHandJoint, + SomaHandJointRotationArray, + SomaHandJointRotations, + SomaHandJointRotationsRecord, + # SOMA evaluated hand joint-pose types. + SomaHandJointPose, + SomaHandJointPoseArray, + SomaHandJointPoses, + SomaHandJointPosesRecord, ) # Deprecated aliases, resolved lazily via __getattr__ so accessing them emits a @@ -213,26 +213,26 @@ def __getattr__(name: str): "FullBodyPose", "FullBodyPoseRecord", # Shared SOMA types. - "SomaHandednessV0", - "SomaJointRotationV0", - # SOMA body joint-rotation v0 types. - "SomaBodyJointV0", - "SomaBodyJointRotationArrayV0", - "SomaBodyJointRotationsV0", - "SomaBodyJointRotationsV0Record", - # SOMA evaluated body joint-pose v0 types. - "SomaBodyJointPoseV0", - "SomaBodyJointPoseArrayV0", - "SomaBodyJointPosesV0", - "SomaBodyJointPosesV0Record", - # SOMA hand joint-rotation v0 types. - "SomaHandJointV0", - "SomaHandJointRotationArrayV0", - "SomaHandJointRotationsV0", - "SomaHandJointRotationsV0Record", - # SOMA evaluated hand joint-pose v0 types. - "SomaHandJointPoseV0", - "SomaHandJointPoseArrayV0", - "SomaHandJointPosesV0", - "SomaHandJointPosesV0Record", + "SomaHandedness", + "SomaJointRotation", + # SOMA body joint-rotation types. + "SomaBodyJoint", + "SomaBodyJointRotationArray", + "SomaBodyJointRotations", + "SomaBodyJointRotationsRecord", + # SOMA evaluated body joint-pose types. + "SomaBodyJointPose", + "SomaBodyJointPoseArray", + "SomaBodyJointPoses", + "SomaBodyJointPosesRecord", + # SOMA hand joint-rotation types. + "SomaHandJoint", + "SomaHandJointRotationArray", + "SomaHandJointRotations", + "SomaHandJointRotationsRecord", + # SOMA evaluated hand joint-pose types. + "SomaHandJointPose", + "SomaHandJointPoseArray", + "SomaHandJointPoses", + "SomaHandJointPosesRecord", ] diff --git a/tests/cpp/core/schema/CMakeLists.txt b/tests/cpp/core/schema/CMakeLists.txt index 59d25bcfb2..46290c8e45 100644 --- a/tests/cpp/core/schema/CMakeLists.txt +++ b/tests/cpp/core/schema/CMakeLists.txt @@ -16,10 +16,10 @@ add_executable(schema_tests test_joint_se3_pose.cpp test_se3_tracker.cpp test_serialized.cpp - test_soma_body_v0.cpp - test_soma_body_joint_poses_v0.cpp - test_soma_hand_v0.cpp - test_soma_hand_joint_poses_v0.cpp + test_soma_body_joint_rotations.cpp + test_soma_body_joint_poses.cpp + test_soma_hand_joint_rotations.cpp + test_soma_hand_joint_poses.cpp test_schema_conform.cpp ) target_link_libraries(schema_tests PRIVATE diff --git a/tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp b/tests/cpp/core/schema/test_soma_body_joint_poses.cpp similarity index 62% rename from tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp rename to tests/cpp/core/schema/test_soma_body_joint_poses.cpp index 6860aaa523..c364e24a44 100644 --- a/tests/cpp/core/schema/test_soma_body_joint_poses_v0.cpp +++ b/tests/cpp/core/schema/test_soma_body_joint_poses.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include @@ -18,29 +18,29 @@ constexpr flatbuffers::voffset_t vt(flatbuffers::voffset_t field) return (field + 2) * 2; } -static_assert(core::SomaBodyJointPosesV0::VT_JOINT_POSES == vt(0)); -static_assert(core::SomaBodyJointPosesV0Record::VT_DATA == vt(0)); -static_assert(core::SomaBodyJointPosesV0Record::VT_TIMESTAMP == vt(1)); -static_assert(std::is_trivially_copyable_v); -static_assert(sizeof(core::SomaBodyJointPoseArrayV0) == 77 * sizeof(core::SomaBodyJointPoseV0)); +static_assert(core::SomaBodyJointPoses::VT_JOINT_POSES == vt(0)); +static_assert(core::SomaBodyJointPosesRecord::VT_DATA == vt(0)); +static_assert(core::SomaBodyJointPosesRecord::VT_TIMESTAMP == vt(1)); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaBodyJointPoseArray) == 77 * sizeof(core::SomaBodyJointPose)); -TEST_CASE("SOMA evaluated body joint poses v0 round trip", "[soma_body_joint_poses_v0][flatbuffers]") +TEST_CASE("SOMA evaluated body joint poses round trip", "[soma_body_joint_poses][flatbuffers]") { - core::SomaBodyJointPosesV0T pose; - pose.joint_poses = std::make_shared(); + core::SomaBodyJointPosesT pose; + pose.joint_poses = std::make_shared(); pose.joint_poses->mutable_values()->Mutate( - core::SomaBodyJointV0_HEAD, - core::SomaBodyJointPoseV0( + core::SomaBodyJoint_HEAD, + core::SomaBodyJointPose( core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)), true)); flatbuffers::FlatBufferBuilder builder; - builder.Finish(core::SomaBodyJointPosesV0::Pack(builder, &pose)); + builder.Finish(core::SomaBodyJointPoses::Pack(builder, &pose)); flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); - REQUIRE(verifier.VerifyBuffer(nullptr)); + REQUIRE(verifier.VerifyBuffer(nullptr)); - const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->joint_poses() != nullptr); - const auto* head = (*decoded->joint_poses()->values())[core::SomaBodyJointV0_HEAD]; + const auto* head = (*decoded->joint_poses()->values())[core::SomaBodyJoint_HEAD]; CHECK(head->pose().position().x() == 1.0f); CHECK(head->pose().position().y() == 2.0f); CHECK(head->pose().position().z() == 3.0f); diff --git a/tests/cpp/core/schema/test_soma_body_v0.cpp b/tests/cpp/core/schema/test_soma_body_joint_rotations.cpp similarity index 54% rename from tests/cpp/core/schema/test_soma_body_v0.cpp rename to tests/cpp/core/schema/test_soma_body_joint_rotations.cpp index 5a76d9b4c2..0ca0d5b7d6 100644 --- a/tests/cpp/core/schema/test_soma_body_v0.cpp +++ b/tests/cpp/core/schema/test_soma_body_joint_rotations.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include @@ -13,60 +13,60 @@ #include #define VT(field) (field + 2) * 2 -static_assert(core::SomaBodyJointRotationsV0::VT_JOINT_ROTATIONS == VT(0)); -static_assert(core::SomaBodyJointRotationsV0::VT_GLOBAL_TRANSLATION == VT(1)); -static_assert(core::SomaBodyJointRotationsV0::VT_GLOBAL_TRANSLATION_IS_VALID == VT(2)); -static_assert(core::SomaBodyJointRotationsV0Record::VT_DATA == VT(0)); -static_assert(core::SomaBodyJointRotationsV0Record::VT_TIMESTAMP == VT(1)); - -#define TYPE(field) decltype(std::declval().field()) -static_assert(std::is_same_v); +static_assert(core::SomaBodyJointRotations::VT_JOINT_ROTATIONS == VT(0)); +static_assert(core::SomaBodyJointRotations::VT_GLOBAL_TRANSLATION == VT(1)); +static_assert(core::SomaBodyJointRotations::VT_GLOBAL_TRANSLATION_IS_VALID == VT(2)); +static_assert(core::SomaBodyJointRotationsRecord::VT_DATA == VT(0)); +static_assert(core::SomaBodyJointRotationsRecord::VT_TIMESTAMP == VT(1)); + +#define TYPE(field) decltype(std::declval().field()) +static_assert(std::is_same_v); static_assert(std::is_same_v); static_assert(std::is_same_v); -static_assert(std::is_trivially_copyable_v); -static_assert(std::is_trivially_copyable_v); -static_assert(sizeof(core::SomaBodyJointRotationArrayV0) == 77 * sizeof(core::SomaJointRotationV0)); +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaBodyJointRotationArray) == 77 * sizeof(core::SomaJointRotation)); -static_assert(core::SomaBodyJointV0_HIPS == 0); -static_assert(core::SomaBodyJointV0_LEFT_SHOULDER == 11); -static_assert(core::SomaBodyJointV0_RIGHT_SHOULDER == 39); -static_assert(core::SomaBodyJointV0_LEFT_LEG == 67); -static_assert(core::SomaBodyJointV0_RIGHT_TOE_END == 76); -static_assert(core::SomaBodyJointV0_NUM_JOINTS == 77); +static_assert(core::SomaBodyJoint_HIPS == 0); +static_assert(core::SomaBodyJoint_LEFT_SHOULDER == 11); +static_assert(core::SomaBodyJoint_RIGHT_SHOULDER == 39); +static_assert(core::SomaBodyJoint_LEFT_LEG == 67); +static_assert(core::SomaBodyJoint_RIGHT_TOE_END == 76); +static_assert(core::SomaBodyJoint_NUM_JOINTS == 77); -TEST_CASE("SOMA body v0 rotation array has the public layout", "[soma_body_v0][struct]") +TEST_CASE("SOMA body rotation array has the public layout", "[soma_body_joint_rotations][struct]") { - core::SomaBodyJointRotationArrayV0 rotations; + core::SomaBodyJointRotationArray rotations; - REQUIRE(rotations.values()->size() == static_cast(core::SomaBodyJointV0_NUM_JOINTS)); - CHECK((*rotations.values())[core::SomaBodyJointV0_HIPS]->rotation().x() == 0.0f); - CHECK_FALSE((*rotations.values())[core::SomaBodyJointV0_RIGHT_TOE_END]->is_valid()); + REQUIRE(rotations.values()->size() == static_cast(core::SomaBodyJoint_NUM_JOINTS)); + CHECK((*rotations.values())[core::SomaBodyJoint_HIPS]->rotation().x() == 0.0f); + CHECK_FALSE((*rotations.values())[core::SomaBodyJoint_RIGHT_TOE_END]->is_valid()); - const core::SomaJointRotationV0 hips(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true); - rotations.mutable_values()->Mutate(core::SomaBodyJointV0_HIPS, hips); + const core::SomaJointRotation hips(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true); + rotations.mutable_values()->Mutate(core::SomaBodyJoint_HIPS, hips); - CHECK((*rotations.values())[core::SomaBodyJointV0_HIPS]->rotation().z() == Catch::Approx(0.6f)); - CHECK((*rotations.values())[core::SomaBodyJointV0_HIPS]->rotation().w() == Catch::Approx(0.8f)); - CHECK((*rotations.values())[core::SomaBodyJointV0_HIPS]->is_valid()); + CHECK((*rotations.values())[core::SomaBodyJoint_HIPS]->rotation().z() == Catch::Approx(0.6f)); + CHECK((*rotations.values())[core::SomaBodyJoint_HIPS]->rotation().w() == Catch::Approx(0.8f)); + CHECK((*rotations.values())[core::SomaBodyJoint_HIPS]->is_valid()); } -TEST_CASE("SOMA body joint rotations v0 round trip through FlatBuffers", "[soma_body_v0][flatbuffers]") +TEST_CASE("SOMA body joint rotations round trip through FlatBuffers", "[soma_body_joint_rotations][flatbuffers]") { - core::SomaBodyJointRotationsV0T pose; - pose.joint_rotations = std::make_shared(); + core::SomaBodyJointRotationsT pose; + pose.joint_rotations = std::make_shared(); pose.joint_rotations->mutable_values()->Mutate( - core::SomaBodyJointV0_HEAD, core::SomaJointRotationV0(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true)); + core::SomaBodyJoint_HEAD, core::SomaJointRotation(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true)); pose.global_translation = std::make_shared(1.0f, 2.0f, 3.0f); pose.global_translation_is_valid = true; flatbuffers::FlatBufferBuilder builder; - builder.Finish(core::SomaBodyJointRotationsV0::Pack(builder, &pose)); + builder.Finish(core::SomaBodyJointRotations::Pack(builder, &pose)); - const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->joint_rotations() != nullptr); REQUIRE(decoded->global_translation() != nullptr); - const auto* head = (*decoded->joint_rotations()->values())[core::SomaBodyJointV0_HEAD]; + const auto* head = (*decoded->joint_rotations()->values())[core::SomaBodyJoint_HEAD]; CHECK(head->rotation().z() == Catch::Approx(0.6f)); CHECK(head->rotation().w() == Catch::Approx(0.8f)); CHECK(head->is_valid()); @@ -74,35 +74,35 @@ TEST_CASE("SOMA body joint rotations v0 round trip through FlatBuffers", "[soma_ CHECK(decoded->global_translation_is_valid()); } -TEST_CASE("SOMA body record v0 preserves quaternion components and validity", "[soma_body_v0][flatbuffers]") +TEST_CASE("SOMA body record preserves quaternion components and validity", "[soma_body_joint_rotations][flatbuffers]") { const float scale = 1.0f / std::sqrt(30.0f); - core::SomaBodyJointRotationsV0RecordT record; - record.data = std::make_shared(); - record.data->joint_rotations = std::make_shared(); + core::SomaBodyJointRotationsRecordT record; + record.data = std::make_shared(); + record.data->joint_rotations = std::make_shared(); record.data->joint_rotations->mutable_values()->Mutate( - core::SomaBodyJointV0_HIPS, - core::SomaJointRotationV0(core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale), true)); + core::SomaBodyJoint_HIPS, + core::SomaJointRotation(core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale), true)); record.data->joint_rotations->mutable_values()->Mutate( - core::SomaBodyJointV0_RIGHT_TOE_END, - core::SomaJointRotationV0(core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale), true)); + core::SomaBodyJoint_RIGHT_TOE_END, + core::SomaJointRotation(core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale), true)); record.data->global_translation = std::make_shared(1.0f, -2.0f, 3.0f); record.data->global_translation_is_valid = false; record.timestamp = std::make_shared(100, 200, 300); flatbuffers::FlatBufferBuilder builder; - builder.Finish(core::SomaBodyJointRotationsV0Record::Pack(builder, &record)); + builder.Finish(core::SomaBodyJointRotationsRecord::Pack(builder, &record)); flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); - REQUIRE(verifier.VerifyBuffer(nullptr)); + REQUIRE(verifier.VerifyBuffer(nullptr)); - const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->data() != nullptr); REQUIRE(decoded->timestamp() != nullptr); REQUIRE(decoded->data()->joint_rotations() != nullptr); REQUIRE(decoded->data()->global_translation() != nullptr); const auto* values = decoded->data()->joint_rotations()->values(); - const auto* hips = (*values)[core::SomaBodyJointV0_HIPS]; - const auto* toe = (*values)[core::SomaBodyJointV0_RIGHT_TOE_END]; + const auto* hips = (*values)[core::SomaBodyJoint_HIPS]; + const auto* toe = (*values)[core::SomaBodyJoint_RIGHT_TOE_END]; CHECK(hips->rotation().x() == Catch::Approx(scale)); CHECK(hips->rotation().y() == Catch::Approx(-2.0f * scale)); CHECK(hips->rotation().z() == Catch::Approx(3.0f * scale)); @@ -113,7 +113,7 @@ TEST_CASE("SOMA body record v0 preserves quaternion components and validity", "[ CHECK(toe->rotation().w() == Catch::Approx(4.0f * scale)); CHECK(hips->is_valid()); CHECK(toe->is_valid()); - CHECK_FALSE((*values)[core::SomaBodyJointV0_HEAD]->is_valid()); + CHECK_FALSE((*values)[core::SomaBodyJoint_HEAD]->is_valid()); CHECK(decoded->data()->global_translation()->x() == Catch::Approx(1.0f)); CHECK(decoded->data()->global_translation()->y() == Catch::Approx(-2.0f)); CHECK(decoded->data()->global_translation()->z() == Catch::Approx(3.0f)); diff --git a/tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp b/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp similarity index 52% rename from tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp rename to tests/cpp/core/schema/test_soma_hand_joint_poses.cpp index 4c5d7c6006..3e05d8375a 100644 --- a/tests/cpp/core/schema/test_soma_hand_joint_poses_v0.cpp +++ b/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include @@ -18,32 +18,32 @@ constexpr auto vt(uint16_t field_id) } // namespace -static_assert(core::SomaHandJointPosesV0::VT_JOINT_POSES == vt(0)); -static_assert(core::SomaHandJointPosesV0::VT_HANDEDNESS == vt(1)); -static_assert(core::SomaHandJointPosesV0Record::VT_DATA == vt(0)); -static_assert(core::SomaHandJointPosesV0Record::VT_TIMESTAMP == vt(1)); -static_assert(sizeof(core::SomaHandJointPoseArrayV0) == 25 * sizeof(core::SomaHandJointPoseV0)); +static_assert(core::SomaHandJointPoses::VT_JOINT_POSES == vt(0)); +static_assert(core::SomaHandJointPoses::VT_HANDEDNESS == vt(1)); +static_assert(core::SomaHandJointPosesRecord::VT_DATA == vt(0)); +static_assert(core::SomaHandJointPosesRecord::VT_TIMESTAMP == vt(1)); +static_assert(sizeof(core::SomaHandJointPoseArray) == 25 * sizeof(core::SomaHandJointPose)); -TEST_CASE("SOMA hand joint poses v0 round trip through FlatBuffers", "[soma_hand_joint_poses_v0][flatbuffers]") +TEST_CASE("SOMA hand joint poses round trip through FlatBuffers", "[soma_hand_joint_poses][flatbuffers]") { - core::SomaHandJointPosesV0T pose; - pose.joint_poses = std::make_shared(); + core::SomaHandJointPosesT pose; + pose.joint_poses = std::make_shared(); pose.joint_poses->mutable_values()->Mutate( - core::SomaHandJointV0_INDEX_END, - core::SomaHandJointPoseV0( + core::SomaHandJoint_INDEX_END, + core::SomaHandJointPose( core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)), true)); - pose.handedness = core::SomaHandednessV0_LEFT; + pose.handedness = core::SomaHandedness_LEFT; flatbuffers::FlatBufferBuilder builder; - builder.Finish(core::SomaHandJointPosesV0::Pack(builder, &pose)); + builder.Finish(core::SomaHandJointPoses::Pack(builder, &pose)); flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); - REQUIRE(verifier.VerifyBuffer(nullptr)); + REQUIRE(verifier.VerifyBuffer(nullptr)); - const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->joint_poses() != nullptr); - const auto* index_end = (*decoded->joint_poses()->values())[core::SomaHandJointV0_INDEX_END]; + const auto* index_end = (*decoded->joint_poses()->values())[core::SomaHandJoint_INDEX_END]; CHECK(index_end->pose().position().x() == Catch::Approx(1.0f)); CHECK(index_end->pose().orientation().w() == Catch::Approx(0.8f)); CHECK(index_end->is_valid()); - CHECK(decoded->handedness() == core::SomaHandednessV0_LEFT); + CHECK(decoded->handedness() == core::SomaHandedness_LEFT); } diff --git a/tests/cpp/core/schema/test_soma_hand_v0.cpp b/tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp similarity index 52% rename from tests/cpp/core/schema/test_soma_hand_v0.cpp rename to tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp index 5d9eaf82a9..f039c666ea 100644 --- a/tests/cpp/core/schema/test_soma_hand_v0.cpp +++ b/tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include @@ -13,101 +13,101 @@ #include #define VT(field) (field + 2) * 2 -static_assert(core::SomaHandJointRotationsV0::VT_JOINT_ROTATIONS == VT(0)); -static_assert(core::SomaHandJointRotationsV0::VT_GLOBAL_TRANSLATION == VT(1)); -static_assert(core::SomaHandJointRotationsV0::VT_GLOBAL_TRANSLATION_IS_VALID == VT(2)); -static_assert(core::SomaHandJointRotationsV0::VT_HANDEDNESS == VT(3)); -static_assert(core::SomaHandJointRotationsV0Record::VT_DATA == VT(0)); -static_assert(core::SomaHandJointRotationsV0Record::VT_TIMESTAMP == VT(1)); - -#define TYPE(field) decltype(std::declval().field()) -static_assert(std::is_same_v); +static_assert(core::SomaHandJointRotations::VT_JOINT_ROTATIONS == VT(0)); +static_assert(core::SomaHandJointRotations::VT_GLOBAL_TRANSLATION == VT(1)); +static_assert(core::SomaHandJointRotations::VT_GLOBAL_TRANSLATION_IS_VALID == VT(2)); +static_assert(core::SomaHandJointRotations::VT_HANDEDNESS == VT(3)); +static_assert(core::SomaHandJointRotationsRecord::VT_DATA == VT(0)); +static_assert(core::SomaHandJointRotationsRecord::VT_TIMESTAMP == VT(1)); + +#define TYPE(field) decltype(std::declval().field()) +static_assert(std::is_same_v); static_assert(std::is_same_v); static_assert(std::is_same_v); -static_assert(std::is_same_v); +static_assert(std::is_same_v); -static_assert(std::is_trivially_copyable_v); -static_assert(std::is_trivially_copyable_v); -static_assert(sizeof(core::SomaHandJointRotationArrayV0) == 25 * sizeof(core::SomaJointRotationV0)); +static_assert(std::is_trivially_copyable_v); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaHandJointRotationArray) == 25 * sizeof(core::SomaJointRotation)); -static_assert(core::SomaHandJointV0_WRIST == 0); -static_assert(core::SomaHandJointV0_INDEX1 == 5); -static_assert(core::SomaHandJointV0_MIDDLE1 == 10); -static_assert(core::SomaHandJointV0_RING1 == 15); -static_assert(core::SomaHandJointV0_PINKY_END == 24); -static_assert(core::SomaHandJointV0_NUM_JOINTS == 25); +static_assert(core::SomaHandJoint_WRIST == 0); +static_assert(core::SomaHandJoint_INDEX1 == 5); +static_assert(core::SomaHandJoint_MIDDLE1 == 10); +static_assert(core::SomaHandJoint_RING1 == 15); +static_assert(core::SomaHandJoint_PINKY_END == 24); +static_assert(core::SomaHandJoint_NUM_JOINTS == 25); -TEST_CASE("SOMA hand v0 rotation array has the public layout", "[soma_hand_v0][struct]") +TEST_CASE("SOMA hand rotation array has the public layout", "[soma_hand_joint_rotations][struct]") { - core::SomaHandJointRotationArrayV0 rotations; + core::SomaHandJointRotationArray rotations; - REQUIRE(rotations.values()->size() == static_cast(core::SomaHandJointV0_NUM_JOINTS)); - CHECK((*rotations.values())[core::SomaHandJointV0_WRIST]->rotation().x() == 0.0f); - CHECK_FALSE((*rotations.values())[core::SomaHandJointV0_PINKY_END]->is_valid()); + REQUIRE(rotations.values()->size() == static_cast(core::SomaHandJoint_NUM_JOINTS)); + CHECK((*rotations.values())[core::SomaHandJoint_WRIST]->rotation().x() == 0.0f); + CHECK_FALSE((*rotations.values())[core::SomaHandJoint_PINKY_END]->is_valid()); - const core::SomaJointRotationV0 wrist(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true); - rotations.mutable_values()->Mutate(core::SomaHandJointV0_WRIST, wrist); + const core::SomaJointRotation wrist(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true); + rotations.mutable_values()->Mutate(core::SomaHandJoint_WRIST, wrist); - CHECK((*rotations.values())[core::SomaHandJointV0_WRIST]->rotation().z() == Catch::Approx(0.6f)); - CHECK((*rotations.values())[core::SomaHandJointV0_WRIST]->rotation().w() == Catch::Approx(0.8f)); - CHECK((*rotations.values())[core::SomaHandJointV0_WRIST]->is_valid()); + CHECK((*rotations.values())[core::SomaHandJoint_WRIST]->rotation().z() == Catch::Approx(0.6f)); + CHECK((*rotations.values())[core::SomaHandJoint_WRIST]->rotation().w() == Catch::Approx(0.8f)); + CHECK((*rotations.values())[core::SomaHandJoint_WRIST]->is_valid()); } -TEST_CASE("SOMA hand joint rotations v0 round trip through FlatBuffers", "[soma_hand_v0][flatbuffers]") +TEST_CASE("SOMA hand joint rotations round trip through FlatBuffers", "[soma_hand_joint_rotations][flatbuffers]") { - core::SomaHandJointRotationsV0T pose; - pose.joint_rotations = std::make_shared(); + core::SomaHandJointRotationsT pose; + pose.joint_rotations = std::make_shared(); pose.joint_rotations->mutable_values()->Mutate( - core::SomaHandJointV0_INDEX_END, core::SomaJointRotationV0(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true)); + core::SomaHandJoint_INDEX_END, core::SomaJointRotation(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true)); pose.global_translation = std::make_shared(1.0f, 2.0f, 3.0f); pose.global_translation_is_valid = true; - pose.handedness = core::SomaHandednessV0_RIGHT; + pose.handedness = core::SomaHandedness_RIGHT; flatbuffers::FlatBufferBuilder builder; - builder.Finish(core::SomaHandJointRotationsV0::Pack(builder, &pose)); + builder.Finish(core::SomaHandJointRotations::Pack(builder, &pose)); - const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->joint_rotations() != nullptr); REQUIRE(decoded->global_translation() != nullptr); - const auto* index_end = (*decoded->joint_rotations()->values())[core::SomaHandJointV0_INDEX_END]; + const auto* index_end = (*decoded->joint_rotations()->values())[core::SomaHandJoint_INDEX_END]; CHECK(index_end->rotation().z() == Catch::Approx(0.6f)); CHECK(index_end->rotation().w() == Catch::Approx(0.8f)); CHECK(index_end->is_valid()); CHECK(decoded->global_translation()->x() == Catch::Approx(1.0f)); CHECK(decoded->global_translation_is_valid()); - CHECK(decoded->handedness() == core::SomaHandednessV0_RIGHT); + CHECK(decoded->handedness() == core::SomaHandedness_RIGHT); } -TEST_CASE("SOMA hand record v0 preserves quaternion components and validity", "[soma_hand_v0][flatbuffers]") +TEST_CASE("SOMA hand record preserves quaternion components and validity", "[soma_hand_joint_rotations][flatbuffers]") { const float scale = 1.0f / std::sqrt(30.0f); - core::SomaHandJointRotationsV0RecordT record; - record.data = std::make_shared(); - record.data->joint_rotations = std::make_shared(); + core::SomaHandJointRotationsRecordT record; + record.data = std::make_shared(); + record.data->joint_rotations = std::make_shared(); record.data->joint_rotations->mutable_values()->Mutate( - core::SomaHandJointV0_WRIST, - core::SomaJointRotationV0(core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale), true)); + core::SomaHandJoint_WRIST, + core::SomaJointRotation(core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale), true)); record.data->joint_rotations->mutable_values()->Mutate( - core::SomaHandJointV0_PINKY_END, - core::SomaJointRotationV0(core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale), true)); + core::SomaHandJoint_PINKY_END, + core::SomaJointRotation(core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale), true)); record.data->global_translation = std::make_shared(1.0f, -2.0f, 3.0f); record.data->global_translation_is_valid = false; - record.data->handedness = core::SomaHandednessV0_LEFT; + record.data->handedness = core::SomaHandedness_LEFT; record.timestamp = std::make_shared(100, 200, 300); flatbuffers::FlatBufferBuilder builder; - builder.Finish(core::SomaHandJointRotationsV0Record::Pack(builder, &record)); + builder.Finish(core::SomaHandJointRotationsRecord::Pack(builder, &record)); flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); - REQUIRE(verifier.VerifyBuffer(nullptr)); + REQUIRE(verifier.VerifyBuffer(nullptr)); - const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->data() != nullptr); REQUIRE(decoded->timestamp() != nullptr); REQUIRE(decoded->data()->joint_rotations() != nullptr); REQUIRE(decoded->data()->global_translation() != nullptr); const auto* values = decoded->data()->joint_rotations()->values(); - const auto* wrist = (*values)[core::SomaHandJointV0_WRIST]; - const auto* pinky = (*values)[core::SomaHandJointV0_PINKY_END]; + const auto* wrist = (*values)[core::SomaHandJoint_WRIST]; + const auto* pinky = (*values)[core::SomaHandJoint_PINKY_END]; CHECK(wrist->rotation().x() == Catch::Approx(scale)); CHECK(wrist->rotation().y() == Catch::Approx(-2.0f * scale)); CHECK(wrist->rotation().z() == Catch::Approx(3.0f * scale)); @@ -118,12 +118,12 @@ TEST_CASE("SOMA hand record v0 preserves quaternion components and validity", "[ CHECK(pinky->rotation().w() == Catch::Approx(4.0f * scale)); CHECK(wrist->is_valid()); CHECK(pinky->is_valid()); - CHECK_FALSE((*values)[core::SomaHandJointV0_INDEX1]->is_valid()); + CHECK_FALSE((*values)[core::SomaHandJoint_INDEX1]->is_valid()); CHECK(decoded->data()->global_translation()->x() == Catch::Approx(1.0f)); CHECK(decoded->data()->global_translation()->y() == Catch::Approx(-2.0f)); CHECK(decoded->data()->global_translation()->z() == Catch::Approx(3.0f)); CHECK_FALSE(decoded->data()->global_translation_is_valid()); - CHECK(decoded->data()->handedness() == core::SomaHandednessV0_LEFT); + CHECK(decoded->data()->handedness() == core::SomaHandedness_LEFT); CHECK(decoded->timestamp()->available_time_local_common_clock() == 100); CHECK(decoded->timestamp()->sample_time_local_common_clock() == 200); CHECK(decoded->timestamp()->sample_time_raw_device_clock() == 300); diff --git a/tests/python/core/retargeting_engine/test_soma_body_source.py b/tests/python/core/retargeting_engine/test_soma_body_source.py index 429534f204..3b6dfb37a7 100644 --- a/tests/python/core/retargeting_engine/test_soma_body_source.py +++ b/tests/python/core/retargeting_engine/test_soma_body_source.py @@ -7,13 +7,13 @@ import pytest from isaaccapture.deviceio_trackers import ( - SomaBodyJointPosesV0Tracker, - SomaBodyJointRotationsV0Tracker, + SomaBodyJointPosesTracker, + SomaBodyJointRotationsTracker, ) from isaaccapture.retargeting_engine.deviceio_source_nodes import ( DeviceIOFullBodyPoseTracked, - DeviceIOSomaBodyJointPosesV0Tracked, - DeviceIOSomaBodyJointRotationsV0Tracked, + DeviceIOSomaBodyJointPosesTracked, + DeviceIOSomaBodyJointRotationsTracked, SomaBodyRepresentation, SomaBodySource, ) @@ -24,19 +24,19 @@ BodyJoints, FullBodyPose, Point, - SomaBodyJointPoseArrayV0, - SomaBodyJointPosesV0, - SomaBodyJointRotationArrayV0, - SomaBodyJointRotationsV0, + SomaBodyJointPoseArray, + SomaBodyJointPoses, + SomaBodyJointRotationArray, + SomaBodyJointRotations, ) from isaaccapture.teleop_session_manager.helpers import _get_trackers_from_pipeline def pose(): - joints = SomaBodyJointRotationArrayV0() + joints = SomaBodyJointRotationArray() joints.rotations[:] = (0, 0, 0, 1) joints.is_valid[:] = 1 - return SomaBodyJointRotationsV0(joints, Point(1, 2, 3), True) + return SomaBodyJointRotations(joints, Point(1, 2, 3), True) def fake_layer(): @@ -51,11 +51,11 @@ def source(): def evaluated_pose(): - joints = SomaBodyJointPoseArrayV0() + joints = SomaBodyJointPoseArray() joints.positions[:] = np.arange(231, dtype=np.float32).reshape(77, 3) joints.orientations[:] = np.tile([0, 0, 0, 1], (77, 1)) joints.is_valid[:] = 1 - return SomaBodyJointPosesV0(joints) + return SomaBodyJointPoses(joints) def payload_group(group_type, data): @@ -66,7 +66,7 @@ def payload_group(group_type, data): def test_tracker_discovery_and_specs(): soma_source = source() - assert isinstance(soma_source.get_tracker(), SomaBodyJointRotationsV0Tracker) + assert isinstance(soma_source.get_tracker(), SomaBodyJointRotationsTracker) assert soma_source.get_vendor() is None assert _get_trackers_from_pipeline(soma_source) == [soma_source.get_tracker()] assert list(soma_source.input_spec()) == ["deviceio_soma_body"] @@ -119,7 +119,7 @@ def test_joint_pose_source_maps_received_payload_without_fk(): "vendor.soma", representation=SomaBodyRepresentation.JOINT_POSES, ) - assert isinstance(soma_source.get_tracker(), SomaBodyJointPosesV0Tracker) + assert isinstance(soma_source.get_tracker(), SomaBodyJointPosesTracker) raw = evaluated_pose() assert soma_source._evaluator is None inputs = payload_group(soma_source.input_spec()["deviceio_soma_body"], raw) @@ -164,18 +164,18 @@ def test_absent_sample_clears_reused_output(): def test_soma_and_fullbody_payloads_are_distinct(): - soma = DeviceIOSomaBodyJointRotationsV0Tracked() - soma_poses = DeviceIOSomaBodyJointPosesV0Tracked() + soma = DeviceIOSomaBodyJointRotationsTracked() + soma_poses = DeviceIOSomaBodyJointPosesTracked() fullbody = DeviceIOFullBodyPoseTracked() with pytest.raises(ValueError, match="type mismatch"): soma.check_compatibility(fullbody) - with pytest.raises(TypeError, match="SomaBodyJointRotationsV0"): + with pytest.raises(TypeError, match="SomaBodyJointRotations"): payload_group(soma, FullBodyPose(BodyJoints(), False)) with pytest.raises(TypeError, match="FullBodyPose"): payload_group(fullbody, pose()) with pytest.raises(ValueError, match="type mismatch"): soma.check_compatibility(soma_poses) - with pytest.raises(TypeError, match="SomaBodyJointPosesV0"): + with pytest.raises(TypeError, match="SomaBodyJointPoses"): payload_group(soma_poses, pose()) diff --git a/tests/python/core/retargeting_engine/test_soma_hand_source.py b/tests/python/core/retargeting_engine/test_soma_hand_source.py index 12f6fbf89a..aae9d29603 100644 --- a/tests/python/core/retargeting_engine/test_soma_hand_source.py +++ b/tests/python/core/retargeting_engine/test_soma_hand_source.py @@ -7,12 +7,12 @@ import pytest from isaaccapture.deviceio_trackers import ( - SomaHandJointPosesV0Tracker, - SomaHandJointRotationsV0Tracker, + SomaHandJointPosesTracker, + SomaHandJointRotationsTracker, ) from isaaccapture.retargeting_engine.deviceio_source_nodes import ( - DeviceIOSomaHandJointPosesV0Tracked, - DeviceIOSomaHandJointRotationsV0Tracked, + DeviceIOSomaHandJointPosesTracked, + DeviceIOSomaHandJointRotationsTracked, SomaHandRepresentation, SomaHandSource, ) @@ -20,11 +20,11 @@ from isaaccapture.retargeting_engine.tensor_types import SomaHandInputIndex from isaaccapture.schema import ( Point, - SomaHandednessV0, - SomaHandJointPoseArrayV0, - SomaHandJointPosesV0, - SomaHandJointRotationArrayV0, - SomaHandJointRotationsV0, + SomaHandedness, + SomaHandJointPoseArray, + SomaHandJointPoses, + SomaHandJointRotationArray, + SomaHandJointRotations, ) @@ -35,19 +35,19 @@ def fake_layer(): return layer -def rotations(handedness=SomaHandednessV0.LEFT): - joints = SomaHandJointRotationArrayV0() +def rotations(handedness=SomaHandedness.LEFT): + joints = SomaHandJointRotationArray() joints.rotations[:] = (0, 0, 0, 1) joints.is_valid[:] = 1 - return SomaHandJointRotationsV0(joints, Point(1, 2, 3), True, handedness) + return SomaHandJointRotations(joints, Point(1, 2, 3), True, handedness) -def poses(handedness=SomaHandednessV0.LEFT): - joints = SomaHandJointPoseArrayV0() +def poses(handedness=SomaHandedness.LEFT): + joints = SomaHandJointPoseArray() joints.positions[:] = np.arange(75, dtype=np.float32).reshape(25, 3) joints.orientations[:] = np.tile([0, 0, 0, 1], (25, 1)) joints.is_valid[:] = 1 - return SomaHandJointPosesV0(joints, handedness) + return SomaHandJointPoses(joints, handedness) def payload_group(group_type, data): @@ -57,8 +57,8 @@ def payload_group(group_type, data): def test_rotation_source_evaluates_received_payload(): - source = SomaHandSource("left", "vendor.left", SomaHandednessV0.LEFT, fake_layer()) - assert isinstance(source.get_tracker(), SomaHandJointRotationsV0Tracker) + source = SomaHandSource("left", "vendor.left", SomaHandedness.LEFT, fake_layer()) + assert isinstance(source.get_tracker(), SomaHandJointRotationsTracker) raw = rotations() expected = ( np.ones((25, 3), dtype=np.float32), @@ -86,10 +86,10 @@ def test_joint_pose_source_maps_without_fk(): source = SomaHandSource( "left", "vendor.left", - SomaHandednessV0.LEFT, + SomaHandedness.LEFT, representation=SomaHandRepresentation.JOINT_POSES, ) - assert isinstance(source.get_tracker(), SomaHandJointPosesV0Tracker) + assert isinstance(source.get_tracker(), SomaHandJointPosesTracker) raw = poses() inputs = payload_group(source.input_spec()["deviceio_soma_hand"], raw) @@ -108,32 +108,32 @@ def test_payload_handedness_must_match_collection(): source = SomaHandSource( "left", "vendor.left", - SomaHandednessV0.LEFT, + SomaHandedness.LEFT, representation=SomaHandRepresentation.JOINT_POSES, ) inputs = payload_group( - source.input_spec()["deviceio_soma_hand"], poses(SomaHandednessV0.RIGHT) + source.input_spec()["deviceio_soma_hand"], poses(SomaHandedness.RIGHT) ) with pytest.raises(ValueError, match="handedness"): source({"deviceio_soma_hand": inputs}) def test_hand_transport_profiles_are_distinct(): - rotation_type = DeviceIOSomaHandJointRotationsV0Tracked() - pose_type = DeviceIOSomaHandJointPosesV0Tracked() + rotation_type = DeviceIOSomaHandJointRotationsTracked() + pose_type = DeviceIOSomaHandJointPosesTracked() with pytest.raises(ValueError, match="type mismatch"): rotation_type.check_compatibility(pose_type) - with pytest.raises(TypeError, match="SomaHandJointPosesV0"): + with pytest.raises(TypeError, match="SomaHandJointPoses"): payload_group(pose_type, rotations()) def test_rotation_source_requires_layer_and_valid_side(): with pytest.raises(ValueError, match="prepared SOMA hand layer"): - SomaHandSource("left", "vendor.left", SomaHandednessV0.LEFT) + SomaHandSource("left", "vendor.left", SomaHandedness.LEFT) with pytest.raises(ValueError, match="LEFT or RIGHT"): SomaHandSource( "left", "vendor.left", - SomaHandednessV0.UNSPECIFIED, + SomaHandedness.UNSPECIFIED, fake_layer(), ) diff --git a/tests/python/core/schema/test_soma_body_joint_poses_v0.py b/tests/python/core/schema/test_soma_body_joint_poses.py similarity index 68% rename from tests/python/core/schema/test_soma_body_joint_poses_v0.py rename to tests/python/core/schema/test_soma_body_joint_poses.py index 96bdbe99b6..27cfd9e44a 100644 --- a/tests/python/core/schema/test_soma_body_joint_poses_v0.py +++ b/tests/python/core/schema/test_soma_body_joint_poses.py @@ -6,41 +6,41 @@ from isaaccapture.schema import ( DeviceDataTimestamp, - SomaBodyJointPoseArrayV0, - SomaBodyJointPosesV0, - SomaBodyJointPosesV0Record, - SomaBodyJointV0, + SomaBodyJointPoseArray, + SomaBodyJointPoses, + SomaBodyJointPosesRecord, + SomaBodyJoint, ) def test_soma_body_joint_pose_views_alias_storage(): - joints = SomaBodyJointPoseArrayV0() + joints = SomaBodyJointPoseArray() positions = np.arange(231, dtype=np.float32).reshape(77, 3) orientations = np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)) joints.positions[:] = positions joints.orientations[:] = orientations - joints.is_valid[int(SomaBodyJointV0.HEAD)] = 1 + joints.is_valid[int(SomaBodyJoint.HEAD)] = 1 np.testing.assert_array_equal(joints.positions, positions) np.testing.assert_array_equal(joints.orientations, orientations) - head = joints.values(int(SomaBodyJointV0.HEAD)) - assert head.pose.position.x == positions[int(SomaBodyJointV0.HEAD), 0] + head = joints.values(int(SomaBodyJoint.HEAD)) + assert head.pose.position.x == positions[int(SomaBodyJoint.HEAD), 0] assert head.pose.orientation.w == 1.0 assert head.is_valid def test_soma_body_joint_pose_index_check(): with pytest.raises(IndexError): - SomaBodyJointPoseArrayV0().values(77) + SomaBodyJointPoseArray().values(77) def test_soma_body_joint_poses_record_round_trip(): - joints = SomaBodyJointPoseArrayV0() - joints.positions[int(SomaBodyJointV0.HIPS)] = [1.0, 2.0, 3.0] + joints = SomaBodyJointPoseArray() + joints.positions[int(SomaBodyJoint.HIPS)] = [1.0, 2.0, 3.0] joints.orientations[:, 3] = 1.0 joints.is_valid[:] = 1 - pose = SomaBodyJointPosesV0(joints) - record = SomaBodyJointPosesV0Record(pose, DeviceDataTimestamp(10, 20, 30)) + pose = SomaBodyJointPoses(joints) + record = SomaBodyJointPosesRecord(pose, DeviceDataTimestamp(10, 20, 30)) np.testing.assert_array_equal(record.data.joint_poses.positions, joints.positions) np.testing.assert_array_equal( diff --git a/tests/python/core/schema/test_soma_body_v0.py b/tests/python/core/schema/test_soma_body_joint_rotations.py similarity index 71% rename from tests/python/core/schema/test_soma_body_v0.py rename to tests/python/core/schema/test_soma_body_joint_rotations.py index c069dacdaf..ac36472f87 100644 --- a/tests/python/core/schema/test_soma_body_v0.py +++ b/tests/python/core/schema/test_soma_body_joint_rotations.py @@ -10,15 +10,15 @@ DeviceDataTimestamp, Point, Quaternion, - SomaBodyJointV0, - SomaBodyJointRotationArrayV0, - SomaBodyJointRotationsV0, - SomaBodyJointRotationsV0Record, - SomaJointRotationV0, + SomaBodyJoint, + SomaBodyJointRotationArray, + SomaBodyJointRotations, + SomaBodyJointRotationsRecord, + SomaJointRotation, ) -def test_soma_body_v0_joint_order(): +def test_soma_body_joint_rotations_joint_order(): expected = [ "HIPS", "SPINE1", @@ -99,13 +99,13 @@ def test_soma_body_v0_joint_order(): "RIGHT_TOE_END", ] - assert len(expected) == int(SomaBodyJointV0.NUM_JOINTS) == 77 + assert len(expected) == int(SomaBodyJoint.NUM_JOINTS) == 77 for index, name in enumerate(expected): - assert int(getattr(SomaBodyJointV0, name)) == index + assert int(getattr(SomaBodyJoint, name)) == index -def test_soma_body_v0_joint_rotation(): - rotation = SomaJointRotationV0(Quaternion(0.0, 0.0, 0.6, 0.8), True) +def test_soma_body_joint_rotations_joint_rotation(): + rotation = SomaJointRotation(Quaternion(0.0, 0.0, 0.6, 0.8), True) assert rotation.rotation.x == 0.0 assert rotation.rotation.y == 0.0 @@ -114,8 +114,8 @@ def test_soma_body_v0_joint_rotation(): assert rotation.is_valid is True -def test_soma_body_v0_rotation_views_alias_storage(): - rotations = SomaBodyJointRotationArrayV0() +def test_soma_body_joint_rotations_rotation_views_alias_storage(): + rotations = SomaBodyJointRotationArray() assert rotations.rotations.shape == (77, 4) assert rotations.rotations.dtype == np.float32 @@ -123,27 +123,27 @@ def test_soma_body_v0_rotation_views_alias_storage(): assert rotations.is_valid.dtype == np.uint8 assert not rotations.rotations.flags.owndata - rotations.rotations[SomaBodyJointV0.HEAD] = [0.0, 0.0, 0.6, 0.8] - rotations.is_valid[SomaBodyJointV0.HEAD] = 1 + rotations.rotations[SomaBodyJoint.HEAD] = [0.0, 0.0, 0.6, 0.8] + rotations.is_valid[SomaBodyJoint.HEAD] = 1 - head = rotations.values(int(SomaBodyJointV0.HEAD)) + head = rotations.values(int(SomaBodyJoint.HEAD)) assert head.rotation.z == pytest.approx(0.6) assert head.rotation.w == pytest.approx(0.8) assert head.is_valid is True -def test_soma_body_v0_rotation_index_check(): +def test_soma_body_joint_rotations_rotation_index_check(): with pytest.raises(IndexError): - SomaBodyJointRotationArrayV0().values(77) + SomaBodyJointRotationArray().values(77) -def test_soma_body_v0_pose_construction_and_lifetime(): - rotations = SomaBodyJointRotationArrayV0() +def test_soma_body_joint_rotations_pose_construction_and_lifetime(): + rotations = SomaBodyJointRotationArray() rotations.rotations[:] = [0.0, 0.0, 0.0, 1.0] - rotations.rotations[SomaBodyJointV0.RIGHT_TOE_END] = [0.0, 0.0, 0.6, 0.8] + rotations.rotations[SomaBodyJoint.RIGHT_TOE_END] = [0.0, 0.0, 0.6, 0.8] rotations.is_valid[:] = 1 - pose = SomaBodyJointRotationsV0(rotations, Point(1.0, 2.0, 3.0), True) + pose = SomaBodyJointRotations(rotations, Point(1.0, 2.0, 3.0), True) quaternion_rotations = pose.joint_rotations.rotations del pose gc.collect() @@ -151,14 +151,14 @@ def test_soma_body_v0_pose_construction_and_lifetime(): assert quaternion_rotations[76, 3] == pytest.approx(0.8) -def test_soma_body_v0_pose_defaults_and_global_translation(): - pose = SomaBodyJointRotationsV0() +def test_soma_body_joint_rotations_pose_defaults_and_global_translation(): + pose = SomaBodyJointRotations() assert pose.joint_rotations is not None assert pose.global_translation.x == 0.0 assert pose.global_translation_is_valid is False - translated = SomaBodyJointRotationsV0( + translated = SomaBodyJointRotations( global_translation=Point(1.0, 2.0, 3.0), global_translation_is_valid=True, ) @@ -166,9 +166,9 @@ def test_soma_body_v0_pose_defaults_and_global_translation(): assert translated.global_translation_is_valid is True -def test_soma_body_v0_pose_record(): - record = SomaBodyJointRotationsV0Record( - SomaBodyJointRotationsV0(), DeviceDataTimestamp(100, 200, 300) +def test_soma_body_joint_rotations_pose_record(): + record = SomaBodyJointRotationsRecord( + SomaBodyJointRotations(), DeviceDataTimestamp(100, 200, 300) ) assert record.data is not None @@ -177,20 +177,22 @@ def test_soma_body_v0_pose_record(): @pytest.mark.parametrize("sign", [-1.0, 1.0]) @pytest.mark.parametrize("translation_is_valid", [False, True]) -def test_soma_body_v0_record_preserves_quaternion_layout(sign, translation_is_valid): +def test_soma_body_joint_rotations_record_preserves_quaternion_layout( + sign, translation_is_valid +): quaternion = np.array([1.0, -2.0, 3.0, -4.0], dtype=np.float32) quaternion *= sign / np.linalg.norm(quaternion) - rotations = SomaBodyJointRotationArrayV0() - rotations.rotations[SomaBodyJointV0.HIPS] = quaternion - rotations.rotations[SomaBodyJointV0.RIGHT_TOE_END] = -quaternion - rotations.is_valid[SomaBodyJointV0.HIPS] = 1 - rotations.is_valid[SomaBodyJointV0.RIGHT_TOE_END] = 1 + rotations = SomaBodyJointRotationArray() + rotations.rotations[SomaBodyJoint.HIPS] = quaternion + rotations.rotations[SomaBodyJoint.RIGHT_TOE_END] = -quaternion + rotations.is_valid[SomaBodyJoint.HIPS] = 1 + rotations.is_valid[SomaBodyJoint.RIGHT_TOE_END] = 1 expected_rotations = rotations.rotations.copy() expected_validity = rotations.is_valid.copy() - pose = SomaBodyJointRotationsV0( + pose = SomaBodyJointRotations( rotations, Point(1.0, -2.0, 3.0), translation_is_valid ) - record = SomaBodyJointRotationsV0Record(pose, DeviceDataTimestamp(100, 200, 300)) + record = SomaBodyJointRotationsRecord(pose, DeviceDataTimestamp(100, 200, 300)) rotations.rotations[:] = 0.0 rotations.is_valid[:] = 0 @@ -202,7 +204,7 @@ def test_soma_body_v0_record_preserves_quaternion_layout(sign, translation_is_va data = record.data np.testing.assert_array_equal(data.joint_rotations.rotations, expected_rotations) np.testing.assert_array_equal(data.joint_rotations.is_valid, expected_validity) - hips = data.joint_rotations.values(int(SomaBodyJointV0.HIPS)).rotation + hips = data.joint_rotations.values(int(SomaBodyJoint.HIPS)).rotation np.testing.assert_array_equal([hips.x, hips.y, hips.z, hips.w], quaternion) assert data.global_translation.x == pytest.approx(1.0) assert data.global_translation.y == pytest.approx(-2.0) diff --git a/tests/python/core/schema/test_soma_hand_joint_poses_v0.py b/tests/python/core/schema/test_soma_hand_joint_poses.py similarity index 63% rename from tests/python/core/schema/test_soma_hand_joint_poses_v0.py rename to tests/python/core/schema/test_soma_hand_joint_poses.py index 0073939b36..82eaddb23a 100644 --- a/tests/python/core/schema/test_soma_hand_joint_poses_v0.py +++ b/tests/python/core/schema/test_soma_hand_joint_poses.py @@ -8,16 +8,16 @@ from isaaccapture.schema import ( DeviceDataTimestamp, - SomaHandednessV0, - SomaHandJointPoseArrayV0, - SomaHandJointPosesV0, - SomaHandJointPosesV0Record, - SomaHandJointV0, + SomaHandedness, + SomaHandJointPoseArray, + SomaHandJointPoses, + SomaHandJointPosesRecord, + SomaHandJoint, ) def test_soma_hand_joint_pose_views_alias_storage(): - poses = SomaHandJointPoseArrayV0() + poses = SomaHandJointPoseArray() assert poses.positions.shape == (25, 3) assert poses.orientations.shape == (25, 4) @@ -26,29 +26,29 @@ def test_soma_hand_joint_pose_views_alias_storage(): assert poses.orientations.dtype == np.float32 assert poses.is_valid.dtype == np.uint8 - poses.positions[SomaHandJointV0.INDEX_END] = [1.0, 2.0, 3.0] - poses.orientations[SomaHandJointV0.INDEX_END] = [0.0, 0.0, 0.6, 0.8] - poses.is_valid[SomaHandJointV0.INDEX_END] = 1 + poses.positions[SomaHandJoint.INDEX_END] = [1.0, 2.0, 3.0] + poses.orientations[SomaHandJoint.INDEX_END] = [0.0, 0.0, 0.6, 0.8] + poses.is_valid[SomaHandJoint.INDEX_END] = 1 - index_end = poses.values(int(SomaHandJointV0.INDEX_END)) + index_end = poses.values(int(SomaHandJoint.INDEX_END)) assert index_end.pose.position.y == pytest.approx(2.0) assert index_end.pose.orientation.w == pytest.approx(0.8) assert index_end.is_valid is True def test_soma_hand_joint_poses_record_lifetime(): - poses = SomaHandJointPoseArrayV0() + poses = SomaHandJointPoseArray() poses.positions[:] = np.arange(75, dtype=np.float32).reshape(25, 3) poses.orientations[:, 3] = 1.0 poses.is_valid[:] = 1 - payload = SomaHandJointPosesV0(poses, SomaHandednessV0.RIGHT) - record = SomaHandJointPosesV0Record(payload, DeviceDataTimestamp(100, 200, 300)) + payload = SomaHandJointPoses(poses, SomaHandedness.RIGHT) + record = SomaHandJointPosesRecord(payload, DeviceDataTimestamp(100, 200, 300)) poses.positions[:] = 0.0 payload.joint_poses.positions[:] = 0.0 del poses, payload gc.collect() - assert record.data.handedness == SomaHandednessV0.RIGHT + assert record.data.handedness == SomaHandedness.RIGHT assert record.data.joint_poses.positions[24, 2] == pytest.approx(74.0) assert record.timestamp.sample_time_raw_device_clock == 300 diff --git a/tests/python/core/schema/test_soma_hand_v0.py b/tests/python/core/schema/test_soma_hand_joint_rotations.py similarity index 61% rename from tests/python/core/schema/test_soma_hand_v0.py rename to tests/python/core/schema/test_soma_hand_joint_rotations.py index 235463307c..e3b6367545 100644 --- a/tests/python/core/schema/test_soma_hand_v0.py +++ b/tests/python/core/schema/test_soma_hand_joint_rotations.py @@ -10,16 +10,16 @@ DeviceDataTimestamp, Point, Quaternion, - SomaHandednessV0, - SomaHandJointRotationArrayV0, - SomaHandJointRotationsV0, - SomaHandJointRotationsV0Record, - SomaHandJointV0, - SomaJointRotationV0, + SomaHandedness, + SomaHandJointRotationArray, + SomaHandJointRotations, + SomaHandJointRotationsRecord, + SomaHandJoint, + SomaJointRotation, ) -def test_soma_hand_v0_joint_order(): +def test_soma_hand_joint_rotations_joint_order(): expected = [ "WRIST", "THUMB1", @@ -48,13 +48,13 @@ def test_soma_hand_v0_joint_order(): "PINKY_END", ] - assert len(expected) == int(SomaHandJointV0.NUM_JOINTS) == 25 + assert len(expected) == int(SomaHandJoint.NUM_JOINTS) == 25 for index, name in enumerate(expected): - assert int(getattr(SomaHandJointV0, name)) == index + assert int(getattr(SomaHandJoint, name)) == index -def test_soma_hand_v0_joint_rotation(): - rotation = SomaJointRotationV0(Quaternion(0.0, 0.0, 0.6, 0.8), True) +def test_soma_hand_joint_rotations_joint_rotation(): + rotation = SomaJointRotation(Quaternion(0.0, 0.0, 0.6, 0.8), True) assert rotation.rotation.x == 0.0 assert rotation.rotation.y == 0.0 @@ -63,8 +63,8 @@ def test_soma_hand_v0_joint_rotation(): assert rotation.is_valid is True -def test_soma_hand_v0_rotation_views_alias_storage(): - rotations = SomaHandJointRotationArrayV0() +def test_soma_hand_joint_rotations_rotation_views_alias_storage(): + rotations = SomaHandJointRotationArray() assert rotations.rotations.shape == (25, 4) assert rotations.rotations.dtype == np.float32 @@ -72,28 +72,28 @@ def test_soma_hand_v0_rotation_views_alias_storage(): assert rotations.is_valid.dtype == np.uint8 assert not rotations.rotations.flags.owndata - rotations.rotations[SomaHandJointV0.INDEX_END] = [0.0, 0.0, 0.6, 0.8] - rotations.is_valid[SomaHandJointV0.INDEX_END] = 1 + rotations.rotations[SomaHandJoint.INDEX_END] = [0.0, 0.0, 0.6, 0.8] + rotations.is_valid[SomaHandJoint.INDEX_END] = 1 - index_end = rotations.values(int(SomaHandJointV0.INDEX_END)) + index_end = rotations.values(int(SomaHandJoint.INDEX_END)) assert index_end.rotation.z == pytest.approx(0.6) assert index_end.rotation.w == pytest.approx(0.8) assert index_end.is_valid is True -def test_soma_hand_v0_rotation_index_check(): +def test_soma_hand_joint_rotations_rotation_index_check(): with pytest.raises(IndexError): - SomaHandJointRotationArrayV0().values(25) + SomaHandJointRotationArray().values(25) -def test_soma_hand_v0_pose_construction_and_lifetime(): - rotations = SomaHandJointRotationArrayV0() +def test_soma_hand_joint_rotations_pose_construction_and_lifetime(): + rotations = SomaHandJointRotationArray() rotations.rotations[:] = [0.0, 0.0, 0.0, 1.0] - rotations.rotations[SomaHandJointV0.PINKY_END] = [0.0, 0.0, 0.6, 0.8] + rotations.rotations[SomaHandJoint.PINKY_END] = [0.0, 0.0, 0.6, 0.8] rotations.is_valid[:] = 1 - pose = SomaHandJointRotationsV0( - rotations, Point(1.0, 2.0, 3.0), True, SomaHandednessV0.RIGHT + pose = SomaHandJointRotations( + rotations, Point(1.0, 2.0, 3.0), True, SomaHandedness.RIGHT ) quaternion_rotations = pose.joint_rotations.rotations del pose @@ -102,54 +102,54 @@ def test_soma_hand_v0_pose_construction_and_lifetime(): assert quaternion_rotations[24, 3] == pytest.approx(0.8) -def test_soma_hand_v0_pose_defaults_and_global_translation(): - pose = SomaHandJointRotationsV0() +def test_soma_hand_joint_rotations_pose_defaults_and_global_translation(): + pose = SomaHandJointRotations() assert pose.joint_rotations is not None assert pose.global_translation.x == 0.0 assert pose.global_translation_is_valid is False - assert pose.handedness == SomaHandednessV0.UNSPECIFIED + assert pose.handedness == SomaHandedness.UNSPECIFIED - right = SomaHandJointRotationsV0( + right = SomaHandJointRotations( global_translation=Point(1.0, 2.0, 3.0), global_translation_is_valid=True, - handedness=SomaHandednessV0.RIGHT, + handedness=SomaHandedness.RIGHT, ) assert right.global_translation.y == pytest.approx(2.0) assert right.global_translation_is_valid is True - assert right.handedness == SomaHandednessV0.RIGHT + assert right.handedness == SomaHandedness.RIGHT -def test_soma_hand_v0_pose_record(): - record = SomaHandJointRotationsV0Record( - SomaHandJointRotationsV0(handedness=SomaHandednessV0.LEFT), +def test_soma_hand_joint_rotations_pose_record(): + record = SomaHandJointRotationsRecord( + SomaHandJointRotations(handedness=SomaHandedness.LEFT), DeviceDataTimestamp(100, 200, 300), ) assert record.data is not None - assert record.data.handedness == SomaHandednessV0.LEFT + assert record.data.handedness == SomaHandedness.LEFT assert record.timestamp.sample_time_local_common_clock == 200 @pytest.mark.parametrize("sign", [-1.0, 1.0]) @pytest.mark.parametrize("translation_is_valid", [False, True]) -@pytest.mark.parametrize("handedness", [SomaHandednessV0.LEFT, SomaHandednessV0.RIGHT]) -def test_soma_hand_v0_record_preserves_quaternion_layout( +@pytest.mark.parametrize("handedness", [SomaHandedness.LEFT, SomaHandedness.RIGHT]) +def test_soma_hand_joint_rotations_record_preserves_quaternion_layout( sign, translation_is_valid, handedness ): quaternion = np.array([1.0, -2.0, 3.0, -4.0], dtype=np.float32) quaternion *= sign / np.linalg.norm(quaternion) - rotations = SomaHandJointRotationArrayV0() - rotations.rotations[SomaHandJointV0.WRIST] = quaternion - rotations.rotations[SomaHandJointV0.PINKY_END] = -quaternion - rotations.is_valid[SomaHandJointV0.WRIST] = 1 - rotations.is_valid[SomaHandJointV0.PINKY_END] = 1 + rotations = SomaHandJointRotationArray() + rotations.rotations[SomaHandJoint.WRIST] = quaternion + rotations.rotations[SomaHandJoint.PINKY_END] = -quaternion + rotations.is_valid[SomaHandJoint.WRIST] = 1 + rotations.is_valid[SomaHandJoint.PINKY_END] = 1 expected_rotations = rotations.rotations.copy() expected_validity = rotations.is_valid.copy() - pose = SomaHandJointRotationsV0( + pose = SomaHandJointRotations( rotations, Point(1.0, -2.0, 3.0), translation_is_valid, handedness ) - record = SomaHandJointRotationsV0Record(pose, DeviceDataTimestamp(100, 200, 300)) + record = SomaHandJointRotationsRecord(pose, DeviceDataTimestamp(100, 200, 300)) rotations.rotations[:] = 0.0 rotations.is_valid[:] = 0 @@ -161,7 +161,7 @@ def test_soma_hand_v0_record_preserves_quaternion_layout( data = record.data np.testing.assert_array_equal(data.joint_rotations.rotations, expected_rotations) np.testing.assert_array_equal(data.joint_rotations.is_valid, expected_validity) - wrist = data.joint_rotations.values(int(SomaHandJointV0.WRIST)).rotation + wrist = data.joint_rotations.values(int(SomaHandJoint.WRIST)).rotation np.testing.assert_array_equal([wrist.x, wrist.y, wrist.z, wrist.w], quaternion) assert data.global_translation.x == pytest.approx(1.0) assert data.global_translation.y == pytest.approx(-2.0) diff --git a/tests/python/examples/deviceio_live_view/test_soma_viewer.py b/tests/python/examples/deviceio_live_view/test_soma_viewer.py index ad4fc65f3e..763a846a1b 100644 --- a/tests/python/examples/deviceio_live_view/test_soma_viewer.py +++ b/tests/python/examples/deviceio_live_view/test_soma_viewer.py @@ -6,10 +6,10 @@ import numpy as np import pytest from isaaccapture.deviceio_trackers import ( - SomaBodyJointPosesV0Tracker, - SomaBodyJointRotationsV0Tracker, - SomaHandJointPosesV0Tracker, - SomaHandJointRotationsV0Tracker, + SomaBodyJointPosesTracker, + SomaBodyJointRotationsTracker, + SomaHandJointPosesTracker, + SomaHandJointRotationsTracker, ) from isaaccapture.retargeting_engine.deviceio_source_nodes import ( FullBodySource, @@ -29,15 +29,15 @@ ) from isaaccapture.schema import ( Point, - SomaBodyJointPoseArrayV0, - SomaBodyJointPosesV0, - SomaBodyJointRotationArrayV0, - SomaBodyJointRotationsV0, - SomaHandednessV0, - SomaHandJointPoseArrayV0, - SomaHandJointPosesV0, - SomaHandJointRotationArrayV0, - SomaHandJointRotationsV0, + SomaBodyJointPoseArray, + SomaBodyJointPoses, + SomaBodyJointRotationArray, + SomaBodyJointRotations, + SomaHandedness, + SomaHandJointPoseArray, + SomaHandJointPoses, + SomaHandJointRotationArray, + SomaHandJointRotations, ) from isaaccapture_examples.deviceio_live_view import live_deviceio, soma_body from isaaccapture_examples.deviceio_live_view.body_pipeline import ( @@ -74,33 +74,33 @@ def fake_layer(): def soma_joint_rotations(quaternions, translation): - joints = SomaBodyJointRotationArrayV0() + joints = SomaBodyJointRotationArray() joints.rotations[:] = quaternions joints.is_valid[:] = 1 - return SomaBodyJointRotationsV0(joints, Point(*translation), True) + return SomaBodyJointRotations(joints, Point(*translation), True) def soma_joint_poses(positions, orientations): - joints = SomaBodyJointPoseArrayV0() + joints = SomaBodyJointPoseArray() joints.positions[:] = positions joints.orientations[:] = orientations joints.is_valid[:] = 1 - return SomaBodyJointPosesV0(joints) + return SomaBodyJointPoses(joints) def soma_hand_joint_poses(positions, orientations, handedness): - joints = SomaHandJointPoseArrayV0() + joints = SomaHandJointPoseArray() joints.positions[:] = positions joints.orientations[:] = orientations joints.is_valid[:] = 1 - return SomaHandJointPosesV0(joints, handedness) + return SomaHandJointPoses(joints, handedness) def soma_hand_joint_rotations(rotations, translation, handedness): - joints = SomaHandJointRotationArrayV0() + joints = SomaHandJointRotationArray() joints.rotations[:] = rotations joints.is_valid[:] = 1 - return SomaHandJointRotationsV0(joints, Point(*translation), True, handedness) + return SomaHandJointRotations(joints, Point(*translation), True, handedness) def fake_viz( @@ -131,9 +131,9 @@ def test_default_pipeline_keeps_full_body_source(): @pytest.mark.parametrize("explicit", [False, True]) def test_soma_pipeline_replaces_only_body_source(monkeypatch, tmp_path, explicit): monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) - tracker_factory = MagicMock(wraps=SomaBodyJointRotationsV0Tracker) + tracker_factory = MagicMock(wraps=SomaBodyJointRotationsTracker) monkeypatch.setattr( - soma_body_source, "SomaBodyJointRotationsV0Tracker", tracker_factory + soma_body_source, "SomaBodyJointRotationsTracker", tracker_factory ) body = create_body_view_pipeline( tmp_path, @@ -144,7 +144,7 @@ def test_soma_pipeline_replaces_only_body_source(monkeypatch, tmp_path, explicit sources = {source.name: source for source in pipeline.get_leaf_nodes()} assert set(sources) == {"hands", "head", "controllers", "body"} assert type(sources["body"]) is SomaBodySource - assert isinstance(sources["body"].get_tracker(), SomaBodyJointRotationsV0Tracker) + assert isinstance(sources["body"].get_tracker(), SomaBodyJointRotationsTracker) tracker_factory.assert_called_once_with("vendor_body") assert len(body.layout.joint_names) == 77 assert len(body.layout.bones) == 76 @@ -152,10 +152,8 @@ def test_soma_pipeline_replaces_only_body_source(monkeypatch, tmp_path, explicit def test_soma_joint_pose_pipeline_selects_direct_tracker(monkeypatch, tmp_path): monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) - tracker_factory = MagicMock(wraps=SomaBodyJointPosesV0Tracker) - monkeypatch.setattr( - soma_body_source, "SomaBodyJointPosesV0Tracker", tracker_factory - ) + tracker_factory = MagicMock(wraps=SomaBodyJointPosesTracker) + monkeypatch.setattr(soma_body_source, "SomaBodyJointPosesTracker", tracker_factory) body = create_body_view_pipeline( tmp_path, body_schema=BodySchema.SOMA, @@ -164,7 +162,7 @@ def test_soma_joint_pose_pipeline_selects_direct_tracker(monkeypatch, tmp_path): ) pipeline = build_all_human_pipeline(body=body) source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") - assert isinstance(source.get_tracker(), SomaBodyJointPosesV0Tracker) + assert isinstance(source.get_tracker(), SomaBodyJointPosesTracker) tracker_factory.assert_called_once_with("vendor_body") @@ -187,7 +185,7 @@ def test_soma_joint_pose_hands_replace_only_openxr_hands(): } assert isinstance(sources["hand_left"], SomaHandSource) assert isinstance(sources["hand_right"], SomaHandSource) - assert isinstance(sources["hand_left"].get_tracker(), SomaHandJointPosesV0Tracker) + assert isinstance(sources["hand_left"].get_tracker(), SomaHandJointPosesTracker) assert hands.layout is SOMA_HAND_LAYOUT @@ -203,7 +201,7 @@ def test_soma_hand_pose_reaches_native_25_joint_renderer(): positions = np.arange(75, dtype=np.float32).reshape(25, 3) / 100 orientations = np.tile([0, 0, 0, 1], (25, 1)).astype(np.float32) sources["hand_left"]._tracker.get_data.return_value = soma_hand_joint_poses( - positions, orientations, SomaHandednessV0.LEFT + positions, orientations, SomaHandedness.LEFT ) sources["hand_right"]._tracker.get_data.return_value = None @@ -449,11 +447,11 @@ def test_native_soma_hand_rotations_reach_renderer(soma_assets): identity = np.tile([0, 0, 0, 1], (25, 1)).astype(np.float32) for name, handedness in ( - ("hand_left", SomaHandednessV0.LEFT), - ("hand_right", SomaHandednessV0.RIGHT), + ("hand_left", SomaHandedness.LEFT), + ("hand_right", SomaHandedness.RIGHT), ): source = sources[name] - assert isinstance(source.get_tracker(), SomaHandJointRotationsV0Tracker) + assert isinstance(source.get_tracker(), SomaHandJointRotationsTracker) source._tracker = MagicMock() source._tracker.get_data.return_value = soma_hand_joint_rotations( identity, [0, 0, 0], handedness diff --git a/tests/python/examples/soma_body_publisher/test_publisher.py b/tests/python/examples/soma_body_publisher/test_publisher.py index 15b59aa3b5..5a8f26dc74 100644 --- a/tests/python/examples/soma_body_publisher/test_publisher.py +++ b/tests/python/examples/soma_body_publisher/test_publisher.py @@ -13,8 +13,8 @@ ) from isaaccapture.schema import ( DeviceDataTimestamp, - SomaBodyJointPosesV0Record, - SomaBodyJointRotationsV0Record, + SomaBodyJointPosesRecord, + SomaBodyJointRotationsRecord, ) from isaaccapture_examples.soma_body_publisher import publisher from isaaccapture_examples.soma_body_publisher.publisher import ( @@ -29,7 +29,7 @@ def test_nested_soma_serialization_uses_payload_root(): q = np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)) pose = soma_joint_rotations(q, [1, 2, 3]) - record = SomaBodyJointRotationsV0Record(pose, DeviceDataTimestamp(10, 20, 30)) + record = SomaBodyJointRotationsRecord(pose, DeviceDataTimestamp(10, 20, 30)) assert record.data.to_bytes() == pose.to_bytes() assert record.to_bytes() != pose.to_bytes() @@ -37,7 +37,7 @@ def test_nested_soma_serialization_uses_payload_root(): np.zeros((77, 3), dtype=np.float32), np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)), ) - evaluated_record = SomaBodyJointPosesV0Record( + evaluated_record = SomaBodyJointPosesRecord( evaluated, DeviceDataTimestamp(10, 20, 30) ) assert evaluated_record.data.to_bytes() == evaluated.to_bytes() @@ -131,7 +131,7 @@ def test_demo_to_fbs_to_native_soma_matches_upstream(soma_assets, request): apply_correctives=False, ) payload = soma_joint_rotations(q[0], t[0]) - record = SomaBodyJointRotationsV0Record(payload, DeviceDataTimestamp(10, 20, 30)) + record = SomaBodyJointRotationsRecord(payload, DeviceDataTimestamp(10, 20, 30)) positions, orientations, valid = evaluator.evaluate(record.data) np.testing.assert_allclose(positions, expected["joints"][0].numpy(), atol=1e-4) expected_orientations = matrix_to_quaternion_xyzw( diff --git a/tests/python/examples/soma_hand_publisher/test_publisher.py b/tests/python/examples/soma_hand_publisher/test_publisher.py index 777f827d6a..b806bdb37a 100644 --- a/tests/python/examples/soma_hand_publisher/test_publisher.py +++ b/tests/python/examples/soma_hand_publisher/test_publisher.py @@ -9,7 +9,7 @@ from isaaccapture.retargeting_engine.utilities.soma_hand_evaluator import ( _SomaHandEvaluator, ) -from isaaccapture.schema import SomaHandednessV0 +from isaaccapture.schema import SomaHandedness from isaaccapture_examples.soma_hand_publisher.publisher import ( create_layers, demo_hand_frames, @@ -20,13 +20,13 @@ def test_hand_payload_constructors_preserve_side_and_arrays(): rotations = np.tile([0.0, 0.0, 0.0, 1.0], (25, 1)) - rotation_payload = soma_joint_rotations(rotations, [1, 2, 3], SomaHandednessV0.LEFT) - assert rotation_payload.handedness == SomaHandednessV0.LEFT + rotation_payload = soma_joint_rotations(rotations, [1, 2, 3], SomaHandedness.LEFT) + assert rotation_payload.handedness == SomaHandedness.LEFT np.testing.assert_array_equal(rotation_payload.joint_rotations.rotations, rotations) positions = np.arange(75, dtype=np.float32).reshape(25, 3) - pose_payload = soma_joint_poses(positions, rotations, SomaHandednessV0.RIGHT) - assert pose_payload.handedness == SomaHandednessV0.RIGHT + pose_payload = soma_joint_poses(positions, rotations, SomaHandedness.RIGHT) + assert pose_payload.handedness == SomaHandedness.RIGHT np.testing.assert_array_equal(pose_payload.joint_poses.positions, positions) @@ -36,11 +36,9 @@ def test_pusher_rejects_collection_handedness_mismatch(request): return rotations = np.tile([0.0, 0.0, 0.0, 1.0], (25, 1)) wrong_left = soma_joint_rotations( - rotations, [0, 0, 0], SomaHandednessV0.RIGHT - ).to_bytes() - right = soma_joint_rotations( - rotations, [0, 0, 0], SomaHandednessV0.RIGHT + rotations, [0, 0, 0], SomaHandedness.RIGHT ).to_bytes() + right = soma_joint_rotations(rotations, [0, 0, 0], SomaHandedness.RIGHT).to_bytes() packet = struct.pack(" Date: Tue, 6 Oct 2026 14:18:34 -0400 Subject: [PATCH 04/12] docs(soma): add pose transport usage guide Document SOMA body and hand data flows, representation choices, demo commands, schema locations, collection IDs, and current limitations. Rename the body demo collection and viewer option for symmetry with the standalone hand streams, and distinguish collection routing from tensor schema identifiers. Signed-off-by: Michael Haselton --- docs/source/device/soma.rst | 237 ++++++++++++++++++ docs/source/index.rst | 1 + examples/deviceio_live_view/README.md | 4 +- .../deviceio_live_view/body_pipeline.py | 4 +- .../deviceio_live_view/deviceio_pipeline.py | 4 +- .../deviceio_live_view/live_deviceio.py | 8 +- examples/soma_body_publisher/README.md | 6 +- .../cpp/soma_body_pusher/soma_body_pusher.cpp | 2 +- .../deviceio_live_view/test_soma_viewer.py | 16 +- 9 files changed, 260 insertions(+), 22 deletions(-) create mode 100644 docs/source/device/soma.rst diff --git a/docs/source/device/soma.rst b/docs/source/device/soma.rst new file mode 100644 index 0000000000..fdfc3fd204 --- /dev/null +++ b/docs/source/device/soma.rst @@ -0,0 +1,237 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +SOMA Pose Transport +=================== + +Isaac Teleop can transport SOMA body and hand data through FlatBuffers and +``SchemaPusher``. Producers publish either joint rotations or evaluated joint +poses. Consumer-side sources normalize both representations to global joint +poses for retargeting or visualization. + +The schemas and examples are defined and verified against the SOMA-X v0.3.1 +pose interface. See the `SOMA-X repository `_ +for the upstream model and assets. + +.. contents:: On this page + :local: + :depth: 2 + +Data flow +--------- + +Body and hand data use separate schemas and collections. Applications can +select their body and hand sources independently. + +.. code-block:: text + + vendor body SDK -> SOMA body FlatBuffer -> SchemaPusher -> SOMA body tracker + vendor hand SDK -> SOMA hand FlatBuffer -> SchemaPusher -> SOMA hand tracker + +The body schema contains SOMA's complete body control set, including its hand +controls. The standalone hand schemas provide independent left and right hand +streams. Isaac Teleop does not automatically fuse standalone hands with the +hand controls in a body payload. + +Choose a representation +----------------------- + +Use the representation closest to the producer's native data. + +.. list-table:: + :header-rows: 1 + :widths: 20 30 50 + + * - Representation + - FlatBuffer types + - Behavior + * - ``joint-rotations`` + - ``SomaBodyJointRotations`` and ``SomaHandJointRotations`` + - Publishes SOMA joint rotations and the global Hips or Wrist translation. + The consumer evaluates forward kinematics against a prepared SOMA + identity. + * - ``joint-poses`` + - ``SomaBodyJointPoses`` and ``SomaHandJointPoses`` + - Publishes global position, orientation, and validity for every joint. + The consumer maps these poses directly and does not run per-frame + forward kinematics. + +``joint-rotations`` most directly matches the SOMA-X pose-control interface. +``joint-poses`` is an Isaac Teleop transport profile for producers that already +have evaluated global joint transforms. + +Both representations use positions in meters and unit XYZW quaternions. They +use a right-handed reference frame with positive Y up and positive Z forward. +The publisher must convert its native skeleton and reference frame before +serialization. See :ref:`tracker-reference` for the complete field and +validity contracts. + +Build the examples +------------------ + +Start with a configured Isaac Teleop checkout. See +:doc:`../getting_started/build_from_source/index` for the complete build +prerequisites. Install the live viewer and both deterministic publishers, then +build their native pushers: + +.. code-block:: bash + + uv pip install -e ./examples/deviceio_live_view + uv pip install -e ./examples/soma_body_publisher + uv pip install -e ./examples/soma_hand_publisher + + .venv/bin/cmake -S . -B build + .venv/bin/cmake --build build \ + --target isaacteleop_python soma_body_pusher soma_hand_pusher \ + -j4 + +The example packages install ``py-soma-x==0.3.1``. Obtain the matching SOMA-X +assets and use their directory as ``/path/to/SOMA-X/assets`` in the commands +below. The body example requires ``example_animation.npy`` and the neutral +identity assets. The hand example also requires ``SOMAHand.npz``. + +Run the body example +-------------------- + +Start the viewer first. It launches the CloudXR runtime and prints the viewer +URL. Open that URL before starting the publisher. On a remote host, replace +``localhost`` in the URL with the host name or IP address. + +.. code-block:: bash + + uv run --no-sync python -m isaaccapture_examples.deviceio_live_view \ + --accept-eula \ + --body-schema soma \ + --soma-data-root /path/to/SOMA-X/assets + +In another terminal, load the runtime environment and start the body +publisher: + +.. code-block:: bash + + source ~/.cloudxr/run/cloudxr.env + uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ + --data-root /path/to/SOMA-X/assets \ + --pusher ./build/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher \ + --loop + +These commands use ``joint-rotations``. To publish evaluated joint poses, add +``--body-representation joint-poses`` to the publisher and +``--soma-body-representation joint-poses`` to the viewer. + +Run the hand example +-------------------- + +Start the viewer with SOMA hands: + +.. code-block:: bash + + uv run --no-sync python -m isaaccapture_examples.deviceio_live_view \ + --accept-eula \ + --hand-schema soma \ + --soma-data-root /path/to/SOMA-X/assets + +In another terminal, load the runtime environment and start the hand +publisher: + +.. code-block:: bash + + source ~/.cloudxr/run/cloudxr.env + uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ + --data-root /path/to/SOMA-X/assets \ + --pusher ./build/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher \ + --loop + +These commands use ``joint-rotations``. To publish evaluated joint poses, add +``--hand-representation joint-poses`` to the publisher and +``--soma-hand-representation joint-poses`` to the viewer. + +Collection IDs and schema identifiers +------------------------------------- + +A collection ID pairs a producer with a consumer and is not encoded in the +FlatBuffer payload. The tensor identifier selects the representation carried by +that collection. The demo publishers use these fixed collection IDs; another +producer can choose different IDs as long as its consumers use the same values. + +.. list-table:: + :header-rows: 1 + :widths: 30 30 40 + + * - Data + - Collection ID + - Viewer override + * - Body + - ``soma_body_demo`` + - ``--soma-body-collection-id`` + * - Left hand + - ``soma_hand_left_demo`` + - ``--soma-left-hand-collection-id`` + * - Right hand + - ``soma_hand_right_demo`` + - ``--soma-right-hand-collection-id`` + +The representation selected by the publisher and consumer must match. The +transport identifiers are ``soma_body_joint_rotations``, +``soma_body_joint_poses``, ``soma_hand_joint_rotations``, and +``soma_hand_joint_poses``. + +Vendor integration +------------------ + +A vendor integration converts its native skeleton to one of the checked-in +SOMA contracts and publishes it through ``SchemaPusher``. Use these schemas as +the serialization source: + +- :code-file:`src/core/schema/fbs/soma_common.fbs` +- :code-file:`src/core/schema/fbs/soma_body_common.fbs` +- :code-file:`src/core/schema/fbs/soma_body_joint_rotations.fbs` +- :code-file:`src/core/schema/fbs/soma_body_joint_poses.fbs` +- :code-file:`src/core/schema/fbs/soma_hand_joint_rotations.fbs` +- :code-file:`src/core/schema/fbs/soma_hand_joint_poses.fbs` + +The FlatBuffer transport and generated trackers do not depend on SOMA-X or +PyTorch. A rotation consumer needs SOMA-X and prepared identity assets to +evaluate forward kinematics. A joint-pose consumer does not need SOMA-X for +per-frame evaluation. + +Current limitations +------------------- + +- The examples evaluate rotations against a neutral identity. They do not + perform subject calibration. +- Body and standalone hand streams are not fused automatically. +- A producer and consumer must select the same representation. +- The live viewer selects sources at launch. It does not switch schemas while + running. + +Troubleshooting +--------------- + +No module named ``isaaccapture_examples`` +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Install the corresponding example with ``uv pip install -e`` and launch it +with ``uv run --no-sync`` from the repository root. + +The publisher cannot connect +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Start the viewer, open its client, and then source +``~/.cloudxr/run/cloudxr.env`` in the publisher terminal. The environment file +identifies the active CloudXR OpenXR runtime. + +The viewer receives no skeleton +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Confirm that the publisher and viewer use the same representation and +collection names. Also confirm that the asset directory matches SOMA-X v0.3.1 +and contains the files required by the selected example. + +See also +-------- + +- :code-file:`examples/soma_body_publisher/README.md` +- :code-file:`examples/soma_hand_publisher/README.md` +- :code-file:`examples/deviceio_live_view/README.md` +- :doc:`trackers` diff --git a/docs/source/index.rst b/docs/source/index.rst index 31cda57dd8..bbf1aa717f 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -61,6 +61,7 @@ Table of Contents :caption: Devices device/trackers + device/soma device/add_device device/joint_space device/body_tracking diff --git a/examples/deviceio_live_view/README.md b/examples/deviceio_live_view/README.md index 3d9f737063..c7ecf9c1c3 100644 --- a/examples/deviceio_live_view/README.md +++ b/examples/deviceio_live_view/README.md @@ -55,8 +55,8 @@ also selects SOMA; an explicit schema always takes precedence. Switch sources by restarting the viewer with the other option. Hands, head, and controllers remain unchanged. -The SOMA viewer reads the `soma_demo` collection by default. Use -`--soma-collection-id ` to match another publisher's collection. +The SOMA viewer reads the `soma_body_demo` collection by default. Use +`--soma-body-collection-id ` to match another publisher's collection. Joint rotations are the default SOMA profile. Pass `--soma-body-representation joint-poses` when the publisher sends evaluated joint poses instead. The publisher and viewer options must match. diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py index f317c8dda2..57390a2f15 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py @@ -51,7 +51,7 @@ def create_body_view_pipeline( soma_data_root: Path | None = None, *, body_schema: BodySchema | str | None = None, - soma_collection_id: str = "soma_demo", + soma_body_collection_id: str = "soma_body_demo", soma_body_representation: SomaBodyRepresentation | str = SomaBodyRepresentation.JOINT_ROTATIONS, ) -> BodyViewPipeline: @@ -65,5 +65,5 @@ def create_body_view_pipeline( assert soma_data_root is not None return create_soma_body_pipeline( - soma_data_root, soma_collection_id, soma_body_representation + soma_data_root, soma_body_collection_id, soma_body_representation ) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py index 0d18346dad..9a3cf3ce4e 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py @@ -17,7 +17,7 @@ def build_all_human_pipeline( soma_data_root=None, *, body_schema=None, - soma_collection_id="soma_demo", + soma_body_collection_id="soma_body_demo", soma_body_representation="joint-rotations", body: BodyViewPipeline | None = None, hands: HandViewPipeline | None = None, @@ -28,7 +28,7 @@ def build_all_human_pipeline( selected_body = body or create_body_view_pipeline( soma_data_root, body_schema=body_schema, - soma_collection_id=soma_collection_id, + soma_body_collection_id=soma_body_collection_id, soma_body_representation=soma_body_representation, ) selected_hands = hands or create_hand_view_pipeline() diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py index c4b211e9aa..040e08e43f 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py @@ -55,9 +55,9 @@ def main(argv: list[str]) -> int: ) parser.add_argument("--soma-data-root", type=Path, help="SOMA POC assets directory") parser.add_argument( - "--soma-collection-id", - default="soma_demo", - help="SOMA publisher's tensor collection ID", + "--soma-body-collection-id", + default="soma_body_demo", + help="SOMA body publisher's tensor collection ID", ) parser.add_argument( "--soma-body-representation", @@ -95,7 +95,7 @@ def main(argv: list[str]) -> int: body = create_body_view_pipeline( args.soma_data_root, body_schema=body_schema, - soma_collection_id=args.soma_collection_id, + soma_body_collection_id=args.soma_body_collection_id, soma_body_representation=args.soma_body_representation, ) hands = create_hand_view_pipeline( diff --git a/examples/soma_body_publisher/README.md b/examples/soma_body_publisher/README.md index 60d6fb7383..3302eab610 100644 --- a/examples/soma_body_publisher/README.md +++ b/examples/soma_body_publisher/README.md @@ -12,14 +12,14 @@ testing SOMA consumers. It is not a vendor plugin or vendor network protocol. The Python process converts the bundled motion into the pinned SOMA-X v0.3.1 pose contract and writes framed FlatBuffers to the native `soma_body_pusher`. It can publish joint rotations for downstream FK or pre-evaluated joint poses. -The native process validates the selected payload and publishes the `soma_demo` -tensor collection. +The native process validates the selected payload and publishes the +`soma_body_demo` tensor collection. ```text SOMA-X example_animation.npy -> Python SOMA body encoder -> soma_body_pusher - -> SchemaPusher collection soma_demo + -> SchemaPusher collection soma_body_demo ``` Install the example and build the native pusher: diff --git a/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp b/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp index 835af293a9..ffa4e73fbe 100644 --- a/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp +++ b/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp @@ -125,7 +125,7 @@ int main(int argc, char** argv) session = std::make_unique( "SomaBodyDemoPublisher", core::SchemaPusher::get_required_extensions(), false); pusher = std::make_unique( - session->get_handles(), core::SchemaPusherConfig{ .collection_id = "soma_demo", + session->get_handles(), core::SchemaPusherConfig{ .collection_id = "soma_body_demo", .max_flatbuffer_size = max_flatbuffer_size, .tensor_identifier = tensor_identifier, .localized_name = "SOMA Body Demo" }); diff --git a/tests/python/examples/deviceio_live_view/test_soma_viewer.py b/tests/python/examples/deviceio_live_view/test_soma_viewer.py index 763a846a1b..8e349c9e92 100644 --- a/tests/python/examples/deviceio_live_view/test_soma_viewer.py +++ b/tests/python/examples/deviceio_live_view/test_soma_viewer.py @@ -138,7 +138,7 @@ def test_soma_pipeline_replaces_only_body_source(monkeypatch, tmp_path, explicit body = create_body_view_pipeline( tmp_path, body_schema=BodySchema.SOMA if explicit else None, - soma_collection_id="vendor_body", + soma_body_collection_id="vendor_body", ) pipeline = build_all_human_pipeline(body=body) sources = {source.name: source for source in pipeline.get_leaf_nodes()} @@ -157,7 +157,7 @@ def test_soma_joint_pose_pipeline_selects_direct_tracker(monkeypatch, tmp_path): body = create_body_view_pipeline( tmp_path, body_schema=BodySchema.SOMA, - soma_collection_id="vendor_body", + soma_body_collection_id="vendor_body", soma_body_representation=SomaBodyRepresentation.JOINT_POSES, ) pipeline = build_all_human_pipeline(body=body) @@ -314,10 +314,10 @@ def test_viewer_uses_selected_native_body_layout_and_hides_missing_devices(): @pytest.mark.parametrize( "schema,use_assets,expected,collection", [ - (None, False, BodySchema.FULL_BODY_POSE, "soma_demo"), - (None, True, BodySchema.SOMA, "soma_demo"), + (None, False, BodySchema.FULL_BODY_POSE, "soma_body_demo"), + (None, True, BodySchema.SOMA, "soma_body_demo"), ("soma", True, BodySchema.SOMA, "vendor_body"), - ("full-body-pose", True, BodySchema.FULL_BODY_POSE, "soma_demo"), + ("full-body-pose", True, BodySchema.FULL_BODY_POSE, "soma_body_demo"), ], ) def test_live_cli_selects_source_and_launches_runtime( @@ -347,12 +347,12 @@ def test_live_cli_selects_source_and_launches_runtime( args += ["--body-schema", schema] if use_assets: args += ["--soma-data-root", str(tmp_path)] - args += ["--soma-collection-id", collection] + args += ["--soma-body-collection-id", collection] assert live_deviceio.main(args) == 0 select.assert_called_once_with( tmp_path if use_assets else None, body_schema=expected, - soma_collection_id=collection, + soma_body_collection_id=collection, soma_body_representation="joint-rotations", ) select_hands.assert_called_once_with( @@ -378,7 +378,7 @@ def test_live_cli_selects_source_and_launches_runtime( "soma", "--soma-data-root", "/unused", - "--soma-collection-id", + "--soma-body-collection-id", "", ], "collection_id must not be empty", From 3efd6ba9a5e3b92a242c40f11b09f9a53fa202e3 Mon Sep 17 00:00:00 2001 From: Michael Haselton Date: Tue, 6 Oct 2026 14:22:44 -0400 Subject: [PATCH 05/12] style(soma): format schema bindings Signed-off-by: Michael Haselton --- src/core/schema/python/soma_body_joint_poses_bindings.h | 9 +++------ .../schema/python/soma_body_joint_rotations_bindings.h | 3 ++- src/core/schema/python/soma_hand_joint_poses_bindings.h | 9 +++------ .../schema/python/soma_hand_joint_rotations_bindings.h | 3 +-- 4 files changed, 9 insertions(+), 15 deletions(-) diff --git a/src/core/schema/python/soma_body_joint_poses_bindings.h b/src/core/schema/python/soma_body_joint_poses_bindings.h index 13097ab171..63fc7386bf 100644 --- a/src/core/schema/python/soma_body_joint_poses_bindings.h +++ b/src/core/schema/python/soma_body_joint_poses_bindings.h @@ -26,8 +26,7 @@ inline const SomaBodyJointPose& first_soma_body_joint_pose(const py::object& sel constexpr py::ssize_t SOMA_BODY_JOINT_POSE_STRIDE = static_cast(sizeof(SomaBodyJointPose)); constexpr py::ssize_t SOMA_BODY_JOINT_POSE_COUNT = static_cast(SomaBodyJoint_NUM_JOINTS); -static_assert(sizeof(SomaBodyJointPoseArray) == - sizeof(SomaBodyJointPose) * static_cast(SomaBodyJoint_NUM_JOINTS), +static_assert(sizeof(SomaBodyJointPoseArray) == sizeof(SomaBodyJointPose) * static_cast(SomaBodyJoint_NUM_JOINTS), "SomaBodyJointPoseArray.values length must equal SomaBodyJoint::NUM_JOINTS"); inline void bind_soma_body_joint_poses(py::module& m) @@ -55,8 +54,7 @@ inline void bind_soma_body_joint_poses(py::module& m) "positions", [](py::object self) { - const auto* first = - reinterpret_cast(&first_soma_body_joint_pose(self).pose().position()); + const auto* first = reinterpret_cast(&first_soma_body_joint_pose(self).pose().position()); return strided_field_view(self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 3); }, "Joint positions as a writable (77, 3) float32 view in SomaBodyJoint order.") @@ -91,8 +89,7 @@ inline void bind_soma_body_joint_poses(py::module& m) .def_property_readonly( "joint_poses", field(&SomaBodyJointPoses::joint_poses), py::return_value_policy::reference_internal); - bind_record( - m, "SomaBodyJointPosesRecord", "SomaBodyJointPoses"); + bind_record(m, "SomaBodyJointPosesRecord", "SomaBodyJointPoses"); } } // namespace core diff --git a/src/core/schema/python/soma_body_joint_rotations_bindings.h b/src/core/schema/python/soma_body_joint_rotations_bindings.h index 81cc87242f..7598734be9 100644 --- a/src/core/schema/python/soma_body_joint_rotations_bindings.h +++ b/src/core/schema/python/soma_body_joint_rotations_bindings.h @@ -129,7 +129,8 @@ inline void bind_soma_body_joint_rotations(py::module& m) "rotations", [](py::object self) { - const auto* first = reinterpret_cast(&first_soma_body_joint_rotations_rotation(self).rotation()); + const auto* first = + reinterpret_cast(&first_soma_body_joint_rotations_rotation(self).rotation()); return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 4); }, "Unit XYZW quaternions as a writable (77, 4) float32 view.") diff --git a/src/core/schema/python/soma_hand_joint_poses_bindings.h b/src/core/schema/python/soma_hand_joint_poses_bindings.h index 51650a159e..e4881142d8 100644 --- a/src/core/schema/python/soma_hand_joint_poses_bindings.h +++ b/src/core/schema/python/soma_hand_joint_poses_bindings.h @@ -26,8 +26,7 @@ inline const SomaHandJointPose& first_soma_hand_joint_pose(const py::object& sel constexpr py::ssize_t SOMA_HAND_JOINT_POSE_STRIDE = static_cast(sizeof(SomaHandJointPose)); constexpr py::ssize_t SOMA_HAND_JOINT_POSE_COUNT = static_cast(SomaHandJoint_NUM_JOINTS); -static_assert(sizeof(SomaHandJointPoseArray) == - sizeof(SomaHandJointPose) * static_cast(SomaHandJoint_NUM_JOINTS), +static_assert(sizeof(SomaHandJointPoseArray) == sizeof(SomaHandJointPose) * static_cast(SomaHandJoint_NUM_JOINTS), "SomaHandJointPoseArray.values length must equal SomaHandJoint::NUM_JOINTS"); inline void bind_soma_hand_joint_poses(py::module& m) @@ -55,8 +54,7 @@ inline void bind_soma_hand_joint_poses(py::module& m) "positions", [](py::object self) { - const auto* first = - reinterpret_cast(&first_soma_hand_joint_pose(self).pose().position()); + const auto* first = reinterpret_cast(&first_soma_hand_joint_pose(self).pose().position()); return strided_field_view(self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 3); }, "Joint positions as a writable (25, 3) float32 view in SomaHandJoint order.") @@ -93,8 +91,7 @@ inline void bind_soma_hand_joint_poses(py::module& m) "joint_poses", field(&SomaHandJointPoses::joint_poses), py::return_value_policy::reference_internal) .def_property_readonly("handedness", field(&SomaHandJointPoses::handedness)); - bind_record( - m, "SomaHandJointPosesRecord", "SomaHandJointPoses"); + bind_record(m, "SomaHandJointPosesRecord", "SomaHandJointPoses"); } } // namespace core diff --git a/src/core/schema/python/soma_hand_joint_rotations_bindings.h b/src/core/schema/python/soma_hand_joint_rotations_bindings.h index 6243696d2e..392755fb46 100644 --- a/src/core/schema/python/soma_hand_joint_rotations_bindings.h +++ b/src/core/schema/python/soma_hand_joint_rotations_bindings.h @@ -108,8 +108,7 @@ inline void bind_soma_hand_joint_rotations(py::module& m) py::return_value_policy::reference_internal) .def_property_readonly("global_translation", field(&SomaHandJointRotations::global_translation), py::return_value_policy::reference_internal) - .def_property_readonly( - "global_translation_is_valid", field(&SomaHandJointRotations::global_translation_is_valid)) + .def_property_readonly("global_translation_is_valid", field(&SomaHandJointRotations::global_translation_is_valid)) .def_property_readonly("handedness", field(&SomaHandJointRotations::handedness)); bind_record( From a3444c0c6bac8ce93679f1c47307b3c061b04b64 Mon Sep 17 00:00:00 2001 From: Michael Haselton Date: Tue, 6 Oct 2026 15:30:19 -0400 Subject: [PATCH 06/12] fix(soma): decouple viewer from asset paths Select body and hand schemas explicitly, resolve rotation-FK assets through SOMA-X, and use static SOMA topology for evaluated joint poses. Signed-off-by: Michael Haselton --- docs/source/device/soma.rst | 20 ++- examples/deviceio_live_view/README.md | 32 ++--- .../deviceio_live_view/body_pipeline.py | 22 +-- .../deviceio_live_view/deviceio_pipeline.py | 2 - .../deviceio_live_view/hand_pipeline.py | 10 -- .../deviceio_live_view/live_deviceio.py | 8 +- .../deviceio_live_view/soma_body.py | 127 ++++++++++++++++-- .../deviceio_live_view/soma_hand.py | 8 +- examples/soma_hand_publisher/README.md | 3 +- .../deviceio_live_view/test_soma_viewer.py | 75 ++++++----- 10 files changed, 190 insertions(+), 117 deletions(-) diff --git a/docs/source/device/soma.rst b/docs/source/device/soma.rst index fdfc3fd204..290567373f 100644 --- a/docs/source/device/soma.rst +++ b/docs/source/device/soma.rst @@ -85,10 +85,10 @@ build their native pushers: --target isaacteleop_python soma_body_pusher soma_hand_pusher \ -j4 -The example packages install ``py-soma-x==0.3.1``. Obtain the matching SOMA-X -assets and use their directory as ``/path/to/SOMA-X/assets`` in the commands -below. The body example requires ``example_animation.npy`` and the neutral -identity assets. The hand example also requires ``SOMAHand.npz``. +The example packages install ``py-soma-x==0.3.1``. SOMA-X resolves the model +assets needed by rotation consumers. The demo publishers additionally require a +matching SOMA-X asset directory containing ``example_animation.npy``. The hand +publisher also requires ``SOMAHand.npz``. Run the body example -------------------- @@ -101,8 +101,7 @@ URL. Open that URL before starting the publisher. On a remote host, replace uv run --no-sync python -m isaaccapture_examples.deviceio_live_view \ --accept-eula \ - --body-schema soma \ - --soma-data-root /path/to/SOMA-X/assets + --body-schema soma In another terminal, load the runtime environment and start the body publisher: @@ -128,8 +127,7 @@ Start the viewer with SOMA hands: uv run --no-sync python -m isaaccapture_examples.deviceio_live_view \ --accept-eula \ - --hand-schema soma \ - --soma-data-root /path/to/SOMA-X/assets + --hand-schema soma In another terminal, load the runtime environment and start the hand publisher: @@ -191,7 +189,7 @@ the serialization source: - :code-file:`src/core/schema/fbs/soma_hand_joint_poses.fbs` The FlatBuffer transport and generated trackers do not depend on SOMA-X or -PyTorch. A rotation consumer needs SOMA-X and prepared identity assets to +PyTorch. A rotation consumer uses SOMA-X and its resolved neutral identity to evaluate forward kinematics. A joint-pose consumer does not need SOMA-X for per-frame evaluation. @@ -225,8 +223,8 @@ The viewer receives no skeleton ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Confirm that the publisher and viewer use the same representation and -collection names. Also confirm that the asset directory matches SOMA-X v0.3.1 -and contains the files required by the selected example. +collection names. For a demo publisher, also confirm that its asset directory +matches SOMA-X v0.3.1 and contains the required animation and model files. See also -------- diff --git a/examples/deviceio_live_view/README.md b/examples/deviceio_live_view/README.md index c7ecf9c1c3..949f86aaa0 100644 --- a/examples/deviceio_live_view/README.md +++ b/examples/deviceio_live_view/README.md @@ -32,16 +32,14 @@ SomaBodyJointRotations -> tracker -> upstream FK --+ SomaBodyJointPoses ----> tracker -> direct map ----+ ``` -Use SOMA-X 0.3.1 and its neutral identity assets. For an uninstalled source -build, set `PYTHONPATH` to the built IsaacCapture package and this example's -`python/` directory. +Use SOMA-X 0.3.1. For an uninstalled source build, set `PYTHONPATH` to the built +IsaacCapture package and this example's `python/` directory. ```bash uv pip install -e ./examples/deviceio_live_view uv run --no-sync python -m isaaccapture_examples.deviceio_live_view --accept-eula \ - --body-schema soma \ - --soma-data-root /path/to/SOMA-X/assets + --body-schema soma ``` Use the independent [SOMA body publisher](../soma_body_publisher/README.md) to @@ -49,11 +47,8 @@ send the bundled motion, or connect another publisher using the checked-in SOMA body schema. Select the body input at launch with `--body-schema full-body-pose` (the default) -or `--body-schema soma`. SOMA requires `--soma-data-root` for downstream FK. -For compatibility with earlier commands, supplying assets without `--body-schema` -also selects SOMA; an explicit schema always takes precedence. Switch sources by -restarting the viewer with the other option. Hands, head, and controllers remain -unchanged. +or `--body-schema soma`. Switch sources by restarting the viewer with the other +option. Hands, head, and controllers remain unchanged. The SOMA viewer reads the `soma_body_demo` collection by default. Use `--soma-body-collection-id ` to match another publisher's collection. @@ -64,11 +59,11 @@ joint poses instead. The publisher and viewer options must match. `SomaBodySource` follows the normal DeviceIO source contract: the session discovers the generated tracker selected for `SomaBodyJointRotations` or `SomaBodyJointPoses`. The raw tracker remains the transport and recording -boundary. The source evaluates rotations once per graph step against its -caller-supplied SOMA identity; evaluated poses map directly. Both modes emit +boundary. The source evaluates rotations once per graph step against the neutral +SOMA identity resolved by SOMA-X; evaluated poses map directly. Both modes emit the same global positions, orientations, and validity as `SomaBodyInput`. The -viewer uses the joint names and hierarchy reported by the pinned SOMA layer. It -does not convert SOMA into `FullBodyPose`. +viewer uses the pinned SOMA joint names and hierarchy. It does not convert SOMA +into `FullBodyPose`. SOMA-X and PyTorch are source-evaluation dependencies, not tracker or transport dependencies. @@ -94,13 +89,12 @@ SomaHandJointPoses ----> tracker -> direct map -------+ ```bash uv run --no-sync python -m isaaccapture_examples.deviceio_live_view --accept-eula \ - --hand-schema soma \ - --soma-data-root /path/to/SOMA-X-v0.3.1-assets + --hand-schema soma ``` -The rotation profile requires the v0.3.1 `SOMAHand.npz` asset and uses the -neutral hand identity for FK. Use `--soma-hand-representation joint-poses` for -evaluated poses, which do not require SOMA-X during per-frame source evaluation. +The rotation profile uses the neutral hand identity that SOMA-X resolves for FK. +Use `--soma-hand-representation joint-poses` for evaluated poses, which do not +require SOMA-X during per-frame source evaluation. The default collections are `soma_hand_left_demo` and `soma_hand_right_demo`; override them with `--soma-left-hand-collection-id` and `--soma-right-hand-collection-id`. diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py index 57390a2f15..48ba0083f2 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py @@ -5,7 +5,6 @@ from dataclasses import dataclass from enum import Enum -from pathlib import Path from isaaccapture.retargeting_engine.interface.retargeter_core_types import ( OutputSelector, @@ -32,30 +31,22 @@ class BodyViewPipeline: layout: BodyViewLayout -def resolve_body_schema( - body_schema: BodySchema | str | None, soma_data_root: Path | None -) -> BodySchema: - selected = ( +def resolve_body_schema(body_schema: BodySchema | str | None) -> BodySchema: + return ( BodySchema(body_schema) if body_schema is not None - else ( - BodySchema.SOMA if soma_data_root is not None else BodySchema.FULL_BODY_POSE - ) + else BodySchema.FULL_BODY_POSE ) - if selected is BodySchema.SOMA and soma_data_root is None: - raise ValueError("--body-schema soma requires --soma-data-root") - return selected def create_body_view_pipeline( - soma_data_root: Path | None = None, *, body_schema: BodySchema | str | None = None, soma_body_collection_id: str = "soma_body_demo", soma_body_representation: SomaBodyRepresentation | str = SomaBodyRepresentation.JOINT_ROTATIONS, ) -> BodyViewPipeline: - selected = resolve_body_schema(body_schema, soma_data_root) + selected = resolve_body_schema(body_schema) if selected is BodySchema.FULL_BODY_POSE: from .full_body_pose import create_full_body_pose_pipeline @@ -63,7 +54,4 @@ def create_body_view_pipeline( from .soma_body import create_soma_body_pipeline - assert soma_data_root is not None - return create_soma_body_pipeline( - soma_data_root, soma_body_collection_id, soma_body_representation - ) + return create_soma_body_pipeline(soma_body_collection_id, soma_body_representation) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py index 9a3cf3ce4e..1621dd933f 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py @@ -14,7 +14,6 @@ def build_all_human_pipeline( - soma_data_root=None, *, body_schema=None, soma_body_collection_id="soma_body_demo", @@ -26,7 +25,6 @@ def build_all_human_pipeline( head = HeadSource(name="head") controllers = ControllersSource(name="controllers") selected_body = body or create_body_view_pipeline( - soma_data_root, body_schema=body_schema, soma_body_collection_id=soma_body_collection_id, soma_body_representation=soma_body_representation, diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py index 0e82e33853..3c5589a131 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py @@ -5,7 +5,6 @@ from dataclasses import dataclass from enum import Enum -from pathlib import Path from isaaccapture.retargeting_engine.deviceio_source_nodes import ( SomaHandRepresentation, @@ -36,7 +35,6 @@ class HandViewPipeline: def create_hand_view_pipeline( - soma_data_root: Path | None = None, *, hand_schema: HandSchema | str = HandSchema.OPENXR_HAND_POSE, soma_left_collection_id: str = "soma_hand_left_demo", @@ -51,17 +49,9 @@ def create_hand_view_pipeline( return create_openxr_hand_pipeline() representation = SomaHandRepresentation(soma_hand_representation) - if ( - representation is SomaHandRepresentation.JOINT_ROTATIONS - and soma_data_root is None - ): - raise ValueError( - "--hand-schema soma with joint-rotations requires --soma-data-root" - ) from .soma_hand import create_soma_hand_pipeline return create_soma_hand_pipeline( - soma_data_root, soma_left_collection_id, soma_right_collection_id, representation, diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py index 040e08e43f..4a28f9e3b8 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/live_deviceio.py @@ -20,7 +20,6 @@ import argparse import time -from pathlib import Path import viser @@ -51,9 +50,8 @@ def main(argv: list[str]) -> int: parser.add_argument( "--body-schema", choices=[schema.value for schema in BodySchema], - help="Input body schema (default: full-body-pose, or soma when assets are supplied)", + help="Input body schema (default: full-body-pose)", ) - parser.add_argument("--soma-data-root", type=Path, help="SOMA POC assets directory") parser.add_argument( "--soma-body-collection-id", default="soma_body_demo", @@ -91,15 +89,13 @@ def main(argv: list[str]) -> int: args = parser.parse_args(argv[1:]) try: - body_schema = resolve_body_schema(args.body_schema, args.soma_data_root) + body_schema = resolve_body_schema(args.body_schema) body = create_body_view_pipeline( - args.soma_data_root, body_schema=body_schema, soma_body_collection_id=args.soma_body_collection_id, soma_body_representation=args.soma_body_representation, ) hands = create_hand_view_pipeline( - args.soma_data_root, hand_schema=args.hand_schema, soma_left_collection_id=args.soma_left_hand_collection_id, soma_right_collection_id=args.soma_right_hand_collection_id, diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py index 1fd04215aa..551e99913b 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py @@ -15,10 +15,113 @@ from .body_pipeline import BodyViewLayout, BodyViewPipeline -def create_layer(data_root: Path): +# SOMA-X v0.3.1 public hierarchy with virtual Root omitted. Hips uses -1. +SOMA_BODY_PARENT_IDS: tuple[int, ...] = ( + -1, + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 6, + 6, + 6, + 3, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 14, + 19, + 20, + 21, + 22, + 14, + 24, + 25, + 26, + 27, + 14, + 29, + 30, + 31, + 32, + 14, + 34, + 35, + 36, + 37, + 3, + 39, + 40, + 41, + 42, + 43, + 44, + 45, + 42, + 47, + 48, + 49, + 50, + 42, + 52, + 53, + 54, + 55, + 42, + 57, + 58, + 59, + 60, + 42, + 62, + 63, + 64, + 65, + 0, + 67, + 68, + 69, + 70, + 0, + 72, + 73, + 74, + 75, +) + +SOMA_BODY_LAYOUT = BodyViewLayout( + joint_names=tuple( + name + for name, joint in sorted( + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ), + bones=tuple( + (parent, child) + for child, parent in enumerate(SOMA_BODY_PARENT_IDS) + if parent >= 0 + ), + positions_index=int(SomaBodyInputIndex.JOINT_POSITIONS), + valid_index=int(SomaBodyInputIndex.JOINT_VALID), +) + + +def create_layer(data_root: Path | None = None): import soma import torch + # Resolve through the public helper because py-soma-x 0.3.1's body-layer + # fallback imports a nonexistent soma.body.assets module. + if data_root is None: + data_root = soma.get_assets_dir() layer = soma.SOMALayer( data_root=str(data_root), identity_model_type="soma", @@ -37,6 +140,12 @@ def create_layer(data_root: Path): ] if [name.upper() for name in layer.public_joint_names] != ["ROOT", *expected]: raise ValueError("SOMA layer joint order differs from the FBS") + parent_ids = tuple( + -1 if parent == 0 else int(parent) - 1 + for parent in layer.output_joint_parent_ids[1:] + ) + if parent_ids != SOMA_BODY_PARENT_IDS: + raise ValueError("SOMA layer hierarchy differs from the viewer layout") with torch.no_grad(): layer.prepare_identity( torch.zeros((1, layer.identity_model.num_identity_coeffs)) @@ -45,23 +154,23 @@ def create_layer(data_root: Path): def create_soma_body_pipeline( - data_root: Path, collection_id: str, representation: SomaBodyRepresentation | str = SomaBodyRepresentation.JOINT_ROTATIONS, ) -> BodyViewPipeline: + representation = SomaBodyRepresentation(representation) + layer = ( + create_layer() + if representation is SomaBodyRepresentation.JOINT_ROTATIONS + else None + ) source = SomaBodySource( "body", collection_id, - create_layer(data_root), + layer, representation=representation, ) return BodyViewPipeline( output=source.output(SomaBodySource.BODY), - layout=BodyViewLayout( - joint_names=source.joint_names, - bones=source.bones, - positions_index=int(SomaBodyInputIndex.JOINT_POSITIONS), - valid_index=int(SomaBodyInputIndex.JOINT_VALID), - ), + layout=SOMA_BODY_LAYOUT, ) diff --git a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py index 429cc06458..e9ff49c901 100644 --- a/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py @@ -56,10 +56,12 @@ ) -def create_layer(data_root: Path, hand_type: str): +def create_layer(hand_type: str, data_root: Path | None = None): import soma import torch + if data_root is None: + data_root = soma.get_assets_dir() layer = soma.SOMAHandLayer( data_root=str(data_root), hand_type=hand_type, @@ -86,15 +88,13 @@ def create_layer(data_root: Path, hand_type: str): def create_soma_hand_pipeline( - data_root: Path | None, left_collection_id: str, right_collection_id: str, representation: SomaHandRepresentation, ) -> HandViewPipeline: layers = (None, None) if representation is SomaHandRepresentation.JOINT_ROTATIONS: - assert data_root is not None - layers = (create_layer(data_root, "left"), create_layer(data_root, "right")) + layers = (create_layer("left"), create_layer("right")) left = SomaHandSource( "hand_left", left_collection_id, diff --git a/examples/soma_hand_publisher/README.md b/examples/soma_hand_publisher/README.md index 271ad08099..4e47ac81a2 100644 --- a/examples/soma_hand_publisher/README.md +++ b/examples/soma_hand_publisher/README.md @@ -35,8 +35,7 @@ publisher in another terminal: ```bash uv run --no-sync python -m isaaccapture_examples.deviceio_live_view \ --accept-eula \ - --hand-schema soma \ - --soma-data-root /path/to/SOMA-X-v0.3.1-assets + --hand-schema soma source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ diff --git a/tests/python/examples/deviceio_live_view/test_soma_viewer.py b/tests/python/examples/deviceio_live_view/test_soma_viewer.py index 8e349c9e92..b492fb6548 100644 --- a/tests/python/examples/deviceio_live_view/test_soma_viewer.py +++ b/tests/python/examples/deviceio_live_view/test_soma_viewer.py @@ -128,16 +128,14 @@ def test_default_pipeline_keeps_full_body_source(): } -@pytest.mark.parametrize("explicit", [False, True]) -def test_soma_pipeline_replaces_only_body_source(monkeypatch, tmp_path, explicit): - monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) +def test_soma_pipeline_replaces_only_body_source(monkeypatch): + monkeypatch.setattr(soma_body, "create_layer", fake_layer) tracker_factory = MagicMock(wraps=SomaBodyJointRotationsTracker) monkeypatch.setattr( soma_body_source, "SomaBodyJointRotationsTracker", tracker_factory ) body = create_body_view_pipeline( - tmp_path, - body_schema=BodySchema.SOMA if explicit else None, + body_schema=BodySchema.SOMA, soma_body_collection_id="vendor_body", ) pipeline = build_all_human_pipeline(body=body) @@ -150,12 +148,12 @@ def test_soma_pipeline_replaces_only_body_source(monkeypatch, tmp_path, explicit assert len(body.layout.bones) == 76 -def test_soma_joint_pose_pipeline_selects_direct_tracker(monkeypatch, tmp_path): - monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) +def test_soma_joint_pose_pipeline_selects_direct_tracker(monkeypatch): + create = MagicMock(side_effect=AssertionError("joint poses must not load SOMA")) + monkeypatch.setattr(soma_body, "create_layer", create) tracker_factory = MagicMock(wraps=SomaBodyJointPosesTracker) monkeypatch.setattr(soma_body_source, "SomaBodyJointPosesTracker", tracker_factory) body = create_body_view_pipeline( - tmp_path, body_schema=BodySchema.SOMA, soma_body_collection_id="vendor_body", soma_body_representation=SomaBodyRepresentation.JOINT_POSES, @@ -164,6 +162,7 @@ def test_soma_joint_pose_pipeline_selects_direct_tracker(monkeypatch, tmp_path): source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") assert isinstance(source.get_tracker(), SomaBodyJointPosesTracker) tracker_factory.assert_called_once_with("vendor_body") + create.assert_not_called() def test_soma_joint_pose_hands_replace_only_openxr_hands(): @@ -224,19 +223,19 @@ def test_soma_hand_pose_reaches_native_25_joint_renderer(): assert len(viz.hand_left.bones.points) == len(SOMA_HAND_LAYOUT.bones) -def test_explicit_full_body_selection_does_not_load_soma(monkeypatch, tmp_path): +def test_explicit_full_body_selection_does_not_load_soma(monkeypatch): create = MagicMock(side_effect=AssertionError("SOMA must remain optional")) monkeypatch.setattr(soma_body, "create_layer", create) - body = create_body_view_pipeline(tmp_path, body_schema=BodySchema.FULL_BODY_POSE) + body = create_body_view_pipeline(body_schema=BodySchema.FULL_BODY_POSE) pipeline = build_all_human_pipeline(body=body) source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") assert type(source) is FullBodySource create.assert_not_called() -def test_soma_evaluation_runs_once_per_step_downstream(monkeypatch, tmp_path): - monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) - body = create_body_view_pipeline(tmp_path) +def test_soma_evaluation_runs_once_per_step_downstream(monkeypatch): + monkeypatch.setattr(soma_body, "create_layer", fake_layer) + body = create_body_view_pipeline(body_schema=BodySchema.SOMA) selector = body.output pipeline = build_all_human_pipeline(body=body) source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") @@ -312,16 +311,15 @@ def test_viewer_uses_selected_native_body_layout_and_hides_missing_devices(): @pytest.mark.parametrize( - "schema,use_assets,expected,collection", + "schema,expected,collection", [ - (None, False, BodySchema.FULL_BODY_POSE, "soma_body_demo"), - (None, True, BodySchema.SOMA, "soma_body_demo"), - ("soma", True, BodySchema.SOMA, "vendor_body"), - ("full-body-pose", True, BodySchema.FULL_BODY_POSE, "soma_body_demo"), + (None, BodySchema.FULL_BODY_POSE, "soma_body_demo"), + ("soma", BodySchema.SOMA, "vendor_body"), + ("full-body-pose", BodySchema.FULL_BODY_POSE, "soma_body_demo"), ], ) def test_live_cli_selects_source_and_launches_runtime( - monkeypatch, tmp_path, schema, use_assets, expected, collection + monkeypatch, schema, expected, collection ): body = create_body_view_pipeline() hands = create_hand_view_pipeline() @@ -345,18 +343,14 @@ def test_live_cli_selects_source_and_launches_runtime( args = ["viewer"] if schema is not None: args += ["--body-schema", schema] - if use_assets: - args += ["--soma-data-root", str(tmp_path)] args += ["--soma-body-collection-id", collection] assert live_deviceio.main(args) == 0 select.assert_called_once_with( - tmp_path if use_assets else None, body_schema=expected, soma_body_collection_id=collection, soma_body_representation="joint-rotations", ) select_hands.assert_called_once_with( - tmp_path if use_assets else None, hand_schema="openxr-hand-pose", soma_left_collection_id="soma_hand_left_demo", soma_right_collection_id="soma_hand_right_demo", @@ -370,14 +364,12 @@ def test_live_cli_selects_source_and_launches_runtime( @pytest.mark.parametrize( "args,message", [ - (["--body-schema", "soma"], "requires --soma-data-root"), (["--body-schema", "unknown"], "invalid choice"), + (["--soma-data-root", "/unused"], "unrecognized arguments"), ( [ "--body-schema", "soma", - "--soma-data-root", - "/unused", "--soma-body-collection-id", "", ], @@ -386,7 +378,7 @@ def test_live_cli_selects_source_and_launches_runtime( ], ) def test_bad_body_selection_fails_before_launch(monkeypatch, capsys, args, message): - monkeypatch.setattr(soma_body, "create_layer", lambda _: fake_layer()) + monkeypatch.setattr(soma_body, "create_layer", fake_layer) launch = MagicMock() monkeypatch.setattr(live_deviceio.CloudXRLauncher, "launch_context", launch) server = MagicMock() @@ -399,8 +391,10 @@ def test_bad_body_selection_fails_before_launch(monkeypatch, capsys, args, messa server.assert_not_called() -def test_native_soma_pose_reaches_renderer(soma_assets): - body = create_body_view_pipeline(soma_assets) +def test_native_soma_pose_reaches_renderer(monkeypatch, soma_assets): + create_layer = soma_body.create_layer + monkeypatch.setattr(soma_body, "create_layer", lambda: create_layer(soma_assets)) + body = create_body_view_pipeline(body_schema=BodySchema.SOMA) pipeline = build_all_human_pipeline(body=body) source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") source._tracker = MagicMock() @@ -433,8 +427,16 @@ def test_native_soma_pose_reaches_renderer(soma_assets): assert 0 < active["body_joints"] < 77 -def test_native_soma_hand_rotations_reach_renderer(soma_assets): - hands = create_hand_view_pipeline(soma_assets, hand_schema=HandSchema.SOMA) +def test_native_soma_hand_rotations_reach_renderer(monkeypatch, soma_assets): + from isaaccapture_examples.deviceio_live_view import soma_hand + + create_layer = soma_hand.create_layer + monkeypatch.setattr( + soma_hand, + "create_layer", + lambda side: create_layer(side, soma_assets), + ) + hands = create_hand_view_pipeline(hand_schema=HandSchema.SOMA) pipeline = build_all_human_pipeline(hands=hands) sources = {source.name: source for source in pipeline.get_leaf_nodes()} inputs = {} @@ -466,18 +468,17 @@ def test_native_soma_hand_rotations_reach_renderer(soma_assets): assert viz.hand_right.points.points.shape == (25, 3) -def test_evaluated_soma_pose_reaches_renderer_without_fk(soma_assets): +def test_evaluated_soma_pose_reaches_renderer_without_fk(monkeypatch): + create = MagicMock(side_effect=AssertionError("joint poses must not load SOMA")) + monkeypatch.setattr(soma_body, "create_layer", create) body = create_body_view_pipeline( - soma_assets, body_schema=BodySchema.SOMA, soma_body_representation=SomaBodyRepresentation.JOINT_POSES, ) pipeline = build_all_human_pipeline(body=body) source = next(node for node in pipeline.get_leaf_nodes() if node.name == "body") source._tracker = MagicMock() - source._evaluator.evaluate = MagicMock( - side_effect=AssertionError("joint poses must bypass FK") - ) + assert source._evaluator is None inputs = {} for node in pipeline.get_leaf_nodes(): inputs[node.name] = {} @@ -493,4 +494,4 @@ def test_evaluated_soma_pose_reaches_renderer_without_fk(soma_assets): viz = fake_viz(body.layout) assert viz.update(pipeline.execute_pipeline(inputs))["body_joints"] == 77 np.testing.assert_array_equal(viz.body.points.points, positions) - source._evaluator.evaluate.assert_not_called() + create.assert_not_called() From 73be4bc12505ba949dab4187c22acb952cc2f3d2 Mon Sep 17 00:00:00 2001 From: Michael Haselton Date: Tue, 6 Oct 2026 16:29:31 -0400 Subject: [PATCH 07/12] refactor(soma): key body and hand joint payloads Replace fixed joint arrays with sorted, unique vectors keyed by typed SOMA joint identifiers. Treat absent entries as unavailable and reconstruct canonical dense arrays at the SOMA-X helper boundary. Update body and hand publishers, schema bindings, evaluators, tests, documentation, and schema goldens. Signed-off-by: Michael Haselton --- docs/source/device/soma.rst | 12 +- docs/source/references/generated_trackers.rst | 8 +- examples/soma_body_publisher/README.md | 3 + .../soma_body_publisher/publisher.py | 34 ++- examples/soma_hand_publisher/README.md | 3 + .../soma_hand_publisher/publisher.py | 32 ++- src/core/schema/fbs/soma_body_common.fbs | 2 +- src/core/schema/fbs/soma_body_joint_poses.fbs | 12 +- .../schema/fbs/soma_body_joint_rotations.fbs | 14 +- src/core/schema/fbs/soma_common.fbs | 13 - src/core/schema/fbs/soma_hand_joint_poses.fbs | 12 +- .../schema/fbs/soma_hand_joint_rotations.fbs | 13 +- .../schema/golden/soma_body_joint_poses.bfbs | Bin 6680 -> 6544 bytes .../golden/soma_body_joint_rotations.bfbs | Bin 6904 -> 6480 bytes src/core/schema/golden/soma_common.bfbs | Bin 1000 -> 448 bytes .../schema/golden/soma_hand_joint_poses.bfbs | Bin 4616 -> 4352 bytes .../golden/soma_hand_joint_rotations.bfbs | Bin 3712 -> 3552 bytes .../python/soma_body_joint_poses_bindings.h | 117 ++++----- .../soma_body_joint_rotations_bindings.h | 105 ++++---- src/core/schema/python/soma_common_bindings.h | 5 - .../python/soma_hand_joint_poses_bindings.h | 118 ++++----- .../soma_hand_joint_rotations_bindings.h | 104 ++++---- .../deviceio_source_nodes/soma_body_source.py | 12 +- .../deviceio_source_nodes/soma_hand_source.py | 11 +- .../utilities/soma_body_evaluator.py | 59 ++++- .../utilities/soma_hand_evaluator.py | 12 +- src/python/isaaccapture/schema/__init__.py | 14 +- .../schema/test_soma_body_joint_poses.cpp | 22 +- .../schema/test_soma_body_joint_rotations.cpp | 70 ++--- .../schema/test_soma_hand_joint_poses.cpp | 21 +- .../schema/test_soma_hand_joint_rotations.cpp | 71 ++---- .../test_soma_body_evaluator.py | 21 +- .../test_soma_body_source.py | 41 +-- .../test_soma_hand_source.py | 37 ++- .../core/schema/test_soma_body_joint_poses.py | 71 +++--- .../schema/test_soma_body_joint_rotations.py | 239 ++++-------------- .../core/schema/test_soma_hand_joint_poses.py | 77 +++--- .../schema/test_soma_hand_joint_rotations.py | 190 ++++---------- .../deviceio_live_view/test_soma_viewer.py | 60 +++-- .../soma_body_publisher/test_publisher.py | 15 +- .../soma_hand_publisher/test_publisher.py | 20 +- 41 files changed, 758 insertions(+), 912 deletions(-) diff --git a/docs/source/device/soma.rst b/docs/source/device/soma.rst index 290567373f..ddf88579db 100644 --- a/docs/source/device/soma.rst +++ b/docs/source/device/soma.rst @@ -52,9 +52,9 @@ Use the representation closest to the producer's native data. identity. * - ``joint-poses`` - ``SomaBodyJointPoses`` and ``SomaHandJointPoses`` - - Publishes global position, orientation, and validity for every joint. - The consumer maps these poses directly and does not run per-frame - forward kinematics. + - Publishes global position and orientation for each provided joint. The + consumer maps these poses directly and does not run per-frame forward + kinematics. ``joint-rotations`` most directly matches the SOMA-X pose-control interface. ``joint-poses`` is an Isaac Teleop transport profile for producers that already @@ -66,6 +66,12 @@ The publisher must convert its native skeleton and reference frame before serialization. See :ref:`tracker-reference` for the complete field and validity contracts. +Body and hand joint values are sorted, unique keyed vectors. Every entry carries +an explicit ``SomaBodyJoint`` or ``SomaHandJoint`` identifier. A missing joint is +unavailable for that frame; producers do not serialize placeholder values for +joints they cannot provide. Consumers expand the keyed values into SOMA-X's +canonical dense order when invoking its body or hand helpers. + Build the examples ------------------ diff --git a/docs/source/references/generated_trackers.rst b/docs/source/references/generated_trackers.rst index 28ad576c54..796675edd2 100644 --- a/docs/source/references/generated_trackers.rst +++ b/docs/source/references/generated_trackers.rst @@ -91,16 +91,16 @@ Generated today - Overrides ``class``; the default would give ``Se3TrackerTracker`` * - ``soma_body_joint_rotations`` - ``SomaBodyJointRotationsTracker`` - - SOMA body joint rotations with 77 controls for the 78-joint skeleton; defined against SOMA-X v0.3.1 + - Keyed SOMA body rotations for up to 77 controls of the 78-joint skeleton; defined against SOMA-X v0.3.1 * - ``soma_body_joint_poses`` - ``SomaBodyJointPosesTracker`` - - Evaluated SOMA positions and orientations for the same 77 public joints + - Keyed evaluated SOMA positions and orientations for up to 77 public joints * - ``soma_hand_joint_rotations`` - ``SomaHandJointRotationsTracker`` - - SOMA hand joint rotations with 25 controls; defined against SOMA-X v0.3.1 + - Keyed SOMA hand rotations for up to 25 controls; defined against SOMA-X v0.3.1 * - ``soma_hand_joint_poses`` - ``SomaHandJointPosesTracker`` - - Evaluated SOMA positions and orientations for the same 25 joints + - Keyed evaluated SOMA positions and orientations for up to 25 joints * - ``oglo_tactile`` - ``OgloTactileTracker`` - MCAP channels are ``oglo``/``oglo_tracked``, so ``channel`` is overridden diff --git a/examples/soma_body_publisher/README.md b/examples/soma_body_publisher/README.md index 3302eab610..037e7a50fd 100644 --- a/examples/soma_body_publisher/README.md +++ b/examples/soma_body_publisher/README.md @@ -15,6 +15,9 @@ It can publish joint rotations for downstream FK or pre-evaluated joint poses. The native process validates the selected payload and publishes the `soma_body_demo` tensor collection. +Each payload uses explicitly keyed SOMA joints. The bundled motion provides all +77 joints; a partial producer can omit joints it does not provide. + ```text SOMA-X example_animation.npy -> Python SOMA body encoder diff --git a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py index 5ebd9d6a90..229bb8b2a9 100644 --- a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py +++ b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py @@ -17,15 +17,25 @@ from isaaccapture.schema import ( Point, - SomaBodyJointPoseArray, + Pose, + Quaternion, + SomaBodyJointPose, SomaBodyJointPoses, - SomaBodyJointRotationArray, SomaBodyJoint, + SomaBodyJointRotation, SomaBodyJointRotations, ) logger = logging.getLogger("isaaccapture.examples.soma_body_publisher") +BODY_JOINTS = tuple( + joint + for name, joint in sorted( + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" +) + def create_layer(data_root: Path): """Create the neutral SOMA-X v0.3.1 model used by the bundled motion.""" @@ -84,9 +94,10 @@ def demo_controls(path: Path, layer): def soma_joint_rotations(quaternions, translation): - joints = SomaBodyJointRotationArray() - joints.rotations[:] = quaternions - joints.is_valid[:] = 1 + joints = [ + SomaBodyJointRotation(joint, Quaternion(*quaternion)) + for joint, quaternion in zip(BODY_JOINTS, quaternions, strict=True) + ] return SomaBodyJointRotations(joints, Point(*translation), True) @@ -108,10 +119,15 @@ def evaluate_demo_controls(layer, rotations, translations): def soma_joint_poses(positions, orientations): - joints = SomaBodyJointPoseArray() - joints.positions[:] = positions - joints.orientations[:] = orientations - joints.is_valid[:] = 1 + joints = [ + SomaBodyJointPose( + joint, + Pose(Point(*position), Quaternion(*orientation)), + ) + for joint, position, orientation in zip( + BODY_JOINTS, positions, orientations, strict=True + ) + ] return SomaBodyJointPoses(joints) diff --git a/examples/soma_hand_publisher/README.md b/examples/soma_hand_publisher/README.md index 4e47ac81a2..351b9599c5 100644 --- a/examples/soma_hand_publisher/README.md +++ b/examples/soma_hand_publisher/README.md @@ -15,6 +15,9 @@ native process validates each payload and publishes independent `soma_hand_left_demo` and `soma_hand_right_demo` collections on one OpenXR session. +Each payload uses explicitly keyed SOMA joints. The bundled motion provides all +25 joints per hand; a partial producer can omit joints it does not provide. + ```text SOMA-X example_animation.npy -> left/right SOMAHandLayer controls and evaluated poses diff --git a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py index 7584698b8c..8d538237b4 100644 --- a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py +++ b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py @@ -17,15 +17,26 @@ from isaaccapture.schema import ( Point, + Pose, + Quaternion, SomaHandedness, - SomaHandJointPoseArray, + SomaHandJoint, + SomaHandJointPose, SomaHandJointPoses, - SomaHandJointRotationArray, + SomaHandJointRotation, SomaHandJointRotations, ) logger = logging.getLogger("isaaccapture.examples.soma_hand_publisher") +HAND_JOINTS = tuple( + joint + for name, joint in sorted( + SomaHandJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" +) + def create_layers(data_root: Path): """Create the neutral full-body and hand layers used by the bundled motion.""" @@ -133,17 +144,20 @@ def demo_hand_frames(path: Path, body, hands): def soma_joint_rotations(rotations, translation, handedness): - joints = SomaHandJointRotationArray() - joints.rotations[:] = rotations - joints.is_valid[:] = 1 + joints = [ + SomaHandJointRotation(joint, Quaternion(*rotation)) + for joint, rotation in zip(HAND_JOINTS, rotations, strict=True) + ] return SomaHandJointRotations(joints, Point(*translation), True, handedness) def soma_joint_poses(positions, orientations, handedness): - joints = SomaHandJointPoseArray() - joints.positions[:] = positions - joints.orientations[:] = orientations - joints.is_valid[:] = 1 + joints = [ + SomaHandJointPose(joint, Pose(Point(*position), Quaternion(*orientation))) + for joint, position, orientation in zip( + HAND_JOINTS, positions, orientations, strict=True + ) + ] return SomaHandJointPoses(joints, handedness) diff --git a/src/core/schema/fbs/soma_body_common.fbs b/src/core/schema/fbs/soma_body_common.fbs index 3e1a65baed..7ad8ddebed 100644 --- a/src/core/schema/fbs/soma_body_common.fbs +++ b/src/core/schema/fbs/soma_body_common.fbs @@ -4,7 +4,7 @@ namespace core; // User-facing SOMA-X body pose order. The public skeleton has 78 joints, but its -// virtual Root is always identity and is not part of transported body arrays. +// virtual Root is always identity and is not transported. // Hips is index 0, matching SOMALayer's 77-entry public pose input. enum SomaBodyJoint : uint8 { HIPS = 0, diff --git a/src/core/schema/fbs/soma_body_joint_poses.fbs b/src/core/schema/fbs/soma_body_joint_poses.fbs index d20f302015..e7348c9313 100644 --- a/src/core/schema/fbs/soma_body_joint_poses.fbs +++ b/src/core/schema/fbs/soma_body_joint_poses.fbs @@ -8,13 +8,8 @@ include "timestamp.fbs"; namespace core; struct SomaBodyJointPose { + joint: SomaBodyJoint (key); pose: Pose; - is_valid: bool; -} - -// Fixed-size evaluated SOMA body joint-pose array in SomaBodyJoint order. -struct SomaBodyJointPoseArray { - values: [SomaBodyJointPose:77]; } // Evaluated SOMA full-body joint-pose transport schema. @@ -24,7 +19,10 @@ struct SomaBodyJointPoseArray { // Positions are meters in a right-handed frame with +Y up and +Z forward. // Orientations are unit XYZW quaternions. The virtual Root is omitted. table SomaBodyJointPoses { - joint_poses: SomaBodyJointPoseArray (id: 0); + // Complete current-pose snapshot, sorted ascending and unique by joint. + // Use CreateVectorOfSortedStructs when building the vector directly. + // A joint absent from this vector is not provided by the producer. + joint_poses: [SomaBodyJointPose] (id: 0); } table SomaBodyJointPosesRecord { diff --git a/src/core/schema/fbs/soma_body_joint_rotations.fbs b/src/core/schema/fbs/soma_body_joint_rotations.fbs index 38b9a2cda9..56ee78f11d 100644 --- a/src/core/schema/fbs/soma_body_joint_rotations.fbs +++ b/src/core/schema/fbs/soma_body_joint_rotations.fbs @@ -3,14 +3,15 @@ include "point.fbs"; include "soma_body_common.fbs"; -include "soma_common.fbs"; +include "quaternion.fbs"; include "timestamp.fbs"; namespace core; -// Fixed-size SOMA body joint-rotation array in SomaBodyJoint order. -struct SomaBodyJointRotationArray { - values: [SomaJointRotation:77]; +// One explicitly identified SOMA body rotation. +struct SomaBodyJointRotation { + joint: SomaBodyJoint (key); + rotation: Quaternion; } // SOMA full-body joint-rotation transport schema. @@ -24,7 +25,10 @@ struct SomaBodyJointRotationArray { // unit XYZW quaternions. Global translation is meters in a right-handed // reference frame with +Y up and +Z forward. table SomaBodyJointRotations { - joint_rotations: SomaBodyJointRotationArray (id: 0); + // Complete current-pose snapshot, sorted ascending and unique by joint. + // Use CreateVectorOfSortedStructs when building the vector directly. + // A joint absent from this vector is not provided by the producer. + joint_rotations: [SomaBodyJointRotation] (id: 0); // Translation applied to Hips in the reference frame. Ignore this value // when global_translation_is_valid is false. diff --git a/src/core/schema/fbs/soma_common.fbs b/src/core/schema/fbs/soma_common.fbs index ae094b25c2..fea03b88be 100644 --- a/src/core/schema/fbs/soma_common.fbs +++ b/src/core/schema/fbs/soma_common.fbs @@ -1,8 +1,6 @@ // SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 -include "quaternion.fbs"; - namespace core; // Hand side shared by the SOMA pose transport schemas. @@ -11,14 +9,3 @@ enum SomaHandedness : uint8 { LEFT = 1, RIGHT = 2, } - -// One SOMA rotation and its tracking validity. -// -// Body and hand profiles define the joint order and reference frame. -// Valid rotations are unit XYZW quaternions. q and -q represent the same -// rotation; no quaternion-sign convention is required. Ignore rotation when -// is_valid is false. -struct SomaJointRotation { - rotation: Quaternion; - is_valid: bool; -} diff --git a/src/core/schema/fbs/soma_hand_joint_poses.fbs b/src/core/schema/fbs/soma_hand_joint_poses.fbs index 663117d149..1df71e7678 100644 --- a/src/core/schema/fbs/soma_hand_joint_poses.fbs +++ b/src/core/schema/fbs/soma_hand_joint_poses.fbs @@ -9,13 +9,8 @@ include "timestamp.fbs"; namespace core; struct SomaHandJointPose { + joint: SomaHandJoint (key); pose: Pose; - is_valid: bool; -} - -// Fixed-size evaluated SOMA hand joint-pose array in SomaHandJoint order. -struct SomaHandJointPoseArray { - values: [SomaHandJointPose:25]; } // Evaluated SOMA hand joint-pose transport schema. @@ -25,7 +20,10 @@ struct SomaHandJointPoseArray { // Positions are meters in a right-handed frame with +Y up and +Z forward. // Orientations are unit XYZW quaternions. table SomaHandJointPoses { - joint_poses: SomaHandJointPoseArray (id: 0); + // Complete current-pose snapshot, sorted ascending and unique by joint. + // Use CreateVectorOfSortedStructs when building the vector directly. + // A joint absent from this vector is not provided by the producer. + joint_poses: [SomaHandJointPose] (id: 0); handedness: SomaHandedness = UNSPECIFIED (id: 1); } diff --git a/src/core/schema/fbs/soma_hand_joint_rotations.fbs b/src/core/schema/fbs/soma_hand_joint_rotations.fbs index 8bbe270598..65741c98ba 100644 --- a/src/core/schema/fbs/soma_hand_joint_rotations.fbs +++ b/src/core/schema/fbs/soma_hand_joint_rotations.fbs @@ -2,6 +2,7 @@ // SPDX-License-Identifier: Apache-2.0 include "point.fbs"; +include "quaternion.fbs"; include "soma_common.fbs"; include "timestamp.fbs"; @@ -39,9 +40,10 @@ enum SomaHandJoint : uint8 { NUM_JOINTS = 25, } -// Fixed-size SOMA hand joint-rotation array in SomaHandJoint order. -struct SomaHandJointRotationArray { - values: [SomaJointRotation:25]; +// One explicitly identified SOMA hand rotation. +struct SomaHandJointRotation { + joint: SomaHandJoint (key); + rotation: Quaternion; } // SOMA hand joint-rotation transport schema. @@ -54,7 +56,10 @@ struct SomaHandJointRotationArray { // quaternions. Global translation is meters in a right-handed reference frame // with +Y up and +Z forward. table SomaHandJointRotations { - joint_rotations: SomaHandJointRotationArray (id: 0); + // Complete current-pose snapshot, sorted ascending and unique by joint. + // Use CreateVectorOfSortedStructs when building the vector directly. + // A joint absent from this vector is not provided by the producer. + joint_rotations: [SomaHandJointRotation] (id: 0); // Translation applied to Wrist in the reference frame. Ignore this value // when global_translation_is_valid is false. diff --git a/src/core/schema/golden/soma_body_joint_poses.bfbs b/src/core/schema/golden/soma_body_joint_poses.bfbs index f2a39339523424c399edd9f79ae1856e1a4e47ed..894b1cdfca1fb8804a55bd717c94e6be49a604f4 100644 GIT binary patch literal 6544 zcmai2KX6+~8UK_xiQ&XqxNzZuf+9r<6)94YwAFu&iv-SV+5gC*r^hVK&TmnXb7{KS)YXU8iQJ|BA(D@8F z2P8ftG6PeU&x#a3hi4eL5B~0Xk+T@T3)}*70R7NwA8`4P$Hx(H`J0mCGq`*{4J2T1 z1?@}p&0(yD-%fW0&wcRap4ad!(C70hU=L$^cy6M9N9l>2M}Dk;pazr(dJrQLx{P)h zpbbzZ9$&66luNhkl@Cg@^@WA{;>BOwUKZK^6MR|MtOJn;o_rd1fKea}Y)3>M0CPYR z=m%N@hzY0w3E<#Wku6{q$N*tr`%@yn2WEgM@B)E81m=JdVE+}7$G{Sh1P(tbvI*P+ zQb0fO{1YO-1B$>9u#1Er0X5(faD<850#<=hpbvP4$yx`dfH1Iq25|s0Kor?U>%qOq5$hZTfbAiSU|bQQSEE> z`Namf9~~ca94X5UfYL-C%$S-ZP08Q9QMxvn%@+$Ijs4@}82V6Vv)0fUX(+{tu(y`NFg)-uPG{jk(6Si|?A{jVcb~^L$NAP8K~sPCsk) zmpbYfGTA)xSN1bjzp3;wt=4`neHFhOe-85Jvy+zMu$H;>gic>J6mPG5y!4G9r_Z`mmOANs>C3LNf5lUmrqahU_UwDfq1%a(TmeXcItZ{9jL zH6@Q_kq3(RL(99>oyW3BOYweSdH1{XSQZH%1o?g6^0vG4SQgm;FNT(R@O*MYdoD*D zQ#!{R*;Fd$-XE*V@AoV@qquy(srq&M(L*>^@6m;hlw{2e;F5}x(JTp<~ zYxdwVzQMl-c_#2o+3CS!Tsg&y8y0d#pUbgRoQ&%wbYf^(pNoJK+H>5t`V=!aZ;ZS8 zJXLjac4R4<7SO{v)W*w=D0l2akEy6z`jsmq4G(F*xt|9>Cue zhe2JwVfEQm6el=WCnl%UrLk!o^eyG*g5}*;oZ#Fu>QYmFzOMW{SDe64Av1Y1mr76L z?>+F``~GW=hd!6jYv?oQSKCq1P`s~N-X+E3I%wmW?Y5zK!G)`+~+}UP8<`m0X?_g zzG%ryipzc8&gH_{Q@k%&-lHBoE}ZDU1Am{lyca!qTsTF=7n9r?@;Cx?_^zqqhVL^SaV8K@lN{lj|Cb0PmmvVC_CMGCcm8Gai3Bm z=Q^X1OFCht7pT4ZicXixjOl1Uee7;Gs&xI!>;U+FTp z0>0eR2r6uDbLheI|FiwaZQwIQDZD zBgbd(A7i}*TmX2keEd#hs=i!Zo~~k`g7H=CNi?AlZpqx8Sk&t?+mCj+o*MHD)#XNc zVF|z2;8#L@zJAW?bDpTr7%SyQ84dC|dDF%0m?M3$V$^Ti_A*+&v;R-bxTlHO3hR{x zf}AmTIE;yTdo_$8qjTntjh?)B(5nNCEe3Ee{*qTbr6tzGynE3V8pVGJ!0EB&IjQgA z6wh47vF5Bx4B*+(IevWSV9#S1wGL{x=JgHG25Uioj7a2lKU;KJVTCDYF$xKtHut4o zzJtityDw$7UcSbhu}j-zo7mBQXy={Zn!^D1v73KRd_3;YxXF=w<}pwMs83y!XR7+% z{A@La=`N}zB2Que6ryl8?{dvxDbWvO;T~DWa;jBJbX2-ie!o;Prb@H5`s}YTEdAyk z+3(xfYO}!_XdB>_Yxb7ZS&71iUK8#e=G{KiOAyl=cqqSDp0AZ}yRbT$lwrKfpc>*j zi2Uf+p=-tzDUX#E*fcwbQj9U=DdF#!&>NhNO_;Su$&+rj~hO6OYFaH6~ z1#8GPT)#74U2K#a^Yuk=-2U(l*&l(u_F8a0g8d-_v`erbS|EAoi-dRK-UgqA|Fm%>|3c&r%wPkAXQ(s;7QP0!Jlk1rFHof}3;$nFpeS`3S z3Q;+o7i~Jfh0Z1DaBn#so+0S=?RT#uet%Qq03oeV%8*MpsN?>-qmbsmq}VqhNLn&8=R*7(sm?X>ALC&tTjf^#+u T&@ayrx7Mj)-v@pz7~}r{@<`Z< literal 6680 zcmai2J#brD6+TLwMC=OeWCa8;%UjTb0TX+71sE`3JCY+Owrp%=$j)R&Q7kcQkgZ^A zvkv283l}L|P`Ff)!bJ*~F1B#tB1MW6DqL*gB83N1WCpWq*Z>CeefQn_-mzAZj_31z z@80vBd(OFkde;+?k5!bzw^S`j#s5h5YSG@m_wmGCyBgxb(Z*OJv059NK*wT_$IiX7a}T zWP$DTh#eRLI)FNoTLaz$l0X#L?G$+c6oG!=@Jk{)z$%adVnF?iBJ02npc{A!$2WlY zfL`F>Rgo>=4v++1yn?=g4}laA1)h9CZPay(`$i=|65NdE}3f3-5VP{lkxI6h`OLY4`DQv2KSG0d2}BTUI&pU7XG9Lwe~ z^JCEWP=_>|u|;8|t~f}iOpjg3%=F{@v0sy~w%%Od{2hw*2ueog6PSgrl>^c6hEpzon%{Csy( zR~+VYJUyzzmz3g-Iugn#>RtTITfp9F~-doTsD&8+G@44b(3)k^Y zk7ciz)YKGj(DD+fb9rID{dsP3N*>c9yNdS<%e&c{$FxZ7FpTf#mbc!T$FxXF@qT7` zPg?Vs7Q^#UGL8YjNfh>6j5v~Xj1yz2)VSL}wpG7BwPgC@xZK}Veta%6bf|bgvAmUb zJY=Z%A7OreYx>_3^{To-je!Nq5+wkbus^aw-7Um-ATKP%8$n#J# zKbHU}8O3eLPcCzPV#MX=NcH<8OQy}2^lP|`Gx=GVpI*z`Zo^}oWySlU4qR#mlo_E{_Fji>upvSy?-|Dltr8wcaIyyO>&JR!HppU%>5_K*Q^rL}i zmfODKB`mMpnrD_<_rJn?e8=)OTJy{@%PHQsEpM+i&n&Yw#e2i@;;3`+T|iwJUvNwa zc^=vrKT9W5ii@AT4cw$WQoL_D63Y86dGE-P;=QhTJ1u!{OY+~LpWjrx7cF^jNm=pu zyVOZP>RoQQ&cocCI3{EtdhYk^H!PVp9rt__E+RS29iStPD@F`|wT=7n>r|^h4QapZ#QC1c224< z>#3}G952dYYo1w88;ZwRD7~n2d1>OOSxDjwG)KNe&QZ60*!8^0@@EGtg% zy%8P@lK5Yk4}Ql`HWerM9`KI^>4v@s9rlCIO6doF`Rejq>9dVeRr>gpmaD_RmH2TOFFwzW)RZ2%ne=eV{f>x1-$Nba z#pin`tPLPj0*>2`19lN zxd{-sA9NXeKASg3&K_-2KQlHp!&b)P;$z!m{G-3OfF6Kn%jfSkrYcLNrRfqHifH$+ zE>T1x7%}d(wEq$IA{1r`(k|yzb#A`2R4vTk!Sj9eE0K;s|C!ZiUYw7`LbZTb#B=iY zi_tMg_Qm{CzYG6Z4g%b_U*a?FYP#&`=-CB=oH6$}+NTviu1?M$+UENEJ+F20cNo9f z-`T|cF~5xa0RmlRC6EAkMmG0lcyY0C&+JW9XRiHm>w=v#@R9!U_EY41=Y+9M`>efN zD8G;HrNicZ2zC7c+dW&dUws#-{5BVY9OD#F1$bUH_Yn>9I{cy^XW>($yn~wa;u_ro$^iALYhp{4?#|7YQkYUXH4Wtv?4Lq(&gKf|C8jI;p)VZUB~00JDbJ4b zi-iyJMPn*ITdvIh0nHLMdvP?dvD09KInp-3E0pc+th3S$8$Hl-bHMGD{tY;L34MA4 zFNM2>xpLvQbE}z2bJjpL!nxrA%q!zz$ups^k6HHbhnf2%@FBppYI4S9j%VJya|T2W z3l)K!GxqR`wa;D5x4q@Z-^sCH4!LeBi*u!gYN0w;SpdhaK{T9@&|YIMI38|2;GvUt z3DzTuIb#^l7yZ5?LwKeQ#_HB4%f7xpzF(-878l@%AGxoSK-(Hn1h|KAZkZhXCQz3H z%-hq5lVe1?JTpU{YCFFfx~!7Hcpv_a=j&>4VE1Qwi5hwpP>=N@!=QRmtd4eiV9 z2U+CYIEUz{$C>Vz5=r9C1L!07N0uA|jyHC%V4cuT1|W~~!H&7Px86gaI*x}8J}?e{ nPVoFWYx>bKecYhWnCLIhB92)CV81-4++3#y#|RSSft*`!Q8)L2a_NPq_qv!pNvVP zMszt_uy;@=wva)ej7KyFeII#;h)@5-ASm@@>dX}q3JuaoI6={m&2OrvYy&rxpT;<3 zHQKQkV@}5`?Wkv-(Y+q!j6qN-3{bdMCd)@U;*dc|*}ra`<2yYNilbikJphBCmX~$CBHngeESvZ>+!K$Wx4h@yBaa~`KoJqDDF3$Tm#~^E zCAhe{u(AeupXCQ{)=6b$vf%Gz8-^$Rg!2iv`ZMsQ;XOZL2Qe`3gd6zQ?>vpw+^8?# C`*gwp delta 1031 zcmZ8g&ref95S}d$Y_SQiKi+GbAT+@kLYiU-Y7fQ~ELcb(4aK8Er8P8xzJykz#-u%X z;J~Hs%^1TUz`+Oy4;(!ZPn-;Y02(j;I2hANW&NhGN?iD|JKxNHJ2P*(KhnH17A2zb z$?+UE3Q~*=a+8-#oWm5QUhG6^AOXaHC=dq8d=?Omu|Okhocz~ZU^WHO4)$}vicPc? z0m4LYEuwW?+Y1oYLquKhKLwV7dEhdDFl0dV`J+-9+~qT`ChTS0VH@)F2!Y;DO4Xss z9Ahy;HA=vnM4*YwF$4_4)*{zoCvj2R8Q3u-r|lu7)F_0afE#E+qjjtljs`!f(SdUv z;PQI`)I&?oS}~rhIji|}zFa64%Ed~BUVc$33a1HhXY^VGuf;k+&Q0A)a|evCO0l39 zU_Y;oGqId#W@3^=RKX1h@}d_v%Z==Sd%$rph>{~;GIjKB2Zk$fMx;o}~257vP};6O%#1ODA# zO1aQCt?{lUXM=J1%(~?LC%o5^&x6yJj_Li~e!GX&gcMfD9&DXtlLW)xg3B|yWZW9^ za7XyLet9QwAvLzPmVZnKCjx?b?uLm!gF2B0PZ!TIk*{Z_ZLt)y5A!PziWQ>$mK?Y5 z`nT|+3xPVPon!gN9`Sz%EzXqFJeEK05!1kfIx>OI$#iJ6);i510d>xVU((;khx71o z1%t7$s+5Eq{;Uy1P rfq)K@u~2{1#f@Tcc&W00dP@ZyF{>sxFMhD{mDMe`tgG8kLLbB*V$sNE diff --git a/src/core/schema/golden/soma_common.bfbs b/src/core/schema/golden/soma_common.bfbs index 5ac03e2f95f3158d2aed0d9d2ba255074beb543e..7a9eb4d4def1ae77ae1a5413816826cd10327505 100644 GIT binary patch delta 182 zcmaFCet?-rfq{X+$;~MkNHI+0(6LehF|YtepqLF*9K_}UVh$k50wg(rY!ilwZAzRm zK$Zs(8%#dPC_izB2ImqWCk2Q#Ccc!OY``dC%LL?rz?A?0|AWi{VOAgx@^tqI0n$7S Y5`0dIZt!k^L@9X$ z3QEd60tF92K|w*8$B0xZk&PmKhtF2>jtB*ChbBp+DAhcP;kobzN!{0lD z@9SoFtF_u`cUn#J_dd^Y1@9U3Cd#iInrA_yky6<1eli>;@$$F56rbv8f(tMY#`Ui! zgE-qxvM3uQF#+?MTzdT}+f#X-Y?`PnAYQb0JME!;14aDLVOj9{^)`ca_b56T^u(!i z+EuUJq?PUa;0aXMWNzh9_w@I$ml~8`{?UYb{a@uS!_;~Ie^~?{8I)IjQXW4y8izlk ztUrqBz+<~?Y!S`{_zpe*U26OGc}zE#ymOKJeE(0`L!0rnAG5i`S3s5L{CURt1xJ-h kcZt5ce^eUtZ=dmh!gt&XvoX@?dZoI(xj)2itmTBw0}91!1ONa4 diff --git a/src/core/schema/golden/soma_hand_joint_poses.bfbs b/src/core/schema/golden/soma_hand_joint_poses.bfbs index cfd6e2174aa361b5a57ab88c3cd476d3d41c98c7..25119933db64fe217abdbde1a3a3443a945beff5 100644 GIT binary patch literal 4352 zcmai%J#bq^6opr@u^l2}K?DRapv z@h}uFQn*Orf+D4h6e(S#NRc81MT!&@6e&_rPF^clNz~TFVOT%<18;%Uqn7y&7u_w9ZI%a|F^Z5(292dwoO^D@ud zabq418uR!SV;&qe<{rXHP({!B5o3lSEgv?9SF;6ff+ko5+FO?Nf~UW?T2*xEx5h5- zi^%I;SYwYh^dD#69K6JCtF_ENZoSI>&tJk9WYVmkVtt2qr*EA1G34F*4DX}J=v_Vy zp?`?=W#$w7cD(QZY_)E&o&s!V9^uc;y;h5#mJ;KK@HWV5Kk%59O|S??z_T7=1a5)P zz%bZj(+6M!EPyoFMbUk56U>7Y*g@|-unv@yA@CYls;`z$&ed0nlf_!;V!cu$sO#9O zb=)^^0gXxKP-AUWzNRl&cLwKqgC`JpLPw%=1Y%{#ok6km(sRl!Q-kPF6DE%sfobjJx@PJBN`i?jLvZ&lQrpo+VN4% zxpBWM|E!t!_#D${>~!UyG3y>*wz@HdozCC3J*Uk&^Uhz@uEwOt)m(l$H?a^V_*YNA za_`2PC!>0^G9)g>w?J&z;*Kbg(X&C~6YtSUc2jU{aVo8o;Gq<~~~-ZGg64Y9CPUsjM+YUJ0*c|6x;6_}6$g_H{U2xk6mUE}-vME5X^Vmf)hv$MC)~ikl+;b z!`IN4yvET%-9=hga)R1ibvZRVVJ>s5Wr6$?SI_ss!McuQquRa!FAemK`{LO1ts2B_ zUw!LNMr=?$SFw=oZU-tn@}B8%sG>FcSY^a|Z0!dAC++_c(jo5y|E()lxAgnx;kPg^ zx+i469X4Y5PxDBd6d~vP6x$?EjQaMK}oA=tEd5Hc)SmAW8asCiFtbJsQ z^4FkMR?7u#R9G!uFO;mR!cw)q^cBlyfcV6)%;|X0p+hn7bs$F>23MTZqVJ($ctMO3 zHbz!=R3mMhPOwnCR;*NuSDabhRN8Mls4?}z6i~gmGo@ae$*4;Om#lhB7W@EoN7$I@ zdf~VJ%T%rg?0<_)M58~bZ}y16aQ<%x{zvtqK3!j}lxsngLdKn!NKyEst*z!TAo(C>-4%j?)|19O?NM&IogEJ>9W>L;Da# zeWmo{op`GB;Avu6-cmp|su%0294O!1meDzpo=G4%)kBzb`+w*Y_VoR@(_w@B2x>y# lK7Do{-=|+X`0`2i>N`j|O9SnzZ>08jc<_zHbQBBO_%9G31JeKi literal 4616 zcmaKvJ#dso7{{L@Aq0r#(5OKp9y2LYWC$NAQlvnec#>xKKp#` zzLznxLx&HYptBwuw07&XE=%*=YZ>dKHyZ~dU=U=$tu5l36+sHTs%>I&4P5Rp zb1Q9j_jR+CZnOTaX1{bY2a3pjy4~#BHnaPTFTZBSklg@Fpbky}>Cy|@!Rl}8>)#_& z&#wKxzJ3Q8dT#?2_F6*D44gjZ?z4~PgjsZ`Fn1mP2=k-NpW8OLo2?}xwF*RL_30c>e|*<Ka?H4v4Y;y5H-7T?q1fK#Kt4xM(k=#TB;Tz;|HlEJ<0)x3Tk+AqXT4#s^!4pQ zD?a*n!dKl&_vZmewtqG~aU#b_*Tbx06XjRhoqejc4A6L;KwPH2cx?ARQZM^%fFc+G zs`m|NW~sPPs@0II9VA`%Wpuu(_x4^#M=v+3#UtZ6=DWDL!gU$ORKw!>c0X7v8;^}0 z)>77Xu~seGv!5sVA?-BKxO~!?$dgRf%gMp^kmAjiv;9c}NQ@_UE7@0?6zLEjK#oe5{S{^C&DgC^b5 zf4Mc?ikozk55Lw`nLjH(SZ{2YoDQ^8W(zACglS?_k4M}=3*;-lWY=HH|1I$waP^vt;l^$m)I~Ky>~00#Fw7(7@f_K(_u_Aki?Uq zL9iRVsM~b4UZ|I=3ys_xh;-VJd$;3#5#jc2&b<}Rh%on7W4?G^PoH}|O(%yUtQwOK zRo~(l`H^kQK)J2D(M0Q*CpOqUV%}!koV}7M$+{2ZSABCUmuRod)ikWP%|#zmn~h_6khx(XpRPex{{eV0uamVdwQA+<{logxcNIga5q|oCZ1Vn>T|-03N$2#^9irT*0P)3j`ejS!%X6hH z$6PKr8r}EeCN%Ieb>FgNtM{Girp7T^Ddn|Mez9;NUv#4KbCv4cH%!|AI;POf%Xro# zV^@<5){!D})%p4Af|sH*+OHp>URjdpu$z~{`9ir;IPKMFCBl8HK~0VDBVWsx^0#_@ z+r}}CHg?{M&Vej*SHOld%H2J_ewegf$Wp92TnseQXxo@oOEKT(_V#i1-(1l@TSrRj zMLtf7gXh@p}{LrnKD&NHSS;(XNR-;SlT+RU5sd%Z^Ju@_<6EHGV=iM;440QIm zoY2+Ev;JwzvQD>(fuwDTYKuKmFnoNLoAgrNRu{{qh3LR!jjz>2Q2fx4r}%_El5M^g zcu39zjblYwC$Sj4(cYg?edm0e&1}H`fp6Dky?cpxSXh_%xy0 zDtX(6ojzyutqN_sd@o!8`d-u-=sAy?IN!lh98>J-Iqi|oiJVJt1{w47{Y~>1wGUoY z&TPRt@l?rCZ>eG4GC(%#L{zUBDBgT2Bu~#oHa%$K%O>foZ$QON{?@+wKJ+ypMtsjCzL1Xp1Db6-(f|Me diff --git a/src/core/schema/golden/soma_hand_joint_rotations.bfbs b/src/core/schema/golden/soma_hand_joint_rotations.bfbs index d5682a770fa394b3367af77d8a219f06bd4f9d32..f14fa7eea3f84b564548652a811cd1fc15dbc704 100644 GIT binary patch delta 1082 zcmZ8gJxo(k6h8O$wS8@$P)b{C`Fole387S|3Pxg(6ts#25ffZQlZJ>x@ux8kL>+K* z&>W)+Zgnv%Mg|85O&k~;)DWi*xENy`6n{db<@ue;v&7SW_ucbzzkAMk=iO{tZrI;Q zME!&PC&OfsP6nA2q%g%`ZN!tP7ib48K=iDa5Jl8?_t4fG8a8u4yN3vWy67h=m_Psr zeMHL$wg5MLSzrVZLZR1zPb;=Pjx(|sS%q%loall)xbomy2ElH`#lGA2680PM-P8)h z7y=xE9_W2I7NqG=l-a)e*1hRDJuk7O!xjlZds zn&+KbQWg0WR+DdO$wcp(Z7cW%(t(l8@UhdK=m6{vdp+zT=lO${Qga+}CDm&la3!_U z8eesFB-%?=5^2?FX815T=pO72NsTZ-#`&!)rEarZPpapzw zVUx$T!HzoJ2UG!TIM#&*+ttI;CXqMdtdknyl(7=lAM2d+wE2dx?Ex%)6ll?cC4TH# zQ5HY+c1IUc3p`4p*Uk*hm-(}|IkbpGNa_E~#WKf@gV8K*6z&3W7H9>&66cH^VGFOw zMSjfr-w*=~ykJU7-cMWSCVN%N{_o4HLifwQZ5lOA_w`;(W#{*q@P&yMcTX-VS`E=e~Qhxx^cjv1B delta 1237 zcmYjRPe_wt7=PYx-{!Y(E2nKXQPX6F2yW>tjU-C8LNgJSu!}X)XrNAXN?yuCbn2iy zQ5|+LDCp3kKOQ=aDDdFHgGGdghynu-9YPEwr|b7DzveT3@Ap1`zTflbeZPGxo34mf z5mD>8)*d{_OEsjEL2mM3@24s%$CD@mGy*k1InY-?h#=|!6hN~l%Wlu%dCWlsA8k5` zn)d;2yo~)4jBbI8$PWJi&;>LDen1EM*0b3!h?Tv_4d{Vg-3DC;-TM2}!xMFNZ98T4foVClF1WV15}t^y2T5leED9qNHE1(!62XetFnuJTJ&=3Cn1 zkxE5o_`3?L1@>tnwZT!W0iMu8b-i2JtW+bwAgHUY^TOqNqBQIl`vKTRrul;wR`YDP zh14n^w}mushOgU#b&*V-L`K!!)_EQr^c;4JWWO*#M)|!hte)_1kbL4JdPvi^`H~*2 zYuwJ0$a!3Bi$pr2^^}6$BEcAQfB9ks=B>#6`b zF9YK+cKDTJUA6Gjf~IrtkPmjHFm3A=4Pn->6#}_)kS>$);lc4_YAeqodsr=e>o$jZ2o;jc!Jz2;GOzKq2T zS3BE%R!lkO4%R}MisD)4!IByrrzmHzNRyI$L-AW@tyAVA`73$KsEC{$8;ecR%0B`n zb{UZeKOLWvk-KQ}y%H4_mgY2}k^`HJ4NVM=qo)jiH2aI?0%-`eYMS5SR#!vuuiSmh zx3js!cU=u09SN<-W}FMJxEg(#-71>FE|o|oFV8wEdo}DLz#LElTuY1%27402vGzoK zB-x!v#**>G2pn_>^-H~u+?k3cFDASq3Vs`L+Ep3QWEuhkh>u@EQ}F?8@jv8&O+HxS PO0$$(%oDFo_h +#include #include -#include -#include -#include +#include +#include +#include namespace py = pybind11; namespace core { -inline const SomaBodyJointPose& first_soma_body_joint_pose(const py::object& self) -{ - return *(*self.cast().values())[0]; -} - -constexpr py::ssize_t SOMA_BODY_JOINT_POSE_STRIDE = static_cast(sizeof(SomaBodyJointPose)); -constexpr py::ssize_t SOMA_BODY_JOINT_POSE_COUNT = static_cast(SomaBodyJoint_NUM_JOINTS); - -static_assert(sizeof(SomaBodyJointPoseArray) == sizeof(SomaBodyJointPose) * static_cast(SomaBodyJoint_NUM_JOINTS), - "SomaBodyJointPoseArray.values length must equal SomaBodyJoint::NUM_JOINTS"); - inline void bind_soma_body_joint_poses(py::module& m) { - py::class_(m, "SomaBodyJointPose") - .def(py::init<>()) - .def(py::init(), py::arg("pose"), py::arg("is_valid") = false) + py::class_(m, "SomaBodyJointPose", "One keyed evaluated SOMA body pose.") + .def(py::init(), py::arg("joint"), py::arg("pose")) + .def_property_readonly("joint", &SomaBodyJointPose::joint) .def_property_readonly("pose", &SomaBodyJointPose::pose, py::return_value_policy::reference_internal) - .def_property_readonly("is_valid", &SomaBodyJointPose::is_valid); - - py::class_(m, "SomaBodyJointPoseArray") - .def(py::init<>()) - .def( - "values", - [](const SomaBodyJointPoseArray& self, size_t index) -> const SomaBodyJointPose* - { - if (index >= static_cast(SomaBodyJoint_NUM_JOINTS)) - { - throw py::index_error("SomaBodyJointPoseArray index out of range"); - } - return (*self.values())[index]; - }, - py::arg("index"), py::return_value_policy::reference_internal) - .def_property_readonly( - "positions", - [](py::object self) - { - const auto* first = reinterpret_cast(&first_soma_body_joint_pose(self).pose().position()); - return strided_field_view(self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 3); - }, - "Joint positions as a writable (77, 3) float32 view in SomaBodyJoint order.") - .def_property_readonly( - "orientations", - [](py::object self) - { - const auto* first = - reinterpret_cast(&first_soma_body_joint_pose(self).pose().orientation()); - return strided_field_view(self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 4); - }, - "Unit XYZW quaternions as a writable (77, 4) float32 view.") - .def_property_readonly( - "is_valid", - [offset = FBS_FIELD_OFFSET(SomaBodyJointPose, is_valid)](py::object self) - { - const auto* first = fbs_field_address(first_soma_body_joint_pose(self), offset); - return strided_field_view( - self, first, SOMA_BODY_JOINT_POSE_STRIDE, SOMA_BODY_JOINT_POSE_COUNT, 0); - }, - "Per-joint validity as a writable (77,) uint8 view."); + .def("__repr__", [](const SomaBodyJointPose& self) + { return "SomaBodyJointPose(joint=" + std::string(EnumNameSomaBodyJoint(self.joint())) + ")"; }); serialized_class(m, "SomaBodyJointPoses", "Encoded evaluated SOMA body poses.") .def(py::init( - [](const SomaBodyJointPoseArray& joint_poses) + [](std::vector joint_poses) { + std::sort(joint_poses.begin(), joint_poses.end(), + [](const auto& a, const auto& b) { return a.joint() < b.joint(); }); + const auto invalid = + std::find_if(joint_poses.begin(), joint_poses.end(), + [](const auto& entry) { return entry.joint() >= SomaBodyJoint_NUM_JOINTS; }); + if (invalid != joint_poses.end()) + { + throw py::value_error("joint_poses: NUM_JOINTS is not a joint"); + } + const auto duplicate = + std::adjacent_find(joint_poses.begin(), joint_poses.end(), + [](const auto& a, const auto& b) { return a.joint() == b.joint(); }); + if (duplicate != joint_poses.end()) + { + throw py::value_error("joint_poses: duplicate SomaBodyJoint " + + std::string(EnumNameSomaBodyJoint(duplicate->joint()))); + } SomaBodyJointPosesT native; - native.joint_poses = std::make_shared(joint_poses); + native.joint_poses = std::move(joint_poses); return pack(native); }), - py::arg("joint_poses") = SomaBodyJointPoseArray()) - .def_property_readonly( - "joint_poses", field(&SomaBodyJointPoses::joint_poses), py::return_value_policy::reference_internal); + py::arg("joint_poses") = std::vector{}) + .def_property_readonly("joint_poses", + [](const Serialized& self) + { + std::vector out; + const auto* entries = self->joint_poses(); + if (entries != nullptr) + { + out.reserve(entries->size()); + for (const auto* entry : *entries) + { + out.push_back(*entry); + } + } + return out; + }) + .def( + "lookup", + [](const Serialized& self, SomaBodyJoint joint) -> py::object + { + const auto* entries = self->joint_poses(); + const auto* found = entries != nullptr ? entries->LookupByKey(joint) : nullptr; + return found != nullptr ? py::cast(*found) : py::none(); + }, + py::arg("joint"), "Return one provided joint pose, or None when absent."); bind_record(m, "SomaBodyJointPosesRecord", "SomaBodyJointPoses"); } diff --git a/src/core/schema/python/soma_body_joint_rotations_bindings.h b/src/core/schema/python/soma_body_joint_rotations_bindings.h index 7598734be9..55924716f4 100644 --- a/src/core/schema/python/soma_body_joint_rotations_bindings.h +++ b/src/core/schema/python/soma_body_joint_rotations_bindings.h @@ -3,33 +3,21 @@ #pragma once -#include "schema_array_views.h" #include "schema_serialized.h" #include +#include #include -#include -#include -#include +#include +#include +#include namespace py = pybind11; namespace core { -inline const SomaJointRotation& first_soma_body_joint_rotations_rotation(const py::object& self) -{ - return *(*self.cast().values())[0]; -} - -constexpr py::ssize_t SOMA_BODY_JOINT_STRIDE = static_cast(sizeof(SomaJointRotation)); -constexpr py::ssize_t SOMA_BODY_JOINT_COUNT = static_cast(SomaBodyJoint_NUM_JOINTS); - -static_assert(sizeof(SomaBodyJointRotationArray) == - sizeof(SomaJointRotation) * static_cast(SomaBodyJoint_NUM_JOINTS), - "SomaBodyJointRotationArray.values length must equal SomaBodyJoint::NUM_JOINTS"); - inline void bind_soma_body_joint_rotations(py::module& m) { py::enum_(m, "SomaBodyJoint") @@ -112,52 +100,67 @@ inline void bind_soma_body_joint_rotations(py::module& m) .value("RIGHT_TOE_END", SomaBodyJoint_RIGHT_TOE_END) .value("NUM_JOINTS", SomaBodyJoint_NUM_JOINTS); - py::class_(m, "SomaBodyJointRotationArray") - .def(py::init<>()) - .def( - "values", - [](const SomaBodyJointRotationArray& self, size_t index) -> const SomaJointRotation* - { - if (index >= static_cast(SomaBodyJoint_NUM_JOINTS)) - { - throw py::index_error("SomaBodyJointRotationArray index out of range"); - } - return (*self.values())[index]; - }, - py::arg("index"), py::return_value_policy::reference_internal) - .def_property_readonly( - "rotations", - [](py::object self) - { - const auto* first = - reinterpret_cast(&first_soma_body_joint_rotations_rotation(self).rotation()); - return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 4); - }, - "Unit XYZW quaternions as a writable (77, 4) float32 view.") - .def_property_readonly( - "is_valid", - [offset = FBS_FIELD_OFFSET(SomaJointRotation, is_valid)](py::object self) - { - const auto* first = fbs_field_address(first_soma_body_joint_rotations_rotation(self), offset); - return strided_field_view(self, first, SOMA_BODY_JOINT_STRIDE, SOMA_BODY_JOINT_COUNT, 0); - }, - "Per-joint validity as a writable (77,) uint8 view."); + py::class_(m, "SomaBodyJointRotation", "One keyed SOMA body rotation.") + .def(py::init(), py::arg("joint"), py::arg("rotation")) + .def_property_readonly("joint", &SomaBodyJointRotation::joint) + .def_property_readonly("rotation", &SomaBodyJointRotation::rotation, py::return_value_policy::reference_internal) + .def("__repr__", [](const SomaBodyJointRotation& self) + { return "SomaBodyJointRotation(joint=" + std::string(EnumNameSomaBodyJoint(self.joint())) + ")"; }); serialized_class(m, "SomaBodyJointRotations", "Encoded SOMA body joint rotations.") .def(py::init( - [](const SomaBodyJointRotationArray& joint_rotations, const Point& global_translation, + [](std::vector joint_rotations, const Point& global_translation, bool global_translation_is_valid) { + std::sort(joint_rotations.begin(), joint_rotations.end(), + [](const auto& a, const auto& b) { return a.joint() < b.joint(); }); + const auto invalid = + std::find_if(joint_rotations.begin(), joint_rotations.end(), + [](const auto& entry) { return entry.joint() >= SomaBodyJoint_NUM_JOINTS; }); + if (invalid != joint_rotations.end()) + { + throw py::value_error("joint_rotations: NUM_JOINTS is not a joint"); + } + const auto duplicate = + std::adjacent_find(joint_rotations.begin(), joint_rotations.end(), + [](const auto& a, const auto& b) { return a.joint() == b.joint(); }); + if (duplicate != joint_rotations.end()) + { + throw py::value_error("joint_rotations: duplicate SomaBodyJoint " + + std::string(EnumNameSomaBodyJoint(duplicate->joint()))); + } SomaBodyJointRotationsT native; - native.joint_rotations = std::make_shared(joint_rotations); + native.joint_rotations = std::move(joint_rotations); native.global_translation = std::make_shared(global_translation); native.global_translation_is_valid = global_translation_is_valid; return pack(native); }), - py::arg("joint_rotations") = SomaBodyJointRotationArray(), py::arg("global_translation") = Point(), + py::arg("joint_rotations") = std::vector{}, py::arg("global_translation") = Point(), py::arg("global_translation_is_valid") = false) - .def_property_readonly("joint_rotations", field(&SomaBodyJointRotations::joint_rotations), - py::return_value_policy::reference_internal) + .def_property_readonly("joint_rotations", + [](const Serialized& self) + { + std::vector out; + const auto* entries = self->joint_rotations(); + if (entries != nullptr) + { + out.reserve(entries->size()); + for (const auto* entry : *entries) + { + out.push_back(*entry); + } + } + return out; + }) + .def( + "lookup", + [](const Serialized& self, SomaBodyJoint joint) -> py::object + { + const auto* entries = self->joint_rotations(); + const auto* found = entries != nullptr ? entries->LookupByKey(joint) : nullptr; + return found != nullptr ? py::cast(*found) : py::none(); + }, + py::arg("joint"), "Return one provided joint rotation, or None when absent.") .def_property_readonly("global_translation", field(&SomaBodyJointRotations::global_translation), py::return_value_policy::reference_internal) .def_property_readonly( diff --git a/src/core/schema/python/soma_common_bindings.h b/src/core/schema/python/soma_common_bindings.h index c8b76f91a9..7113f6b158 100644 --- a/src/core/schema/python/soma_common_bindings.h +++ b/src/core/schema/python/soma_common_bindings.h @@ -18,11 +18,6 @@ inline void bind_soma_common(py::module& m) .value("LEFT", SomaHandedness_LEFT) .value("RIGHT", SomaHandedness_RIGHT); - py::class_(m, "SomaJointRotation") - .def(py::init<>()) - .def(py::init(), py::arg("rotation"), py::arg("is_valid") = false) - .def_property_readonly("rotation", &SomaJointRotation::rotation, py::return_value_policy::reference_internal) - .def_property_readonly("is_valid", &SomaJointRotation::is_valid); } } // namespace core diff --git a/src/core/schema/python/soma_hand_joint_poses_bindings.h b/src/core/schema/python/soma_hand_joint_poses_bindings.h index e4881142d8..2a5ea85954 100644 --- a/src/core/schema/python/soma_hand_joint_poses_bindings.h +++ b/src/core/schema/python/soma_hand_joint_poses_bindings.h @@ -3,92 +3,82 @@ #pragma once -#include "schema_array_views.h" #include "schema_serialized.h" #include +#include #include -#include -#include -#include +#include +#include +#include namespace py = pybind11; namespace core { -inline const SomaHandJointPose& first_soma_hand_joint_pose(const py::object& self) -{ - return *(*self.cast().values())[0]; -} - -constexpr py::ssize_t SOMA_HAND_JOINT_POSE_STRIDE = static_cast(sizeof(SomaHandJointPose)); -constexpr py::ssize_t SOMA_HAND_JOINT_POSE_COUNT = static_cast(SomaHandJoint_NUM_JOINTS); - -static_assert(sizeof(SomaHandJointPoseArray) == sizeof(SomaHandJointPose) * static_cast(SomaHandJoint_NUM_JOINTS), - "SomaHandJointPoseArray.values length must equal SomaHandJoint::NUM_JOINTS"); - inline void bind_soma_hand_joint_poses(py::module& m) { - py::class_(m, "SomaHandJointPose") - .def(py::init<>()) - .def(py::init(), py::arg("pose"), py::arg("is_valid") = false) + py::class_(m, "SomaHandJointPose", "One keyed evaluated SOMA hand pose.") + .def(py::init(), py::arg("joint"), py::arg("pose")) + .def_property_readonly("joint", &SomaHandJointPose::joint) .def_property_readonly("pose", &SomaHandJointPose::pose, py::return_value_policy::reference_internal) - .def_property_readonly("is_valid", &SomaHandJointPose::is_valid); - - py::class_(m, "SomaHandJointPoseArray") - .def(py::init<>()) - .def( - "values", - [](const SomaHandJointPoseArray& self, size_t index) -> const SomaHandJointPose* - { - if (index >= static_cast(SomaHandJoint_NUM_JOINTS)) - { - throw py::index_error("SomaHandJointPoseArray index out of range"); - } - return (*self.values())[index]; - }, - py::arg("index"), py::return_value_policy::reference_internal) - .def_property_readonly( - "positions", - [](py::object self) - { - const auto* first = reinterpret_cast(&first_soma_hand_joint_pose(self).pose().position()); - return strided_field_view(self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 3); - }, - "Joint positions as a writable (25, 3) float32 view in SomaHandJoint order.") - .def_property_readonly( - "orientations", - [](py::object self) - { - const auto* first = - reinterpret_cast(&first_soma_hand_joint_pose(self).pose().orientation()); - return strided_field_view(self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 4); - }, - "Unit XYZW quaternions as a writable (25, 4) float32 view.") - .def_property_readonly( - "is_valid", - [offset = FBS_FIELD_OFFSET(SomaHandJointPose, is_valid)](py::object self) - { - const auto* first = fbs_field_address(first_soma_hand_joint_pose(self), offset); - return strided_field_view( - self, first, SOMA_HAND_JOINT_POSE_STRIDE, SOMA_HAND_JOINT_POSE_COUNT, 0); - }, - "Per-joint validity as a writable (25,) uint8 view."); + .def("__repr__", [](const SomaHandJointPose& self) + { return "SomaHandJointPose(joint=" + std::string(EnumNameSomaHandJoint(self.joint())) + ")"; }); serialized_class(m, "SomaHandJointPoses", "Encoded evaluated SOMA hand poses.") .def(py::init( - [](const SomaHandJointPoseArray& joint_poses, SomaHandedness handedness) + [](std::vector joint_poses, SomaHandedness handedness) { + std::sort(joint_poses.begin(), joint_poses.end(), + [](const auto& a, const auto& b) { return a.joint() < b.joint(); }); + const auto invalid = + std::find_if(joint_poses.begin(), joint_poses.end(), + [](const auto& entry) { return entry.joint() >= SomaHandJoint_NUM_JOINTS; }); + if (invalid != joint_poses.end()) + { + throw py::value_error("joint_poses: NUM_JOINTS is not a joint"); + } + const auto duplicate = + std::adjacent_find(joint_poses.begin(), joint_poses.end(), + [](const auto& a, const auto& b) { return a.joint() == b.joint(); }); + if (duplicate != joint_poses.end()) + { + throw py::value_error("joint_poses: duplicate SomaHandJoint " + + std::string(EnumNameSomaHandJoint(duplicate->joint()))); + } SomaHandJointPosesT native; - native.joint_poses = std::make_shared(joint_poses); + native.joint_poses = std::move(joint_poses); native.handedness = handedness; return pack(native); }), - py::arg("joint_poses") = SomaHandJointPoseArray(), py::arg("handedness") = SomaHandedness_UNSPECIFIED) - .def_property_readonly( - "joint_poses", field(&SomaHandJointPoses::joint_poses), py::return_value_policy::reference_internal) + py::arg("joint_poses") = std::vector{}, + py::arg("handedness") = SomaHandedness_UNSPECIFIED) + .def_property_readonly("joint_poses", + [](const Serialized& self) + { + std::vector out; + const auto* entries = self->joint_poses(); + if (entries != nullptr) + { + out.reserve(entries->size()); + for (const auto* entry : *entries) + { + out.push_back(*entry); + } + } + return out; + }) + .def( + "lookup", + [](const Serialized& self, SomaHandJoint joint) -> py::object + { + const auto* entries = self->joint_poses(); + const auto* found = entries != nullptr ? entries->LookupByKey(joint) : nullptr; + return found != nullptr ? py::cast(*found) : py::none(); + }, + py::arg("joint"), "Return one provided joint pose, or None when absent.") .def_property_readonly("handedness", field(&SomaHandJointPoses::handedness)); bind_record(m, "SomaHandJointPosesRecord", "SomaHandJointPoses"); diff --git a/src/core/schema/python/soma_hand_joint_rotations_bindings.h b/src/core/schema/python/soma_hand_joint_rotations_bindings.h index 392755fb46..ceeda65be2 100644 --- a/src/core/schema/python/soma_hand_joint_rotations_bindings.h +++ b/src/core/schema/python/soma_hand_joint_rotations_bindings.h @@ -3,33 +3,21 @@ #pragma once -#include "schema_array_views.h" #include "schema_serialized.h" #include +#include #include -#include -#include -#include +#include +#include +#include namespace py = pybind11; namespace core { -inline const SomaJointRotation& first_soma_hand_rotation(const py::object& self) -{ - return *(*self.cast().values())[0]; -} - -constexpr py::ssize_t SOMA_HAND_JOINT_STRIDE = static_cast(sizeof(SomaJointRotation)); -constexpr py::ssize_t SOMA_HAND_JOINT_COUNT = static_cast(SomaHandJoint_NUM_JOINTS); - -static_assert(sizeof(SomaHandJointRotationArray) == - sizeof(SomaJointRotation) * static_cast(SomaHandJoint_NUM_JOINTS), - "SomaHandJointRotationArray.values length must equal SomaHandJoint::NUM_JOINTS"); - inline void bind_soma_hand_joint_rotations(py::module& m) { py::enum_(m, "SomaHandJoint") @@ -60,52 +48,68 @@ inline void bind_soma_hand_joint_rotations(py::module& m) .value("PINKY_END", SomaHandJoint_PINKY_END) .value("NUM_JOINTS", SomaHandJoint_NUM_JOINTS); - py::class_(m, "SomaHandJointRotationArray") - .def(py::init<>()) - .def( - "values", - [](const SomaHandJointRotationArray& self, size_t index) -> const SomaJointRotation* - { - if (index >= static_cast(SomaHandJoint_NUM_JOINTS)) - { - throw py::index_error("SomaHandJointRotationArray index out of range"); - } - return (*self.values())[index]; - }, - py::arg("index"), py::return_value_policy::reference_internal) - .def_property_readonly( - "rotations", - [](py::object self) - { - const auto* first = reinterpret_cast(&first_soma_hand_rotation(self).rotation()); - return strided_field_view(self, first, SOMA_HAND_JOINT_STRIDE, SOMA_HAND_JOINT_COUNT, 4); - }, - "Unit XYZW quaternions as a writable (25, 4) float32 view.") - .def_property_readonly( - "is_valid", - [offset = FBS_FIELD_OFFSET(SomaJointRotation, is_valid)](py::object self) - { - const auto* first = fbs_field_address(first_soma_hand_rotation(self), offset); - return strided_field_view(self, first, SOMA_HAND_JOINT_STRIDE, SOMA_HAND_JOINT_COUNT, 0); - }, - "Per-joint validity as a writable (25,) uint8 view."); + py::class_(m, "SomaHandJointRotation", "One keyed SOMA hand rotation.") + .def(py::init(), py::arg("joint"), py::arg("rotation")) + .def_property_readonly("joint", &SomaHandJointRotation::joint) + .def_property_readonly("rotation", &SomaHandJointRotation::rotation, py::return_value_policy::reference_internal) + .def("__repr__", [](const SomaHandJointRotation& self) + { return "SomaHandJointRotation(joint=" + std::string(EnumNameSomaHandJoint(self.joint())) + ")"; }); serialized_class(m, "SomaHandJointRotations", "Encoded SOMA hand joint rotations.") .def(py::init( - [](const SomaHandJointRotationArray& joint_rotations, const Point& global_translation, + [](std::vector joint_rotations, const Point& global_translation, bool global_translation_is_valid, SomaHandedness handedness) { + std::sort(joint_rotations.begin(), joint_rotations.end(), + [](const auto& a, const auto& b) { return a.joint() < b.joint(); }); + const auto invalid = + std::find_if(joint_rotations.begin(), joint_rotations.end(), + [](const auto& entry) { return entry.joint() >= SomaHandJoint_NUM_JOINTS; }); + if (invalid != joint_rotations.end()) + { + throw py::value_error("joint_rotations: NUM_JOINTS is not a joint"); + } + const auto duplicate = + std::adjacent_find(joint_rotations.begin(), joint_rotations.end(), + [](const auto& a, const auto& b) { return a.joint() == b.joint(); }); + if (duplicate != joint_rotations.end()) + { + throw py::value_error("joint_rotations: duplicate SomaHandJoint " + + std::string(EnumNameSomaHandJoint(duplicate->joint()))); + } SomaHandJointRotationsT native; - native.joint_rotations = std::make_shared(joint_rotations); + native.joint_rotations = std::move(joint_rotations); native.global_translation = std::make_shared(global_translation); native.global_translation_is_valid = global_translation_is_valid; native.handedness = handedness; return pack(native); }), - py::arg("joint_rotations") = SomaHandJointRotationArray(), py::arg("global_translation") = Point(), + py::arg("joint_rotations") = std::vector{}, py::arg("global_translation") = Point(), py::arg("global_translation_is_valid") = false, py::arg("handedness") = SomaHandedness_UNSPECIFIED) - .def_property_readonly("joint_rotations", field(&SomaHandJointRotations::joint_rotations), - py::return_value_policy::reference_internal) + .def_property_readonly("joint_rotations", + [](const Serialized& self) + { + std::vector out; + const auto* entries = self->joint_rotations(); + if (entries != nullptr) + { + out.reserve(entries->size()); + for (const auto* entry : *entries) + { + out.push_back(*entry); + } + } + return out; + }) + .def( + "lookup", + [](const Serialized& self, SomaHandJoint joint) -> py::object + { + const auto* entries = self->joint_rotations(); + const auto* found = entries != nullptr ? entries->LookupByKey(joint) : nullptr; + return found != nullptr ? py::cast(*found) : py::none(); + }, + py::arg("joint"), "Return one provided joint rotation, or None when absent.") .def_property_readonly("global_translation", field(&SomaHandJointRotations::global_translation), py::return_value_policy::reference_internal) .def_property_readonly("global_translation_is_valid", field(&SomaHandJointRotations::global_translation_is_valid)) diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py index 5d5b7d364c..84dfdd0d4c 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py @@ -6,7 +6,6 @@ from enum import Enum from typing import TYPE_CHECKING, Any -import numpy as np from isaaccapture.deviceio_trackers import ( SomaBodyJointPosesTracker, @@ -22,7 +21,7 @@ from ..interface.tensor_group import TensorGroup from ..interface.tensor_group_type import OptionalType from ..tensor_types import SomaBodyInput, SomaBodyInputIndex -from ..utilities.soma_body_evaluator import _SomaBodyEvaluator +from ..utilities.soma_body_evaluator import _dense_joint_poses, _SomaBodyEvaluator if TYPE_CHECKING: from isaaccapture.deviceio import ITracker @@ -92,15 +91,8 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N if self.representation is SomaBodyRepresentation.JOINT_ROTATIONS: assert self._evaluator is not None positions, orientations, valid = self._evaluator.evaluate(data) - elif data.joint_poses is None: - positions = np.zeros((77, 3), dtype=np.float32) - orientations = np.zeros((77, 4), dtype=np.float32) - orientations[:, 3] = 1.0 - valid = np.zeros(77, dtype=np.uint8) else: - positions = np.asarray(data.joint_poses.positions, dtype=np.float32) - orientations = np.asarray(data.joint_poses.orientations, dtype=np.float32) - valid = np.asarray(data.joint_poses.is_valid, dtype=np.uint8) + positions, orientations, valid = _dense_joint_poses(data.joint_poses, 77) group[SomaBodyInputIndex.JOINT_POSITIONS] = positions group[SomaBodyInputIndex.JOINT_ORIENTATIONS] = orientations group[SomaBodyInputIndex.JOINT_VALID] = valid diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py index 0f325ab9cf..481fc439e3 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py @@ -6,7 +6,6 @@ from enum import Enum from typing import TYPE_CHECKING, Any -import numpy as np from isaaccapture.deviceio_trackers import ( SomaHandJointPosesTracker, @@ -24,6 +23,7 @@ from ..interface.tensor_group_type import OptionalType from ..tensor_types import SomaHandInput, SomaHandInputIndex from ..utilities.soma_hand_evaluator import _SomaHandEvaluator +from ..utilities.soma_body_evaluator import _dense_joint_poses if TYPE_CHECKING: from isaaccapture.deviceio import ITracker @@ -95,15 +95,8 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N if self.representation is SomaHandRepresentation.JOINT_ROTATIONS: assert self._evaluator is not None positions, orientations, valid = self._evaluator.evaluate(data) - elif data.joint_poses is None: - positions = np.zeros((25, 3), dtype=np.float32) - orientations = np.zeros((25, 4), dtype=np.float32) - orientations[:, 3] = 1.0 - valid = np.zeros(25, dtype=np.uint8) else: - positions = np.asarray(data.joint_poses.positions, dtype=np.float32) - orientations = np.asarray(data.joint_poses.orientations, dtype=np.float32) - valid = np.asarray(data.joint_poses.is_valid, dtype=np.uint8) + positions, orientations, valid = _dense_joint_poses(data.joint_poses, 25) group[SomaHandInputIndex.JOINT_POSITIONS] = positions group[SomaHandInputIndex.JOINT_ORIENTATIONS] = orientations group[SomaHandInputIndex.JOINT_VALID] = valid diff --git a/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py index edd07c2a32..0c7b1dd12e 100644 --- a/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py @@ -19,6 +19,53 @@ def _numpy(value: Any) -> np.ndarray: return np.asarray(value) +def _dense_joint_rotations( + entries: Any, joint_count: int +) -> tuple[np.ndarray, np.ndarray]: + rotations = np.zeros((joint_count, 4), dtype=np.float32) + rotations[:, 3] = 1.0 + provided = np.zeros(joint_count, dtype=bool) + previous = -1 + for entry in entries or (): + joint = int(entry.joint) + if joint < 0 or joint >= joint_count: + raise ValueError(f"SOMA joint index {joint} is out of range") + if joint <= previous: + raise ValueError("SOMA joint entries must be sorted and unique") + rotation = entry.rotation + rotations[joint] = (rotation.x, rotation.y, rotation.z, rotation.w) + provided[joint] = True + previous = joint + return rotations, provided + + +def _dense_joint_poses( + entries: Any, joint_count: int +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + positions = np.zeros((joint_count, 3), dtype=np.float32) + orientations = np.zeros((joint_count, 4), dtype=np.float32) + orientations[:, 3] = 1.0 + provided = np.zeros(joint_count, dtype=np.uint8) + previous = -1 + for entry in entries or (): + joint = int(entry.joint) + if joint < 0 or joint >= joint_count: + raise ValueError(f"SOMA joint index {joint} is out of range") + if joint <= previous: + raise ValueError("SOMA joint entries must be sorted and unique") + pose = entry.pose + positions[joint] = (pose.position.x, pose.position.y, pose.position.z) + orientations[joint] = ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) + provided[joint] = 1 + previous = joint + return positions, orientations, provided + + class _SomaBodyEvaluator: """Run upstream SOMA FK and expose all public transported joints.""" @@ -57,17 +104,7 @@ def _joint_validity( def _pose_inputs( data: Any, ) -> tuple[np.ndarray, np.ndarray, np.ndarray, bool]: - # Nested FlatBuffer structs are optional on the wire even though the - # Python constructor supplies defaults. Keep vendor omissions safe. - joint_rotations = data.joint_rotations - if joint_rotations is None: - control_valid = np.zeros(77, dtype=bool) - rotations = np.zeros((77, 4), dtype=np.float32) - rotations[:, 3] = 1.0 - else: - control_valid = np.asarray(joint_rotations.is_valid, dtype=bool) - rotations = np.asarray(joint_rotations.rotations, dtype=np.float32).copy() - rotations[~control_valid] = (0.0, 0.0, 0.0, 1.0) + rotations, control_valid = _dense_joint_rotations(data.joint_rotations, 77) translation = data.global_translation translation_valid = ( diff --git a/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py b/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py index 1231fe53a6..d8d1c1647e 100644 --- a/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py @@ -8,7 +8,7 @@ import numpy as np -from .soma_body_evaluator import _numpy +from .soma_body_evaluator import _dense_joint_rotations, _numpy class _SomaHandEvaluator: @@ -45,15 +45,7 @@ def _joint_validity( def _pose_inputs( data: Any, ) -> tuple[np.ndarray, np.ndarray, np.ndarray, bool]: - joint_rotations = data.joint_rotations - if joint_rotations is None: - control_valid = np.zeros(25, dtype=bool) - rotations = np.zeros((25, 4), dtype=np.float32) - rotations[:, 3] = 1.0 - else: - control_valid = np.asarray(joint_rotations.is_valid, dtype=bool) - rotations = np.asarray(joint_rotations.rotations, dtype=np.float32).copy() - rotations[~control_valid] = (0.0, 0.0, 0.0, 1.0) + rotations, control_valid = _dense_joint_rotations(data.joint_rotations, 25) translation = data.global_translation translation_valid = ( diff --git a/src/python/isaaccapture/schema/__init__.py b/src/python/isaaccapture/schema/__init__.py index d35bde981c..768ba08704 100644 --- a/src/python/isaaccapture/schema/__init__.py +++ b/src/python/isaaccapture/schema/__init__.py @@ -86,25 +86,22 @@ FullBodyPoseRecord, # Shared SOMA types. SomaHandedness, - SomaJointRotation, # SOMA body joint-rotation types. SomaBodyJoint, - SomaBodyJointRotationArray, + SomaBodyJointRotation, SomaBodyJointRotations, SomaBodyJointRotationsRecord, # SOMA evaluated body joint-pose types. SomaBodyJointPose, - SomaBodyJointPoseArray, SomaBodyJointPoses, SomaBodyJointPosesRecord, # SOMA hand joint-rotation types. SomaHandJoint, - SomaHandJointRotationArray, + SomaHandJointRotation, SomaHandJointRotations, SomaHandJointRotationsRecord, # SOMA evaluated hand joint-pose types. SomaHandJointPose, - SomaHandJointPoseArray, SomaHandJointPoses, SomaHandJointPosesRecord, ) @@ -214,25 +211,22 @@ def __getattr__(name: str): "FullBodyPoseRecord", # Shared SOMA types. "SomaHandedness", - "SomaJointRotation", # SOMA body joint-rotation types. "SomaBodyJoint", - "SomaBodyJointRotationArray", + "SomaBodyJointRotation", "SomaBodyJointRotations", "SomaBodyJointRotationsRecord", # SOMA evaluated body joint-pose types. "SomaBodyJointPose", - "SomaBodyJointPoseArray", "SomaBodyJointPoses", "SomaBodyJointPosesRecord", # SOMA hand joint-rotation types. "SomaHandJoint", - "SomaHandJointRotationArray", + "SomaHandJointRotation", "SomaHandJointRotations", "SomaHandJointRotationsRecord", # SOMA evaluated hand joint-pose types. "SomaHandJointPose", - "SomaHandJointPoseArray", "SomaHandJointPoses", "SomaHandJointPosesRecord", ] diff --git a/tests/cpp/core/schema/test_soma_body_joint_poses.cpp b/tests/cpp/core/schema/test_soma_body_joint_poses.cpp index c364e24a44..7ea55f69f2 100644 --- a/tests/cpp/core/schema/test_soma_body_joint_poses.cpp +++ b/tests/cpp/core/schema/test_soma_body_joint_poses.cpp @@ -6,8 +6,6 @@ #include #include -#include -#include #include namespace @@ -21,17 +19,14 @@ constexpr flatbuffers::voffset_t vt(flatbuffers::voffset_t field) static_assert(core::SomaBodyJointPoses::VT_JOINT_POSES == vt(0)); static_assert(core::SomaBodyJointPosesRecord::VT_DATA == vt(0)); static_assert(core::SomaBodyJointPosesRecord::VT_TIMESTAMP == vt(1)); -static_assert(std::is_trivially_copyable_v); -static_assert(sizeof(core::SomaBodyJointPoseArray) == 77 * sizeof(core::SomaBodyJointPose)); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaBodyJointPose) == 32); -TEST_CASE("SOMA evaluated body joint poses round trip", "[soma_body_joint_poses][flatbuffers]") +TEST_CASE("SOMA evaluated body joint poses round trip as a sparse keyed vector", "[soma_body_joint_poses][flatbuffers]") { core::SomaBodyJointPosesT pose; - pose.joint_poses = std::make_shared(); - pose.joint_poses->mutable_values()->Mutate( - core::SomaBodyJoint_HEAD, - core::SomaBodyJointPose( - core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)), true)); + pose.joint_poses.emplace_back( + core::SomaBodyJoint_HEAD, core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f))); flatbuffers::FlatBufferBuilder builder; builder.Finish(core::SomaBodyJointPoses::Pack(builder, &pose)); @@ -40,13 +35,16 @@ TEST_CASE("SOMA evaluated body joint poses round trip", "[soma_body_joint_poses] const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->joint_poses() != nullptr); - const auto* head = (*decoded->joint_poses()->values())[core::SomaBodyJoint_HEAD]; + REQUIRE(decoded->joint_poses()->size() == 1); + const auto* head = decoded->joint_poses()->LookupByKey(core::SomaBodyJoint_HEAD); + REQUIRE(head != nullptr); + CHECK(head->joint() == core::SomaBodyJoint_HEAD); CHECK(head->pose().position().x() == 1.0f); CHECK(head->pose().position().y() == 2.0f); CHECK(head->pose().position().z() == 3.0f); CHECK(head->pose().orientation().z() == 0.6f); CHECK(head->pose().orientation().w() == 0.8f); - CHECK(head->is_valid()); + CHECK(decoded->joint_poses()->LookupByKey(core::SomaBodyJoint_HIPS) == nullptr); } } // namespace diff --git a/tests/cpp/core/schema/test_soma_body_joint_rotations.cpp b/tests/cpp/core/schema/test_soma_body_joint_rotations.cpp index 0ca0d5b7d6..9bcac7971c 100644 --- a/tests/cpp/core/schema/test_soma_body_joint_rotations.cpp +++ b/tests/cpp/core/schema/test_soma_body_joint_rotations.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include @@ -20,13 +19,11 @@ static_assert(core::SomaBodyJointRotationsRecord::VT_DATA == VT(0)); static_assert(core::SomaBodyJointRotationsRecord::VT_TIMESTAMP == VT(1)); #define TYPE(field) decltype(std::declval().field()) -static_assert(std::is_same_v); +static_assert(std::is_same_v*>); static_assert(std::is_same_v); static_assert(std::is_same_v); - -static_assert(std::is_trivially_copyable_v); -static_assert(std::is_trivially_copyable_v); -static_assert(sizeof(core::SomaBodyJointRotationArray) == 77 * sizeof(core::SomaJointRotation)); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaBodyJointRotation) == 20); static_assert(core::SomaBodyJoint_HIPS == 0); static_assert(core::SomaBodyJoint_LEFT_SHOULDER == 11); @@ -35,28 +32,19 @@ static_assert(core::SomaBodyJoint_LEFT_LEG == 67); static_assert(core::SomaBodyJoint_RIGHT_TOE_END == 76); static_assert(core::SomaBodyJoint_NUM_JOINTS == 77); -TEST_CASE("SOMA body rotation array has the public layout", "[soma_body_joint_rotations][struct]") +TEST_CASE("SOMA body rotations have explicit joint identifiers", "[soma_body_joint_rotations][struct]") { - core::SomaBodyJointRotationArray rotations; - - REQUIRE(rotations.values()->size() == static_cast(core::SomaBodyJoint_NUM_JOINTS)); - CHECK((*rotations.values())[core::SomaBodyJoint_HIPS]->rotation().x() == 0.0f); - CHECK_FALSE((*rotations.values())[core::SomaBodyJoint_RIGHT_TOE_END]->is_valid()); - - const core::SomaJointRotation hips(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true); - rotations.mutable_values()->Mutate(core::SomaBodyJoint_HIPS, hips); + const core::SomaBodyJointRotation head(core::SomaBodyJoint_HEAD, core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)); - CHECK((*rotations.values())[core::SomaBodyJoint_HIPS]->rotation().z() == Catch::Approx(0.6f)); - CHECK((*rotations.values())[core::SomaBodyJoint_HIPS]->rotation().w() == Catch::Approx(0.8f)); - CHECK((*rotations.values())[core::SomaBodyJoint_HIPS]->is_valid()); + CHECK(head.joint() == core::SomaBodyJoint_HEAD); + CHECK(head.rotation().z() == Catch::Approx(0.6f)); + CHECK(head.rotation().w() == Catch::Approx(0.8f)); } -TEST_CASE("SOMA body joint rotations round trip through FlatBuffers", "[soma_body_joint_rotations][flatbuffers]") +TEST_CASE("SOMA body joint rotations round trip as a sparse keyed vector", "[soma_body_joint_rotations][flatbuffers]") { core::SomaBodyJointRotationsT pose; - pose.joint_rotations = std::make_shared(); - pose.joint_rotations->mutable_values()->Mutate( - core::SomaBodyJoint_HEAD, core::SomaJointRotation(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true)); + pose.joint_rotations.emplace_back(core::SomaBodyJoint_HEAD, core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)); pose.global_translation = std::make_shared(1.0f, 2.0f, 3.0f); pose.global_translation_is_valid = true; @@ -65,27 +53,27 @@ TEST_CASE("SOMA body joint rotations round trip through FlatBuffers", "[soma_bod const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->joint_rotations() != nullptr); + REQUIRE(decoded->joint_rotations()->size() == 1); REQUIRE(decoded->global_translation() != nullptr); - const auto* head = (*decoded->joint_rotations()->values())[core::SomaBodyJoint_HEAD]; + const auto* head = decoded->joint_rotations()->LookupByKey(core::SomaBodyJoint_HEAD); + REQUIRE(head != nullptr); + CHECK(head->joint() == core::SomaBodyJoint_HEAD); CHECK(head->rotation().z() == Catch::Approx(0.6f)); CHECK(head->rotation().w() == Catch::Approx(0.8f)); - CHECK(head->is_valid()); + CHECK(decoded->joint_rotations()->LookupByKey(core::SomaBodyJoint_HIPS) == nullptr); CHECK(decoded->global_translation()->x() == Catch::Approx(1.0f)); CHECK(decoded->global_translation_is_valid()); } -TEST_CASE("SOMA body record preserves quaternion components and validity", "[soma_body_joint_rotations][flatbuffers]") +TEST_CASE("SOMA body record preserves keyed quaternion components", "[soma_body_joint_rotations][flatbuffers]") { const float scale = 1.0f / std::sqrt(30.0f); core::SomaBodyJointRotationsRecordT record; record.data = std::make_shared(); - record.data->joint_rotations = std::make_shared(); - record.data->joint_rotations->mutable_values()->Mutate( - core::SomaBodyJoint_HIPS, - core::SomaJointRotation(core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale), true)); - record.data->joint_rotations->mutable_values()->Mutate( - core::SomaBodyJoint_RIGHT_TOE_END, - core::SomaJointRotation(core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale), true)); + record.data->joint_rotations.emplace_back( + core::SomaBodyJoint_HIPS, core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale)); + record.data->joint_rotations.emplace_back( + core::SomaBodyJoint_RIGHT_TOE_END, core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale)); record.data->global_translation = std::make_shared(1.0f, -2.0f, 3.0f); record.data->global_translation_is_valid = false; record.timestamp = std::make_shared(100, 200, 300); @@ -98,11 +86,12 @@ TEST_CASE("SOMA body record preserves quaternion components and validity", "[som const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->data() != nullptr); REQUIRE(decoded->timestamp() != nullptr); - REQUIRE(decoded->data()->joint_rotations() != nullptr); - REQUIRE(decoded->data()->global_translation() != nullptr); - const auto* values = decoded->data()->joint_rotations()->values(); - const auto* hips = (*values)[core::SomaBodyJoint_HIPS]; - const auto* toe = (*values)[core::SomaBodyJoint_RIGHT_TOE_END]; + const auto* values = decoded->data()->joint_rotations(); + REQUIRE(values != nullptr); + const auto* hips = values->LookupByKey(core::SomaBodyJoint_HIPS); + const auto* toe = values->LookupByKey(core::SomaBodyJoint_RIGHT_TOE_END); + REQUIRE(hips != nullptr); + REQUIRE(toe != nullptr); CHECK(hips->rotation().x() == Catch::Approx(scale)); CHECK(hips->rotation().y() == Catch::Approx(-2.0f * scale)); CHECK(hips->rotation().z() == Catch::Approx(3.0f * scale)); @@ -111,12 +100,7 @@ TEST_CASE("SOMA body record preserves quaternion components and validity", "[som CHECK(toe->rotation().y() == Catch::Approx(2.0f * scale)); CHECK(toe->rotation().z() == Catch::Approx(-3.0f * scale)); CHECK(toe->rotation().w() == Catch::Approx(4.0f * scale)); - CHECK(hips->is_valid()); - CHECK(toe->is_valid()); - CHECK_FALSE((*values)[core::SomaBodyJoint_HEAD]->is_valid()); - CHECK(decoded->data()->global_translation()->x() == Catch::Approx(1.0f)); - CHECK(decoded->data()->global_translation()->y() == Catch::Approx(-2.0f)); - CHECK(decoded->data()->global_translation()->z() == Catch::Approx(3.0f)); + CHECK(values->LookupByKey(core::SomaBodyJoint_HEAD) == nullptr); CHECK_FALSE(decoded->data()->global_translation_is_valid()); CHECK(decoded->timestamp()->available_time_local_common_clock() == 100); CHECK(decoded->timestamp()->sample_time_local_common_clock() == 200); diff --git a/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp b/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp index 3e05d8375a..deb1bbecfe 100644 --- a/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp +++ b/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include namespace { @@ -22,16 +22,14 @@ static_assert(core::SomaHandJointPoses::VT_JOINT_POSES == vt(0)); static_assert(core::SomaHandJointPoses::VT_HANDEDNESS == vt(1)); static_assert(core::SomaHandJointPosesRecord::VT_DATA == vt(0)); static_assert(core::SomaHandJointPosesRecord::VT_TIMESTAMP == vt(1)); -static_assert(sizeof(core::SomaHandJointPoseArray) == 25 * sizeof(core::SomaHandJointPose)); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaHandJointPose) == 32); -TEST_CASE("SOMA hand joint poses round trip through FlatBuffers", "[soma_hand_joint_poses][flatbuffers]") +TEST_CASE("SOMA hand joint poses round trip as a sparse keyed vector", "[soma_hand_joint_poses][flatbuffers]") { core::SomaHandJointPosesT pose; - pose.joint_poses = std::make_shared(); - pose.joint_poses->mutable_values()->Mutate( - core::SomaHandJoint_INDEX_END, - core::SomaHandJointPose( - core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)), true)); + pose.joint_poses.emplace_back(core::SomaHandJoint_INDEX_END, + core::Pose(core::Point(1.0f, 2.0f, 3.0f), core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f))); pose.handedness = core::SomaHandedness_LEFT; flatbuffers::FlatBufferBuilder builder; @@ -41,9 +39,12 @@ TEST_CASE("SOMA hand joint poses round trip through FlatBuffers", "[soma_hand_jo const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->joint_poses() != nullptr); - const auto* index_end = (*decoded->joint_poses()->values())[core::SomaHandJoint_INDEX_END]; + REQUIRE(decoded->joint_poses()->size() == 1); + const auto* index_end = decoded->joint_poses()->LookupByKey(core::SomaHandJoint_INDEX_END); + REQUIRE(index_end != nullptr); + CHECK(index_end->joint() == core::SomaHandJoint_INDEX_END); CHECK(index_end->pose().position().x() == Catch::Approx(1.0f)); CHECK(index_end->pose().orientation().w() == Catch::Approx(0.8f)); - CHECK(index_end->is_valid()); + CHECK(decoded->joint_poses()->LookupByKey(core::SomaHandJoint_WRIST) == nullptr); CHECK(decoded->handedness() == core::SomaHandedness_LEFT); } diff --git a/tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp b/tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp index f039c666ea..98dc24d110 100644 --- a/tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp +++ b/tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp @@ -8,7 +8,6 @@ #include #include -#include #include #include @@ -21,14 +20,12 @@ static_assert(core::SomaHandJointRotationsRecord::VT_DATA == VT(0)); static_assert(core::SomaHandJointRotationsRecord::VT_TIMESTAMP == VT(1)); #define TYPE(field) decltype(std::declval().field()) -static_assert(std::is_same_v); +static_assert(std::is_same_v*>); static_assert(std::is_same_v); static_assert(std::is_same_v); static_assert(std::is_same_v); - -static_assert(std::is_trivially_copyable_v); -static_assert(std::is_trivially_copyable_v); -static_assert(sizeof(core::SomaHandJointRotationArray) == 25 * sizeof(core::SomaJointRotation)); +static_assert(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaHandJointRotation) == 20); static_assert(core::SomaHandJoint_WRIST == 0); static_assert(core::SomaHandJoint_INDEX1 == 5); @@ -37,28 +34,19 @@ static_assert(core::SomaHandJoint_RING1 == 15); static_assert(core::SomaHandJoint_PINKY_END == 24); static_assert(core::SomaHandJoint_NUM_JOINTS == 25); -TEST_CASE("SOMA hand rotation array has the public layout", "[soma_hand_joint_rotations][struct]") +TEST_CASE("SOMA hand rotations have explicit joint identifiers", "[soma_hand_joint_rotations][struct]") { - core::SomaHandJointRotationArray rotations; - - REQUIRE(rotations.values()->size() == static_cast(core::SomaHandJoint_NUM_JOINTS)); - CHECK((*rotations.values())[core::SomaHandJoint_WRIST]->rotation().x() == 0.0f); - CHECK_FALSE((*rotations.values())[core::SomaHandJoint_PINKY_END]->is_valid()); - - const core::SomaJointRotation wrist(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true); - rotations.mutable_values()->Mutate(core::SomaHandJoint_WRIST, wrist); + const core::SomaHandJointRotation wrist(core::SomaHandJoint_WRIST, core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)); - CHECK((*rotations.values())[core::SomaHandJoint_WRIST]->rotation().z() == Catch::Approx(0.6f)); - CHECK((*rotations.values())[core::SomaHandJoint_WRIST]->rotation().w() == Catch::Approx(0.8f)); - CHECK((*rotations.values())[core::SomaHandJoint_WRIST]->is_valid()); + CHECK(wrist.joint() == core::SomaHandJoint_WRIST); + CHECK(wrist.rotation().z() == Catch::Approx(0.6f)); + CHECK(wrist.rotation().w() == Catch::Approx(0.8f)); } -TEST_CASE("SOMA hand joint rotations round trip through FlatBuffers", "[soma_hand_joint_rotations][flatbuffers]") +TEST_CASE("SOMA hand joint rotations round trip as a sparse keyed vector", "[soma_hand_joint_rotations][flatbuffers]") { core::SomaHandJointRotationsT pose; - pose.joint_rotations = std::make_shared(); - pose.joint_rotations->mutable_values()->Mutate( - core::SomaHandJoint_INDEX_END, core::SomaJointRotation(core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f), true)); + pose.joint_rotations.emplace_back(core::SomaHandJoint_INDEX_END, core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)); pose.global_translation = std::make_shared(1.0f, 2.0f, 3.0f); pose.global_translation_is_valid = true; pose.handedness = core::SomaHandedness_RIGHT; @@ -68,28 +56,28 @@ TEST_CASE("SOMA hand joint rotations round trip through FlatBuffers", "[soma_han const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->joint_rotations() != nullptr); + REQUIRE(decoded->joint_rotations()->size() == 1); REQUIRE(decoded->global_translation() != nullptr); - const auto* index_end = (*decoded->joint_rotations()->values())[core::SomaHandJoint_INDEX_END]; + const auto* index_end = decoded->joint_rotations()->LookupByKey(core::SomaHandJoint_INDEX_END); + REQUIRE(index_end != nullptr); + CHECK(index_end->joint() == core::SomaHandJoint_INDEX_END); CHECK(index_end->rotation().z() == Catch::Approx(0.6f)); CHECK(index_end->rotation().w() == Catch::Approx(0.8f)); - CHECK(index_end->is_valid()); + CHECK(decoded->joint_rotations()->LookupByKey(core::SomaHandJoint_WRIST) == nullptr); CHECK(decoded->global_translation()->x() == Catch::Approx(1.0f)); CHECK(decoded->global_translation_is_valid()); CHECK(decoded->handedness() == core::SomaHandedness_RIGHT); } -TEST_CASE("SOMA hand record preserves quaternion components and validity", "[soma_hand_joint_rotations][flatbuffers]") +TEST_CASE("SOMA hand record preserves keyed quaternion components", "[soma_hand_joint_rotations][flatbuffers]") { const float scale = 1.0f / std::sqrt(30.0f); core::SomaHandJointRotationsRecordT record; record.data = std::make_shared(); - record.data->joint_rotations = std::make_shared(); - record.data->joint_rotations->mutable_values()->Mutate( - core::SomaHandJoint_WRIST, - core::SomaJointRotation(core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale), true)); - record.data->joint_rotations->mutable_values()->Mutate( - core::SomaHandJoint_PINKY_END, - core::SomaJointRotation(core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale), true)); + record.data->joint_rotations.emplace_back( + core::SomaHandJoint_WRIST, core::Quaternion(scale, -2.0f * scale, 3.0f * scale, -4.0f * scale)); + record.data->joint_rotations.emplace_back( + core::SomaHandJoint_PINKY_END, core::Quaternion(-scale, 2.0f * scale, -3.0f * scale, 4.0f * scale)); record.data->global_translation = std::make_shared(1.0f, -2.0f, 3.0f); record.data->global_translation_is_valid = false; record.data->handedness = core::SomaHandedness_LEFT; @@ -103,11 +91,12 @@ TEST_CASE("SOMA hand record preserves quaternion components and validity", "[som const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); REQUIRE(decoded->data() != nullptr); REQUIRE(decoded->timestamp() != nullptr); - REQUIRE(decoded->data()->joint_rotations() != nullptr); - REQUIRE(decoded->data()->global_translation() != nullptr); - const auto* values = decoded->data()->joint_rotations()->values(); - const auto* wrist = (*values)[core::SomaHandJoint_WRIST]; - const auto* pinky = (*values)[core::SomaHandJoint_PINKY_END]; + const auto* values = decoded->data()->joint_rotations(); + REQUIRE(values != nullptr); + const auto* wrist = values->LookupByKey(core::SomaHandJoint_WRIST); + const auto* pinky = values->LookupByKey(core::SomaHandJoint_PINKY_END); + REQUIRE(wrist != nullptr); + REQUIRE(pinky != nullptr); CHECK(wrist->rotation().x() == Catch::Approx(scale)); CHECK(wrist->rotation().y() == Catch::Approx(-2.0f * scale)); CHECK(wrist->rotation().z() == Catch::Approx(3.0f * scale)); @@ -116,13 +105,7 @@ TEST_CASE("SOMA hand record preserves quaternion components and validity", "[som CHECK(pinky->rotation().y() == Catch::Approx(2.0f * scale)); CHECK(pinky->rotation().z() == Catch::Approx(-3.0f * scale)); CHECK(pinky->rotation().w() == Catch::Approx(4.0f * scale)); - CHECK(wrist->is_valid()); - CHECK(pinky->is_valid()); - CHECK_FALSE((*values)[core::SomaHandJoint_INDEX1]->is_valid()); - CHECK(decoded->data()->global_translation()->x() == Catch::Approx(1.0f)); - CHECK(decoded->data()->global_translation()->y() == Catch::Approx(-2.0f)); - CHECK(decoded->data()->global_translation()->z() == Catch::Approx(3.0f)); - CHECK_FALSE(decoded->data()->global_translation_is_valid()); + CHECK(values->LookupByKey(core::SomaHandJoint_INDEX1) == nullptr); CHECK(decoded->data()->handedness() == core::SomaHandedness_LEFT); CHECK(decoded->timestamp()->available_time_local_common_clock() == 100); CHECK(decoded->timestamp()->sample_time_local_common_clock() == 200); diff --git a/tests/python/core/retargeting_engine/test_soma_body_evaluator.py b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py index 2215df2185..dd97acfa53 100644 --- a/tests/python/core/retargeting_engine/test_soma_body_evaluator.py +++ b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py @@ -4,10 +4,13 @@ from unittest.mock import MagicMock import numpy as np +import pytest from isaaccapture.retargeting_engine.utilities.soma_body_evaluator import ( + _dense_joint_rotations, _SomaBodyEvaluator, ) +from isaaccapture.schema import Quaternion, SomaBodyJoint, SomaBodyJointRotation def evaluator_with_two_branches() -> _SomaBodyEvaluator: @@ -62,10 +65,10 @@ def test_evaluator_defaults_omitted_rotations_to_invalid_identity(): def test_evaluator_marks_omitted_translation_invalid(): rotations = np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)) data = MagicMock( - joint_rotations=MagicMock( - rotations=rotations, - is_valid=np.ones(77, dtype=np.uint8), - ), + joint_rotations=[ + SomaBodyJointRotation(SomaBodyJoint(index), Quaternion(*rotation)) + for index, rotation in enumerate(rotations) + ], global_translation=None, global_translation_is_valid=True, ) @@ -83,3 +86,13 @@ def test_evaluator_marks_omitted_translation_invalid(): ._joint_validity(control_valid, translation_valid) .any() ) + + +def test_dense_adapter_rejects_unsorted_or_duplicate_joint_identifiers(): + hips = SomaBodyJointRotation(SomaBodyJoint.HIPS, Quaternion(0.0, 0.0, 0.0, 1.0)) + head = SomaBodyJointRotation(SomaBodyJoint.HEAD, Quaternion(0.0, 0.0, 0.0, 1.0)) + + with pytest.raises(ValueError, match="sorted and unique"): + _dense_joint_rotations([head, hips], 77) + with pytest.raises(ValueError, match="sorted and unique"): + _dense_joint_rotations([head, head], 77) diff --git a/tests/python/core/retargeting_engine/test_soma_body_source.py b/tests/python/core/retargeting_engine/test_soma_body_source.py index 3b6dfb37a7..b393c98c47 100644 --- a/tests/python/core/retargeting_engine/test_soma_body_source.py +++ b/tests/python/core/retargeting_engine/test_soma_body_source.py @@ -24,18 +24,22 @@ BodyJoints, FullBodyPose, Point, - SomaBodyJointPoseArray, + Pose, + Quaternion, + SomaBodyJoint, + SomaBodyJointPose, SomaBodyJointPoses, - SomaBodyJointRotationArray, + SomaBodyJointRotation, SomaBodyJointRotations, ) from isaaccapture.teleop_session_manager.helpers import _get_trackers_from_pipeline def pose(): - joints = SomaBodyJointRotationArray() - joints.rotations[:] = (0, 0, 0, 1) - joints.is_valid[:] = 1 + joints = [ + SomaBodyJointRotation(SomaBodyJoint(index), Quaternion(0, 0, 0, 1)) + for index in range(77) + ] return SomaBodyJointRotations(joints, Point(1, 2, 3), True) @@ -51,10 +55,14 @@ def source(): def evaluated_pose(): - joints = SomaBodyJointPoseArray() - joints.positions[:] = np.arange(231, dtype=np.float32).reshape(77, 3) - joints.orientations[:] = np.tile([0, 0, 0, 1], (77, 1)) - joints.is_valid[:] = 1 + positions = np.arange(231, dtype=np.float32).reshape(77, 3) + joints = [ + SomaBodyJointPose( + SomaBodyJoint(index), + Pose(Point(*position), Quaternion(0, 0, 0, 1)), + ) + for index, position in enumerate(positions) + ] return SomaBodyJointPoses(joints) @@ -126,16 +134,17 @@ def test_joint_pose_source_maps_received_payload_without_fk(): evaluated = soma_source({"deviceio_soma_body": inputs})[SomaBodySource.BODY] - np.testing.assert_array_equal( - evaluated[SomaBodyInputIndex.JOINT_POSITIONS], raw.joint_poses.positions - ) - np.testing.assert_array_equal( - evaluated[SomaBodyInputIndex.JOINT_ORIENTATIONS], - raw.joint_poses.orientations, + positions = np.array( + [ + [entry.pose.position.x, entry.pose.position.y, entry.pose.position.z] + for entry in raw.joint_poses + ], + dtype=np.float32, ) np.testing.assert_array_equal( - evaluated[SomaBodyInputIndex.JOINT_VALID], raw.joint_poses.is_valid + evaluated[SomaBodyInputIndex.JOINT_POSITIONS], positions ) + assert evaluated[SomaBodyInputIndex.JOINT_VALID].all() def test_absent_sample_clears_reused_output(): diff --git a/tests/python/core/retargeting_engine/test_soma_hand_source.py b/tests/python/core/retargeting_engine/test_soma_hand_source.py index aae9d29603..b0d8f5a016 100644 --- a/tests/python/core/retargeting_engine/test_soma_hand_source.py +++ b/tests/python/core/retargeting_engine/test_soma_hand_source.py @@ -20,10 +20,13 @@ from isaaccapture.retargeting_engine.tensor_types import SomaHandInputIndex from isaaccapture.schema import ( Point, + Pose, + Quaternion, SomaHandedness, - SomaHandJointPoseArray, + SomaHandJoint, + SomaHandJointPose, SomaHandJointPoses, - SomaHandJointRotationArray, + SomaHandJointRotation, SomaHandJointRotations, ) @@ -36,17 +39,22 @@ def fake_layer(): def rotations(handedness=SomaHandedness.LEFT): - joints = SomaHandJointRotationArray() - joints.rotations[:] = (0, 0, 0, 1) - joints.is_valid[:] = 1 + joints = [ + SomaHandJointRotation(SomaHandJoint(index), Quaternion(0, 0, 0, 1)) + for index in range(25) + ] return SomaHandJointRotations(joints, Point(1, 2, 3), True, handedness) def poses(handedness=SomaHandedness.LEFT): - joints = SomaHandJointPoseArray() - joints.positions[:] = np.arange(75, dtype=np.float32).reshape(25, 3) - joints.orientations[:] = np.tile([0, 0, 0, 1], (25, 1)) - joints.is_valid[:] = 1 + positions = np.arange(75, dtype=np.float32).reshape(25, 3) + joints = [ + SomaHandJointPose( + SomaHandJoint(index), + Pose(Point(*position), Quaternion(0, 0, 0, 1)), + ) + for index, position in enumerate(positions) + ] return SomaHandJointPoses(joints, handedness) @@ -96,12 +104,17 @@ def test_joint_pose_source_maps_without_fk(): evaluated = source({"deviceio_soma_hand": inputs})[SomaHandSource.HAND] assert source._evaluator is None - np.testing.assert_array_equal( - evaluated[SomaHandInputIndex.JOINT_POSITIONS], raw.joint_poses.positions + expected_positions = np.array( + [ + [entry.pose.position.x, entry.pose.position.y, entry.pose.position.z] + for entry in raw.joint_poses + ], + dtype=np.float32, ) np.testing.assert_array_equal( - evaluated[SomaHandInputIndex.JOINT_VALID], raw.joint_poses.is_valid + evaluated[SomaHandInputIndex.JOINT_POSITIONS], expected_positions ) + assert evaluated[SomaHandInputIndex.JOINT_VALID].all() def test_payload_handedness_must_match_collection(): diff --git a/tests/python/core/schema/test_soma_body_joint_poses.py b/tests/python/core/schema/test_soma_body_joint_poses.py index 27cfd9e44a..0b42d7e39f 100644 --- a/tests/python/core/schema/test_soma_body_joint_poses.py +++ b/tests/python/core/schema/test_soma_body_joint_poses.py @@ -1,51 +1,60 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -import numpy as np import pytest from isaaccapture.schema import ( DeviceDataTimestamp, - SomaBodyJointPoseArray, + Point, + Pose, + Quaternion, + SomaBodyJoint, + SomaBodyJointPose, SomaBodyJointPoses, SomaBodyJointPosesRecord, - SomaBodyJoint, ) -def test_soma_body_joint_pose_views_alias_storage(): - joints = SomaBodyJointPoseArray() - positions = np.arange(231, dtype=np.float32).reshape(77, 3) - orientations = np.tile([0.0, 0.0, 0.0, 1.0], (77, 1)) - joints.positions[:] = positions - joints.orientations[:] = orientations - joints.is_valid[int(SomaBodyJoint.HEAD)] = 1 +def test_soma_body_joint_poses_are_keyed_and_sparse(): + payload = SomaBodyJointPoses( + [ + SomaBodyJointPose( + SomaBodyJoint.HEAD, + Pose(Point(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ) + ] + ) - np.testing.assert_array_equal(joints.positions, positions) - np.testing.assert_array_equal(joints.orientations, orientations) - head = joints.values(int(SomaBodyJoint.HEAD)) - assert head.pose.position.x == positions[int(SomaBodyJoint.HEAD), 0] - assert head.pose.orientation.w == 1.0 - assert head.is_valid + assert len(payload.joint_poses) == 1 + assert payload.joint_poses[0].joint == SomaBodyJoint.HEAD + assert payload.lookup(SomaBodyJoint.HEAD).pose.position.y == pytest.approx(2.0) + assert payload.lookup(SomaBodyJoint.HIPS) is None -def test_soma_body_joint_pose_index_check(): - with pytest.raises(IndexError): - SomaBodyJointPoseArray().values(77) +def test_soma_body_joint_poses_sort_and_reject_duplicates(): + hips = SomaBodyJointPose(SomaBodyJoint.HIPS, Pose()) + head = SomaBodyJointPose(SomaBodyJoint.HEAD, Pose()) + payload = SomaBodyJointPoses([head, hips]) + + assert [entry.joint for entry in payload.joint_poses] == [ + SomaBodyJoint.HIPS, + SomaBodyJoint.HEAD, + ] + with pytest.raises(ValueError, match="duplicate SomaBodyJoint HEAD"): + SomaBodyJointPoses([head, head]) def test_soma_body_joint_poses_record_round_trip(): - joints = SomaBodyJointPoseArray() - joints.positions[int(SomaBodyJoint.HIPS)] = [1.0, 2.0, 3.0] - joints.orientations[:, 3] = 1.0 - joints.is_valid[:] = 1 - pose = SomaBodyJointPoses(joints) - record = SomaBodyJointPosesRecord(pose, DeviceDataTimestamp(10, 20, 30)) - - np.testing.assert_array_equal(record.data.joint_poses.positions, joints.positions) - np.testing.assert_array_equal( - record.data.joint_poses.orientations, joints.orientations + payload = SomaBodyJointPoses( + [ + SomaBodyJointPose( + SomaBodyJoint.RIGHT_TOE_END, + Pose(Point(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ) + ] ) - np.testing.assert_array_equal(record.data.joint_poses.is_valid, joints.is_valid) - assert record.data.to_bytes() == pose.to_bytes() + record = SomaBodyJointPosesRecord(payload, DeviceDataTimestamp(10, 20, 30)) + + assert record.data.lookup(SomaBodyJoint.RIGHT_TOE_END).pose.position.z == 3.0 + assert record.data.to_bytes() == payload.to_bytes() assert record.timestamp.available_time_local_common_clock == 10 diff --git a/tests/python/core/schema/test_soma_body_joint_rotations.py b/tests/python/core/schema/test_soma_body_joint_rotations.py index ac36472f87..0536724b37 100644 --- a/tests/python/core/schema/test_soma_body_joint_rotations.py +++ b/tests/python/core/schema/test_soma_body_joint_rotations.py @@ -1,8 +1,6 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -import gc - import numpy as np import pytest @@ -11,205 +9,80 @@ Point, Quaternion, SomaBodyJoint, - SomaBodyJointRotationArray, + SomaBodyJointRotation, SomaBodyJointRotations, SomaBodyJointRotationsRecord, - SomaJointRotation, ) -def test_soma_body_joint_rotations_joint_order(): - expected = [ - "HIPS", - "SPINE1", - "SPINE2", - "CHEST", - "NECK1", - "NECK2", - "HEAD", - "HEAD_END", - "JAW", - "LEFT_EYE", - "RIGHT_EYE", - "LEFT_SHOULDER", - "LEFT_ARM", - "LEFT_FORE_ARM", - "LEFT_HAND", - "LEFT_HAND_THUMB1", - "LEFT_HAND_THUMB2", - "LEFT_HAND_THUMB3", - "LEFT_HAND_THUMB_END", - "LEFT_HAND_INDEX1", - "LEFT_HAND_INDEX2", - "LEFT_HAND_INDEX3", - "LEFT_HAND_INDEX4", - "LEFT_HAND_INDEX_END", - "LEFT_HAND_MIDDLE1", - "LEFT_HAND_MIDDLE2", - "LEFT_HAND_MIDDLE3", - "LEFT_HAND_MIDDLE4", - "LEFT_HAND_MIDDLE_END", - "LEFT_HAND_RING1", - "LEFT_HAND_RING2", - "LEFT_HAND_RING3", - "LEFT_HAND_RING4", - "LEFT_HAND_RING_END", - "LEFT_HAND_PINKY1", - "LEFT_HAND_PINKY2", - "LEFT_HAND_PINKY3", - "LEFT_HAND_PINKY4", - "LEFT_HAND_PINKY_END", - "RIGHT_SHOULDER", - "RIGHT_ARM", - "RIGHT_FORE_ARM", - "RIGHT_HAND", - "RIGHT_HAND_THUMB1", - "RIGHT_HAND_THUMB2", - "RIGHT_HAND_THUMB3", - "RIGHT_HAND_THUMB_END", - "RIGHT_HAND_INDEX1", - "RIGHT_HAND_INDEX2", - "RIGHT_HAND_INDEX3", - "RIGHT_HAND_INDEX4", - "RIGHT_HAND_INDEX_END", - "RIGHT_HAND_MIDDLE1", - "RIGHT_HAND_MIDDLE2", - "RIGHT_HAND_MIDDLE3", - "RIGHT_HAND_MIDDLE4", - "RIGHT_HAND_MIDDLE_END", - "RIGHT_HAND_RING1", - "RIGHT_HAND_RING2", - "RIGHT_HAND_RING3", - "RIGHT_HAND_RING4", - "RIGHT_HAND_RING_END", - "RIGHT_HAND_PINKY1", - "RIGHT_HAND_PINKY2", - "RIGHT_HAND_PINKY3", - "RIGHT_HAND_PINKY4", - "RIGHT_HAND_PINKY_END", - "LEFT_LEG", - "LEFT_SHIN", - "LEFT_FOOT", - "LEFT_TOE_BASE", - "LEFT_TOE_END", - "RIGHT_LEG", - "RIGHT_SHIN", - "RIGHT_FOOT", - "RIGHT_TOE_BASE", - "RIGHT_TOE_END", - ] - - assert len(expected) == int(SomaBodyJoint.NUM_JOINTS) == 77 - for index, name in enumerate(expected): - assert int(getattr(SomaBodyJoint, name)) == index - - -def test_soma_body_joint_rotations_joint_rotation(): - rotation = SomaJointRotation(Quaternion(0.0, 0.0, 0.6, 0.8), True) - - assert rotation.rotation.x == 0.0 - assert rotation.rotation.y == 0.0 - assert rotation.rotation.z == pytest.approx(0.6) - assert rotation.rotation.w == pytest.approx(0.8) - assert rotation.is_valid is True - - -def test_soma_body_joint_rotations_rotation_views_alias_storage(): - rotations = SomaBodyJointRotationArray() - - assert rotations.rotations.shape == (77, 4) - assert rotations.rotations.dtype == np.float32 - assert rotations.is_valid.shape == (77,) - assert rotations.is_valid.dtype == np.uint8 - assert not rotations.rotations.flags.owndata - - rotations.rotations[SomaBodyJoint.HEAD] = [0.0, 0.0, 0.6, 0.8] - rotations.is_valid[SomaBodyJoint.HEAD] = 1 - - head = rotations.values(int(SomaBodyJoint.HEAD)) - assert head.rotation.z == pytest.approx(0.6) - assert head.rotation.w == pytest.approx(0.8) - assert head.is_valid is True - +def test_soma_body_joint_order(): + assert int(SomaBodyJoint.HIPS) == 0 + assert int(SomaBodyJoint.LEFT_SHOULDER) == 11 + assert int(SomaBodyJoint.RIGHT_SHOULDER) == 39 + assert int(SomaBodyJoint.LEFT_LEG) == 67 + assert int(SomaBodyJoint.RIGHT_TOE_END) == 76 + assert int(SomaBodyJoint.NUM_JOINTS) == 77 -def test_soma_body_joint_rotations_rotation_index_check(): - with pytest.raises(IndexError): - SomaBodyJointRotationArray().values(77) +def test_soma_body_rotation_has_explicit_joint_identifier(): + entry = SomaBodyJointRotation(SomaBodyJoint.HEAD, Quaternion(0.0, 0.0, 0.6, 0.8)) -def test_soma_body_joint_rotations_pose_construction_and_lifetime(): - rotations = SomaBodyJointRotationArray() - rotations.rotations[:] = [0.0, 0.0, 0.0, 1.0] - rotations.rotations[SomaBodyJoint.RIGHT_TOE_END] = [0.0, 0.0, 0.6, 0.8] - rotations.is_valid[:] = 1 + assert entry.joint == SomaBodyJoint.HEAD + assert entry.rotation.z == pytest.approx(0.6) + assert entry.rotation.w == pytest.approx(0.8) + assert "HEAD" in repr(entry) - pose = SomaBodyJointRotations(rotations, Point(1.0, 2.0, 3.0), True) - quaternion_rotations = pose.joint_rotations.rotations - del pose - gc.collect() - assert quaternion_rotations[76, 3] == pytest.approx(0.8) - - -def test_soma_body_joint_rotations_pose_defaults_and_global_translation(): - pose = SomaBodyJointRotations() - - assert pose.joint_rotations is not None - assert pose.global_translation.x == 0.0 - assert pose.global_translation_is_valid is False - - translated = SomaBodyJointRotations( - global_translation=Point(1.0, 2.0, 3.0), - global_translation_is_valid=True, +def test_soma_body_rotations_sort_lookup_and_omit_unavailable_joints(): + payload = SomaBodyJointRotations( + [ + SomaBodyJointRotation( + SomaBodyJoint.RIGHT_TOE_END, Quaternion(0.0, 0.0, 0.6, 0.8) + ), + SomaBodyJointRotation(SomaBodyJoint.HIPS, Quaternion(0.0, 0.0, 0.0, 1.0)), + ], + Point(1.0, 2.0, 3.0), + True, ) - assert translated.global_translation.y == pytest.approx(2.0) - assert translated.global_translation_is_valid is True + assert [entry.joint for entry in payload.joint_rotations] == [ + SomaBodyJoint.HIPS, + SomaBodyJoint.RIGHT_TOE_END, + ] + assert payload.lookup(SomaBodyJoint.RIGHT_TOE_END).rotation.w == pytest.approx(0.8) + assert payload.lookup(SomaBodyJoint.HEAD) is None + assert payload.global_translation.y == pytest.approx(2.0) + assert payload.global_translation_is_valid -def test_soma_body_joint_rotations_pose_record(): - record = SomaBodyJointRotationsRecord( - SomaBodyJointRotations(), DeviceDataTimestamp(100, 200, 300) - ) - assert record.data is not None - assert record.timestamp.sample_time_local_common_clock == 200 +def test_soma_body_rotations_reject_duplicate_joint_identifiers(): + entry = SomaBodyJointRotation(SomaBodyJoint.HEAD, Quaternion(0.0, 0.0, 0.0, 1.0)) + with pytest.raises(ValueError, match="duplicate SomaBodyJoint HEAD"): + SomaBodyJointRotations([entry, entry]) @pytest.mark.parametrize("sign", [-1.0, 1.0]) -@pytest.mark.parametrize("translation_is_valid", [False, True]) -def test_soma_body_joint_rotations_record_preserves_quaternion_layout( - sign, translation_is_valid -): +def test_soma_body_rotation_record_preserves_quaternion_layout(sign): quaternion = np.array([1.0, -2.0, 3.0, -4.0], dtype=np.float32) quaternion *= sign / np.linalg.norm(quaternion) - rotations = SomaBodyJointRotationArray() - rotations.rotations[SomaBodyJoint.HIPS] = quaternion - rotations.rotations[SomaBodyJoint.RIGHT_TOE_END] = -quaternion - rotations.is_valid[SomaBodyJoint.HIPS] = 1 - rotations.is_valid[SomaBodyJoint.RIGHT_TOE_END] = 1 - expected_rotations = rotations.rotations.copy() - expected_validity = rotations.is_valid.copy() - pose = SomaBodyJointRotations( - rotations, Point(1.0, -2.0, 3.0), translation_is_valid + payload = SomaBodyJointRotations( + [SomaBodyJointRotation(SomaBodyJoint.HIPS, Quaternion(*quaternion))] + ) + record = SomaBodyJointRotationsRecord(payload, DeviceDataTimestamp(100, 200, 300)) + + hips = record.data.lookup(SomaBodyJoint.HIPS) + np.testing.assert_array_equal( + [hips.rotation.x, hips.rotation.y, hips.rotation.z, hips.rotation.w], + quaternion, ) - record = SomaBodyJointRotationsRecord(pose, DeviceDataTimestamp(100, 200, 300)) - - rotations.rotations[:] = 0.0 - rotations.is_valid[:] = 0 - pose.joint_rotations.rotations[:] = 0.0 - pose.joint_rotations.is_valid[:] = 0 - del pose, rotations - gc.collect() - - data = record.data - np.testing.assert_array_equal(data.joint_rotations.rotations, expected_rotations) - np.testing.assert_array_equal(data.joint_rotations.is_valid, expected_validity) - hips = data.joint_rotations.values(int(SomaBodyJoint.HIPS)).rotation - np.testing.assert_array_equal([hips.x, hips.y, hips.z, hips.w], quaternion) - assert data.global_translation.x == pytest.approx(1.0) - assert data.global_translation.y == pytest.approx(-2.0) - assert data.global_translation.z == pytest.approx(3.0) - assert data.global_translation_is_valid is translation_is_valid - assert record.timestamp.available_time_local_common_clock == 100 - assert record.timestamp.sample_time_local_common_clock == 200 assert record.timestamp.sample_time_raw_device_clock == 300 + assert record.data.to_bytes() == payload.to_bytes() + + +def test_soma_body_rotations_default_to_an_empty_snapshot(): + payload = SomaBodyJointRotations() + + assert payload.joint_rotations == [] + assert payload.lookup(SomaBodyJoint.HIPS) is None + assert payload.global_translation.x == 0.0 + assert not payload.global_translation_is_valid diff --git a/tests/python/core/schema/test_soma_hand_joint_poses.py b/tests/python/core/schema/test_soma_hand_joint_poses.py index 82eaddb23a..f4ab28a30b 100644 --- a/tests/python/core/schema/test_soma_hand_joint_poses.py +++ b/tests/python/core/schema/test_soma_hand_joint_poses.py @@ -1,54 +1,65 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -import gc - -import numpy as np import pytest from isaaccapture.schema import ( DeviceDataTimestamp, + Point, + Pose, + Quaternion, SomaHandedness, - SomaHandJointPoseArray, + SomaHandJoint, + SomaHandJointPose, SomaHandJointPoses, SomaHandJointPosesRecord, - SomaHandJoint, ) -def test_soma_hand_joint_pose_views_alias_storage(): - poses = SomaHandJointPoseArray() +def test_soma_hand_joint_poses_are_keyed_and_sparse(): + payload = SomaHandJointPoses( + [ + SomaHandJointPose( + SomaHandJoint.INDEX_END, + Pose(Point(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ) + ], + SomaHandedness.LEFT, + ) - assert poses.positions.shape == (25, 3) - assert poses.orientations.shape == (25, 4) - assert poses.is_valid.shape == (25,) - assert poses.positions.dtype == np.float32 - assert poses.orientations.dtype == np.float32 - assert poses.is_valid.dtype == np.uint8 + assert len(payload.joint_poses) == 1 + assert payload.lookup(SomaHandJoint.INDEX_END).pose.position.y == pytest.approx(2.0) + assert payload.lookup(SomaHandJoint.WRIST) is None + assert payload.handedness == SomaHandedness.LEFT - poses.positions[SomaHandJoint.INDEX_END] = [1.0, 2.0, 3.0] - poses.orientations[SomaHandJoint.INDEX_END] = [0.0, 0.0, 0.6, 0.8] - poses.is_valid[SomaHandJoint.INDEX_END] = 1 - index_end = poses.values(int(SomaHandJoint.INDEX_END)) - assert index_end.pose.position.y == pytest.approx(2.0) - assert index_end.pose.orientation.w == pytest.approx(0.8) - assert index_end.is_valid is True +def test_soma_hand_joint_poses_sort_and_reject_duplicates(): + wrist = SomaHandJointPose(SomaHandJoint.WRIST, Pose()) + pinky = SomaHandJointPose(SomaHandJoint.PINKY_END, Pose()) + payload = SomaHandJointPoses([pinky, wrist]) + assert [entry.joint for entry in payload.joint_poses] == [ + SomaHandJoint.WRIST, + SomaHandJoint.PINKY_END, + ] + with pytest.raises(ValueError, match="duplicate SomaHandJoint WRIST"): + SomaHandJointPoses([wrist, wrist]) -def test_soma_hand_joint_poses_record_lifetime(): - poses = SomaHandJointPoseArray() - poses.positions[:] = np.arange(75, dtype=np.float32).reshape(25, 3) - poses.orientations[:, 3] = 1.0 - poses.is_valid[:] = 1 - payload = SomaHandJointPoses(poses, SomaHandedness.RIGHT) - record = SomaHandJointPosesRecord(payload, DeviceDataTimestamp(100, 200, 300)) - poses.positions[:] = 0.0 - payload.joint_poses.positions[:] = 0.0 - del poses, payload - gc.collect() +def test_soma_hand_joint_poses_record_round_trip(): + payload = SomaHandJointPoses( + [ + SomaHandJointPose( + SomaHandJoint.PINKY_END, + Pose(Point(72.0, 73.0, 74.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ) + ], + SomaHandedness.RIGHT, + ) + record = SomaHandJointPosesRecord(payload, DeviceDataTimestamp(10, 20, 30)) + assert record.data.lookup(SomaHandJoint.PINKY_END).pose.position.z == pytest.approx( + 74.0 + ) assert record.data.handedness == SomaHandedness.RIGHT - assert record.data.joint_poses.positions[24, 2] == pytest.approx(74.0) - assert record.timestamp.sample_time_raw_device_clock == 300 + assert record.timestamp.available_time_local_common_clock == 10 diff --git a/tests/python/core/schema/test_soma_hand_joint_rotations.py b/tests/python/core/schema/test_soma_hand_joint_rotations.py index e3b6367545..e253028a9f 100644 --- a/tests/python/core/schema/test_soma_hand_joint_rotations.py +++ b/tests/python/core/schema/test_soma_hand_joint_rotations.py @@ -1,8 +1,6 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -import gc - import numpy as np import pytest @@ -11,163 +9,63 @@ Point, Quaternion, SomaHandedness, - SomaHandJointRotationArray, + SomaHandJoint, + SomaHandJointRotation, SomaHandJointRotations, SomaHandJointRotationsRecord, - SomaHandJoint, - SomaJointRotation, ) -def test_soma_hand_joint_rotations_joint_order(): - expected = [ - "WRIST", - "THUMB1", - "THUMB2", - "THUMB3", - "THUMB_END", - "INDEX1", - "INDEX2", - "INDEX3", - "INDEX4", - "INDEX_END", - "MIDDLE1", - "MIDDLE2", - "MIDDLE3", - "MIDDLE4", - "MIDDLE_END", - "RING1", - "RING2", - "RING3", - "RING4", - "RING_END", - "PINKY1", - "PINKY2", - "PINKY3", - "PINKY4", - "PINKY_END", - ] - - assert len(expected) == int(SomaHandJoint.NUM_JOINTS) == 25 - for index, name in enumerate(expected): - assert int(getattr(SomaHandJoint, name)) == index - - -def test_soma_hand_joint_rotations_joint_rotation(): - rotation = SomaJointRotation(Quaternion(0.0, 0.0, 0.6, 0.8), True) - - assert rotation.rotation.x == 0.0 - assert rotation.rotation.y == 0.0 - assert rotation.rotation.z == pytest.approx(0.6) - assert rotation.rotation.w == pytest.approx(0.8) - assert rotation.is_valid is True - - -def test_soma_hand_joint_rotations_rotation_views_alias_storage(): - rotations = SomaHandJointRotationArray() - - assert rotations.rotations.shape == (25, 4) - assert rotations.rotations.dtype == np.float32 - assert rotations.is_valid.shape == (25,) - assert rotations.is_valid.dtype == np.uint8 - assert not rotations.rotations.flags.owndata - - rotations.rotations[SomaHandJoint.INDEX_END] = [0.0, 0.0, 0.6, 0.8] - rotations.is_valid[SomaHandJoint.INDEX_END] = 1 - - index_end = rotations.values(int(SomaHandJoint.INDEX_END)) - assert index_end.rotation.z == pytest.approx(0.6) - assert index_end.rotation.w == pytest.approx(0.8) - assert index_end.is_valid is True - - -def test_soma_hand_joint_rotations_rotation_index_check(): - with pytest.raises(IndexError): - SomaHandJointRotationArray().values(25) - - -def test_soma_hand_joint_rotations_pose_construction_and_lifetime(): - rotations = SomaHandJointRotationArray() - rotations.rotations[:] = [0.0, 0.0, 0.0, 1.0] - rotations.rotations[SomaHandJoint.PINKY_END] = [0.0, 0.0, 0.6, 0.8] - rotations.is_valid[:] = 1 - - pose = SomaHandJointRotations( - rotations, Point(1.0, 2.0, 3.0), True, SomaHandedness.RIGHT +def test_soma_hand_joint_order(): + assert int(SomaHandJoint.WRIST) == 0 + assert int(SomaHandJoint.INDEX1) == 5 + assert int(SomaHandJoint.MIDDLE1) == 10 + assert int(SomaHandJoint.RING1) == 15 + assert int(SomaHandJoint.PINKY_END) == 24 + assert int(SomaHandJoint.NUM_JOINTS) == 25 + + +def test_soma_hand_rotations_sort_lookup_and_omit_unavailable_joints(): + payload = SomaHandJointRotations( + [ + SomaHandJointRotation( + SomaHandJoint.PINKY_END, Quaternion(0.0, 0.0, 0.6, 0.8) + ), + SomaHandJointRotation(SomaHandJoint.WRIST, Quaternion(0.0, 0.0, 0.0, 1.0)), + ], + Point(1.0, 2.0, 3.0), + True, + SomaHandedness.LEFT, ) - quaternion_rotations = pose.joint_rotations.rotations - del pose - gc.collect() - assert quaternion_rotations[24, 3] == pytest.approx(0.8) + assert [entry.joint for entry in payload.joint_rotations] == [ + SomaHandJoint.WRIST, + SomaHandJoint.PINKY_END, + ] + assert payload.lookup(SomaHandJoint.PINKY_END).rotation.w == pytest.approx(0.8) + assert payload.lookup(SomaHandJoint.INDEX1) is None + assert payload.handedness == SomaHandedness.LEFT -def test_soma_hand_joint_rotations_pose_defaults_and_global_translation(): - pose = SomaHandJointRotations() +def test_soma_hand_rotations_reject_duplicate_joint_identifiers(): + entry = SomaHandJointRotation(SomaHandJoint.WRIST, Quaternion(0.0, 0.0, 0.0, 1.0)) + with pytest.raises(ValueError, match="duplicate SomaHandJoint WRIST"): + SomaHandJointRotations([entry, entry]) - assert pose.joint_rotations is not None - assert pose.global_translation.x == 0.0 - assert pose.global_translation_is_valid is False - assert pose.handedness == SomaHandedness.UNSPECIFIED - right = SomaHandJointRotations( - global_translation=Point(1.0, 2.0, 3.0), - global_translation_is_valid=True, +def test_soma_hand_rotation_record_preserves_quaternion_layout(): + quaternion = np.array([1.0, -2.0, 3.0, -4.0], dtype=np.float32) + quaternion /= np.linalg.norm(quaternion) + payload = SomaHandJointRotations( + [SomaHandJointRotation(SomaHandJoint.WRIST, Quaternion(*quaternion))], handedness=SomaHandedness.RIGHT, ) - assert right.global_translation.y == pytest.approx(2.0) - assert right.global_translation_is_valid is True - assert right.handedness == SomaHandedness.RIGHT - - -def test_soma_hand_joint_rotations_pose_record(): - record = SomaHandJointRotationsRecord( - SomaHandJointRotations(handedness=SomaHandedness.LEFT), - DeviceDataTimestamp(100, 200, 300), - ) - - assert record.data is not None - assert record.data.handedness == SomaHandedness.LEFT - assert record.timestamp.sample_time_local_common_clock == 200 - + record = SomaHandJointRotationsRecord(payload, DeviceDataTimestamp(100, 200, 300)) -@pytest.mark.parametrize("sign", [-1.0, 1.0]) -@pytest.mark.parametrize("translation_is_valid", [False, True]) -@pytest.mark.parametrize("handedness", [SomaHandedness.LEFT, SomaHandedness.RIGHT]) -def test_soma_hand_joint_rotations_record_preserves_quaternion_layout( - sign, translation_is_valid, handedness -): - quaternion = np.array([1.0, -2.0, 3.0, -4.0], dtype=np.float32) - quaternion *= sign / np.linalg.norm(quaternion) - rotations = SomaHandJointRotationArray() - rotations.rotations[SomaHandJoint.WRIST] = quaternion - rotations.rotations[SomaHandJoint.PINKY_END] = -quaternion - rotations.is_valid[SomaHandJoint.WRIST] = 1 - rotations.is_valid[SomaHandJoint.PINKY_END] = 1 - expected_rotations = rotations.rotations.copy() - expected_validity = rotations.is_valid.copy() - pose = SomaHandJointRotations( - rotations, Point(1.0, -2.0, 3.0), translation_is_valid, handedness + wrist = record.data.lookup(SomaHandJoint.WRIST) + np.testing.assert_array_equal( + [wrist.rotation.x, wrist.rotation.y, wrist.rotation.z, wrist.rotation.w], + quaternion, ) - record = SomaHandJointRotationsRecord(pose, DeviceDataTimestamp(100, 200, 300)) - - rotations.rotations[:] = 0.0 - rotations.is_valid[:] = 0 - pose.joint_rotations.rotations[:] = 0.0 - pose.joint_rotations.is_valid[:] = 0 - del pose, rotations - gc.collect() - - data = record.data - np.testing.assert_array_equal(data.joint_rotations.rotations, expected_rotations) - np.testing.assert_array_equal(data.joint_rotations.is_valid, expected_validity) - wrist = data.joint_rotations.values(int(SomaHandJoint.WRIST)).rotation - np.testing.assert_array_equal([wrist.x, wrist.y, wrist.z, wrist.w], quaternion) - assert data.global_translation.x == pytest.approx(1.0) - assert data.global_translation.y == pytest.approx(-2.0) - assert data.global_translation.z == pytest.approx(3.0) - assert data.global_translation_is_valid is translation_is_valid - assert data.handedness == handedness - assert record.timestamp.available_time_local_common_clock == 100 + assert record.data.handedness == SomaHandedness.RIGHT assert record.timestamp.sample_time_local_common_clock == 200 - assert record.timestamp.sample_time_raw_device_clock == 300 diff --git a/tests/python/examples/deviceio_live_view/test_soma_viewer.py b/tests/python/examples/deviceio_live_view/test_soma_viewer.py index b492fb6548..de488a168c 100644 --- a/tests/python/examples/deviceio_live_view/test_soma_viewer.py +++ b/tests/python/examples/deviceio_live_view/test_soma_viewer.py @@ -29,14 +29,18 @@ ) from isaaccapture.schema import ( Point, - SomaBodyJointPoseArray, + Pose, + Quaternion, + SomaBodyJoint, + SomaBodyJointPose, SomaBodyJointPoses, - SomaBodyJointRotationArray, + SomaBodyJointRotation, SomaBodyJointRotations, SomaHandedness, - SomaHandJointPoseArray, + SomaHandJoint, + SomaHandJointPose, SomaHandJointPoses, - SomaHandJointRotationArray, + SomaHandJointRotation, SomaHandJointRotations, ) from isaaccapture_examples.deviceio_live_view import live_deviceio, soma_body @@ -73,33 +77,46 @@ def fake_layer(): return layer -def soma_joint_rotations(quaternions, translation): - joints = SomaBodyJointRotationArray() - joints.rotations[:] = quaternions - joints.is_valid[:] = 1 +def soma_joint_rotations(quaternions, translation, omitted=()): + joints = [ + SomaBodyJointRotation(SomaBodyJoint(index), Quaternion(*quaternion)) + for index, quaternion in enumerate(quaternions) + if index not in omitted + ] return SomaBodyJointRotations(joints, Point(*translation), True) def soma_joint_poses(positions, orientations): - joints = SomaBodyJointPoseArray() - joints.positions[:] = positions - joints.orientations[:] = orientations - joints.is_valid[:] = 1 + joints = [ + SomaBodyJointPose( + SomaBodyJoint(index), + Pose(Point(*position), Quaternion(*orientation)), + ) + for index, (position, orientation) in enumerate( + zip(positions, orientations, strict=True) + ) + ] return SomaBodyJointPoses(joints) def soma_hand_joint_poses(positions, orientations, handedness): - joints = SomaHandJointPoseArray() - joints.positions[:] = positions - joints.orientations[:] = orientations - joints.is_valid[:] = 1 + joints = [ + SomaHandJointPose( + SomaHandJoint(index), + Pose(Point(*position), Quaternion(*orientation)), + ) + for index, (position, orientation) in enumerate( + zip(positions, orientations, strict=True) + ) + ] return SomaHandJointPoses(joints, handedness) def soma_hand_joint_rotations(rotations, translation, handedness): - joints = SomaHandJointRotationArray() - joints.rotations[:] = rotations - joints.is_valid[:] = 1 + joints = [ + SomaHandJointRotation(SomaHandJoint(index), Quaternion(*rotation)) + for index, rotation in enumerate(rotations) + ] return SomaHandJointRotations(joints, Point(*translation), True, handedness) @@ -418,8 +435,9 @@ def test_native_soma_pose_reaches_renderer(monkeypatch, soma_assets): inputs[source.name] = source.poll_tracker(object()) assert not viz.update(pipeline.execute_pipeline(inputs))["body_active"] - invalid = soma_joint_rotations(np.tile([0, 0, 0, 1], (77, 1)), [0, 0, 0]) - invalid.joint_rotations.is_valid[10] = 0 + invalid = soma_joint_rotations( + np.tile([0, 0, 0, 1], (77, 1)), [0, 0, 0], omitted={10} + ) source._tracker.get_data.return_value = invalid inputs[source.name] = source.poll_tracker(object()) active = viz.update(pipeline.execute_pipeline(inputs)) diff --git a/tests/python/examples/soma_body_publisher/test_publisher.py b/tests/python/examples/soma_body_publisher/test_publisher.py index 5a8f26dc74..ff5d900f22 100644 --- a/tests/python/examples/soma_body_publisher/test_publisher.py +++ b/tests/python/examples/soma_body_publisher/test_publisher.py @@ -9,6 +9,7 @@ import pytest from isaaccapture.retargeting_engine.utilities.soma_body_evaluator import ( + _dense_joint_poses, _SomaBodyEvaluator, ) from isaaccapture.schema import ( @@ -150,13 +151,13 @@ def test_demo_to_fbs_to_native_soma_matches_upstream(soma_assets, request): evaluated_payload = soma_joint_poses( evaluated_positions[0], evaluated_orientations[0] ) - np.testing.assert_allclose( - evaluated_payload.joint_poses.positions, positions, atol=1e-4 + sparse_positions, sparse_orientations, sparse_valid = _dense_joint_poses( + evaluated_payload.joint_poses, 77 ) + np.testing.assert_allclose(sparse_positions, positions, atol=1e-4) + assert sparse_valid.all() torch.testing.assert_close( - quaternion_xyzw_to_matrix( - torch.from_numpy(evaluated_payload.joint_poses.orientations.copy()) - ), + quaternion_xyzw_to_matrix(torch.from_numpy(sparse_orientations)), actual_rotations, atol=1e-4, rtol=1e-4, @@ -168,7 +169,7 @@ def test_demo_to_fbs_to_native_soma_matches_upstream(soma_assets, request): executable = request.config.getoption("--soma-pusher") if executable is not None: packets = [] - for frame, (rotations, translation) in enumerate(zip(q, t)): + for frame, (rotations, translation) in enumerate(zip(q, t, strict=True)): encoded = soma_joint_rotations(rotations, translation).to_bytes() packets.append(struct.pack(" Date: Wed, 7 Oct 2026 14:07:30 -0400 Subject: [PATCH 08/12] fix(soma): validate model and pose inputs Validate SOMA model semantics and resolve the supported reference before FK. Normalize usable quaternions and invalidate degenerate or non-finite body and hand measurements across rotation and evaluated-pose inputs. Add focused contract and numerical tests and document the resulting behavior. Signed-off-by: Michael Haselton --- docs/source/device/soma.rst | 9 + .../deviceio_source_nodes/soma_hand_source.py | 5 +- .../utilities/soma_body_evaluator.py | 76 +++++--- .../utilities/soma_contract.py | 145 +++++++++++++++ .../utilities/soma_hand_evaluator.py | 26 +-- .../test_soma_body_evaluator.py | 144 ++++++++++++++- .../test_soma_body_source.py | 47 ++++- .../test_soma_hand_source.py | 174 +++++++++++++++++- .../deviceio_live_view/test_soma_viewer.py | 13 +- .../soma_hand_publisher/test_publisher.py | 2 +- 10 files changed, 581 insertions(+), 60 deletions(-) create mode 100644 src/python/isaaccapture/retargeting_engine/utilities/soma_contract.py diff --git a/docs/source/device/soma.rst b/docs/source/device/soma.rst index ddf88579db..2051715f4c 100644 --- a/docs/source/device/soma.rst +++ b/docs/source/device/soma.rst @@ -66,6 +66,15 @@ The publisher must convert its native skeleton and reference frame before serialization. See :ref:`tracker-reference` for the complete field and validity contracts. +Consumers normalize finite quaternions whose norm is greater than ``1e-12``. A +zero, degenerate, or non-finite quaternion, position, or global translation is +unavailable. Quaternion signs are not canonicalized, so ``q`` and ``-q`` remain +equivalent. + +Rotation consumers resolve the SOMA v0.3.1 reference through SOMA-X and pass it +explicitly during forward kinematics. The prepared identity and subject scale +remain caller-selected calibration and are not constrained by this check. + Body and hand joint values are sorted, unique keyed vectors. Every entry carries an explicit ``SomaBodyJoint`` or ``SomaHandJoint`` identifier. A missing joint is unavailable for that frame; producers do not serialize placeholder values for diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py index 481fc439e3..b318d3479b 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py @@ -63,7 +63,10 @@ def __init__( else: self._tracker = SomaHandJointPosesTracker(collection_id) self._input_type = DeviceIOSomaHandJointPosesTracked() - self._evaluator = _SomaHandEvaluator(layer) if layer is not None else None + expected_hand_type = "left" if handedness is SomaHandedness.LEFT else "right" + self._evaluator = ( + _SomaHandEvaluator(layer, expected_hand_type) if layer is not None else None + ) super().__init__(name) def get_tracker(self) -> "ITracker": diff --git a/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py index 0c7b1dd12e..87e3bf5220 100644 --- a/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py @@ -8,6 +8,11 @@ import numpy as np +from .soma_contract import resolve_soma_body_contract + + +_QUATERNION_EPS = 1e-12 + def _numpy(value: Any) -> np.ndarray: if hasattr(value, "detach"): @@ -19,6 +24,16 @@ def _numpy(value: Any) -> np.ndarray: return np.asarray(value) +def _normalize_quaternion(value: Any) -> tuple[np.ndarray, bool]: + quaternion = np.asarray(value, dtype=np.float32) + if quaternion.shape != (4,) or not np.isfinite(quaternion).all(): + return np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), False + norm = float(np.linalg.norm(quaternion.astype(np.float64))) + if norm <= _QUATERNION_EPS: + return np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), False + return quaternion / np.float32(norm), True + + def _dense_joint_rotations( entries: Any, joint_count: int ) -> tuple[np.ndarray, np.ndarray]: @@ -33,8 +48,9 @@ def _dense_joint_rotations( if joint <= previous: raise ValueError("SOMA joint entries must be sorted and unique") rotation = entry.rotation - rotations[joint] = (rotation.x, rotation.y, rotation.z, rotation.w) - provided[joint] = True + rotations[joint], provided[joint] = _normalize_quaternion( + (rotation.x, rotation.y, rotation.z, rotation.w) + ) previous = joint return rotations, provided @@ -54,35 +70,47 @@ def _dense_joint_poses( if joint <= previous: raise ValueError("SOMA joint entries must be sorted and unique") pose = entry.pose - positions[joint] = (pose.position.x, pose.position.y, pose.position.z) - orientations[joint] = ( - pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w, + position = np.asarray( + (pose.position.x, pose.position.y, pose.position.z), dtype=np.float32 ) - provided[joint] = 1 + orientation, orientation_valid = _normalize_quaternion( + ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) + ) + if np.isfinite(position).all() and orientation_valid: + positions[joint] = position + orientations[joint] = orientation + provided[joint] = 1 previous = joint return positions, orientations, provided +def _translation_input(data: Any) -> tuple[np.ndarray, bool]: + translation = data.global_translation + valid = bool(data.global_translation_is_valid) and translation is not None + value = np.asarray( + (translation.x, translation.y, translation.z) if valid else (0.0, 0.0, 0.0), + dtype=np.float32, + ) + if not np.isfinite(value).all(): + return np.zeros(3, dtype=np.float32), False + return value, valid + + class _SomaBodyEvaluator: """Run upstream SOMA FK and expose all public transported joints.""" def __init__(self, layer: Any) -> None: + reference_pose = resolve_soma_body_contract(layer) joint_names = tuple(str(name) for name in layer.public_joint_names) parent_ids = _numpy(layer.output_joint_parent_ids).astype(np.int64) - if len(joint_names) != 78 or joint_names[0].upper() != "ROOT": - raise ValueError("SOMA evaluator requires Root plus 77 public joints") - if parent_ids.shape != (78,): - raise ValueError("SOMA evaluator requires 78 public parent IDs") - if any( - parent < 0 or parent >= child - for child, parent in enumerate(parent_ids[1:], 1) - ): - raise ValueError("SOMA public joints must follow parent-before-child order") self.layer = layer + self.reference_pose = reference_pose self.joint_names = joint_names[1:] self.parent_ids = tuple(int(parent) for parent in parent_ids) self.bones = tuple( @@ -106,16 +134,7 @@ def _pose_inputs( ) -> tuple[np.ndarray, np.ndarray, np.ndarray, bool]: rotations, control_valid = _dense_joint_rotations(data.joint_rotations, 77) - translation = data.global_translation - translation_valid = ( - bool(data.global_translation_is_valid) and translation is not None - ) - translation_xyz = np.asarray( - [translation.x, translation.y, translation.z] - if translation_valid - else [0.0, 0.0, 0.0], - dtype=np.float32, - ) + translation_xyz, translation_valid = _translation_input(data) return rotations, control_valid, translation_xyz, translation_valid def evaluate(self, data: Any) -> tuple[np.ndarray, np.ndarray, np.ndarray]: @@ -135,6 +154,7 @@ def evaluate(self, data: Any) -> tuple[np.ndarray, np.ndarray, np.ndarray]: pose2rot=False, fk_only=True, apply_correctives=False, + reference_pose=self.reference_pose, ) orientations = transforms.matrix_to_quaternion_xyzw( output["transforms"][0, 1:, :3, :3] diff --git a/src/python/isaaccapture/retargeting_engine/utilities/soma_contract.py b/src/python/isaaccapture/retargeting_engine/utilities/soma_contract.py new file mode 100644 index 0000000000..d4f5aefb8a --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_contract.py @@ -0,0 +1,145 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""SOMA model checks shared by body and hand evaluators.""" + +from typing import Any + +import numpy as np + +from isaaccapture.schema import SomaBodyJoint, SomaHandJoint + + +SOMA_REFERENCE_VERSION = "v0.3.1" + +_BODY_JOINT_NAMES = tuple( + name + for name, joint in sorted( + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" +) +_HAND_JOINT_NAMES = tuple( + name + for name, joint in sorted( + SomaHandJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" +) + +# Parent indices in the transport's 25-joint hand order. Wrist is self-parented +# in SOMA-X; each finger chain starts at Wrist. +_HAND_PARENT_IDS = ( + 0, + 0, + 1, + 2, + 3, + 0, + 5, + 6, + 7, + 8, + 0, + 10, + 11, + 12, + 13, + 0, + 15, + 16, + 17, + 18, + 0, + 20, + 21, + 22, + 23, +) + + +def _normalized(name: Any) -> str: + return str(name).replace("_", "").upper() + + +def _numpy(value: Any) -> np.ndarray: + if hasattr(value, "detach"): + value = value.detach() + if hasattr(value, "cpu"): + value = value.cpu() + if hasattr(value, "numpy"): + value = value.numpy() + return np.asarray(value) + + +def _validate_meters(layer: Any, label: str) -> None: + output_unit = getattr(layer, "output_unit", None) + if getattr(output_unit, "meters_per_unit", None) != 1.0: + raise ValueError(f"{label} must use meters") + + +def _resolve_reference(layer: Any, joint_count: int, label: str) -> Any: + resolver = getattr(layer, "get_reference_pose", None) + if not callable(resolver): + raise ValueError(f"{label} does not expose reference-pose selection") + try: + reference = resolver(version=SOMA_REFERENCE_VERSION) + except (AttributeError, KeyError, TypeError, ValueError) as error: + raise ValueError( + f"{label} is incompatible with the SOMA {SOMA_REFERENCE_VERSION} reference" + ) from error + if tuple(reference.shape) not in ((joint_count, 3, 3), (joint_count, 4, 4)): + raise ValueError(f"{label} returned an incompatible reference pose") + return reference + + +def resolve_soma_body_contract(layer: Any) -> Any: + """Validate body semantics and return the explicit transport reference.""" + + actual_names = tuple(_normalized(name) for name in layer.public_joint_names) + expected_names = ("ROOT", *(_normalized(name) for name in _BODY_JOINT_NAMES)) + if actual_names != expected_names: + raise ValueError("SOMA body joint order differs from the transport contract") + + parent_ids = _numpy(layer.output_joint_parent_ids).astype(np.int64) + if parent_ids.shape != (78,) or any( + parent < 0 or parent >= child for child, parent in enumerate(parent_ids[1:], 1) + ): + raise ValueError("SOMA body hierarchy is incompatible with the transport") + + _validate_meters(layer, "SOMA body layer") + # The public resolver also rejects an incompatible public name or hierarchy + # mapping before returning the selected reference. + return _resolve_reference(layer, 78, "SOMA body layer") + + +def resolve_soma_hand_contract(layer: Any, expected_hand_type: str) -> Any: + """Validate hand semantics and return the explicit transport reference.""" + + hand_type = getattr(layer, "hand_type", None) + if hand_type != expected_hand_type: + raise ValueError( + f"SOMA hand layer side {hand_type!r} does not match {expected_hand_type!r}" + ) + + prefix = _normalized(f"{hand_type}Hand") + actual_names = [] + for index, name in enumerate(layer.rig_data["joint_names"]): + normalized = _normalized(name) + if normalized.startswith(prefix): + normalized = normalized[len(prefix) :] + actual_names.append("WRIST" if index == 0 and not normalized else normalized) + if tuple(actual_names) != tuple(_normalized(name) for name in _HAND_JOINT_NAMES): + raise ValueError("SOMA hand joint order differs from the transport contract") + + raw_parent_ids = _numpy(layer.joint_parent_ids) + parent_ids = tuple(int(parent) for parent in raw_parent_ids) + if ( + raw_parent_ids.shape != (25,) + or not np.issubdtype(raw_parent_ids.dtype, np.integer) + or parent_ids != _HAND_PARENT_IDS + ): + raise ValueError("SOMA hand hierarchy differs from the transport contract") + + _validate_meters(layer, "SOMA hand layer") + return _resolve_reference(layer, 25, "SOMA hand layer") diff --git a/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py b/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py index d8d1c1647e..e9189e43e0 100644 --- a/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py @@ -8,24 +8,20 @@ import numpy as np -from .soma_body_evaluator import _dense_joint_rotations, _numpy +from .soma_body_evaluator import _dense_joint_rotations, _numpy, _translation_input +from .soma_contract import resolve_soma_hand_contract class _SomaHandEvaluator: """Run upstream SOMA hand FK and expose its 25 evaluated joints.""" - def __init__(self, layer: Any) -> None: + def __init__(self, layer: Any, expected_hand_type: str) -> None: + reference_pose = resolve_soma_hand_contract(layer, expected_hand_type) joint_names = tuple(str(name) for name in layer.rig_data["joint_names"]) parent_ids = _numpy(layer.joint_parent_ids).astype(np.int64) - if len(joint_names) != 25 or parent_ids.shape != (25,): - raise ValueError("SOMA hand evaluator requires 25 joints") - if parent_ids[0] != 0 or any( - parent < 0 or parent >= child - for child, parent in enumerate(parent_ids[1:], 1) - ): - raise ValueError("SOMA hand joints must follow parent-before-child order") self.layer = layer + self.reference_pose = reference_pose self.joint_names = joint_names self.parent_ids = tuple(int(parent) for parent in parent_ids) self.bones = tuple( @@ -47,16 +43,7 @@ def _pose_inputs( ) -> tuple[np.ndarray, np.ndarray, np.ndarray, bool]: rotations, control_valid = _dense_joint_rotations(data.joint_rotations, 25) - translation = data.global_translation - translation_valid = ( - bool(data.global_translation_is_valid) and translation is not None - ) - translation_xyz = np.asarray( - [translation.x, translation.y, translation.z] - if translation_valid - else [0.0, 0.0, 0.0], - dtype=np.float32, - ) + translation_xyz, translation_valid = _translation_input(data) return rotations, control_valid, translation_xyz, translation_valid def evaluate(self, data: Any) -> tuple[np.ndarray, np.ndarray, np.ndarray]: @@ -75,6 +62,7 @@ def evaluate(self, data: Any) -> tuple[np.ndarray, np.ndarray, np.ndarray]: pose2rot=False, fk_only=True, apply_correctives=False, + reference_pose=self.reference_pose, ) orientations = transforms.matrix_to_quaternion_xyzw( output["transforms"][0, :, :3, :3] diff --git a/tests/python/core/retargeting_engine/test_soma_body_evaluator.py b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py index dd97acfa53..27612daa7b 100644 --- a/tests/python/core/retargeting_engine/test_soma_body_evaluator.py +++ b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py @@ -7,27 +7,103 @@ import pytest from isaaccapture.retargeting_engine.utilities.soma_body_evaluator import ( + _dense_joint_poses, _dense_joint_rotations, _SomaBodyEvaluator, ) -from isaaccapture.schema import Quaternion, SomaBodyJoint, SomaBodyJointRotation +from isaaccapture.retargeting_engine.utilities.soma_contract import ( + SOMA_REFERENCE_VERSION, +) +from isaaccapture.schema import ( + Point, + Pose, + Quaternion, + SomaBodyJoint, + SomaBodyJointPose, + SomaBodyJointRotation, +) -def evaluator_with_two_branches() -> _SomaBodyEvaluator: +def body_joint_names(): + return tuple( + name + for name, joint in sorted( + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ) + + +def compatible_layer(): layer = MagicMock() - layer.public_joint_names = ("Root", *(f"Joint{index}" for index in range(77))) + layer.public_joint_names = ("Root", *body_joint_names()) layer.output_joint_parent_ids = np.array([0, 0, 1, 1, *range(3, 77)]) - return _SomaBodyEvaluator(layer) + layer.output_unit = MagicMock(meters_per_unit=1.0) + layer.get_reference_pose.return_value = np.tile(np.eye(3), (78, 1, 1)) + return layer + + +def evaluator_with_two_branches() -> _SomaBodyEvaluator: + return _SomaBodyEvaluator(compatible_layer()) def test_evaluator_exposes_native_joint_order_and_topology(): evaluator = evaluator_with_two_branches() - assert evaluator.joint_names[:3] == ("Joint0", "Joint1", "Joint2") + assert evaluator.joint_names[:3] == ("HIPS", "SPINE1", "SPINE2") assert evaluator.bones[:3] == ((0, 1), (0, 2), (2, 3)) assert len(evaluator.joint_names) == 77 assert len(evaluator.bones) == 76 +def test_evaluator_resolves_declared_reference_once(): + layer = compatible_layer() + evaluator = _SomaBodyEvaluator(layer) + + layer.get_reference_pose.assert_called_once_with(version=SOMA_REFERENCE_VERSION) + assert evaluator.reference_pose is layer.get_reference_pose.return_value + + +@pytest.mark.parametrize( + "change,error", + [ + ( + lambda layer: setattr( + layer, + "public_joint_names", + ("Root", *reversed(body_joint_names())), + ), + "joint order", + ), + ( + lambda layer: setattr(layer.output_unit, "meters_per_unit", 0.01), + "meters", + ), + ( + lambda layer: setattr( + layer, + "output_joint_parent_ids", + np.array([0, 0, 2, *range(2, 77)]), + ), + "hierarchy", + ), + ], +) +def test_evaluator_rejects_incompatible_model_contract(change, error): + layer = compatible_layer() + change(layer) + + with pytest.raises(ValueError, match=error): + _SomaBodyEvaluator(layer) + + +def test_evaluator_rejects_unavailable_declared_reference(): + layer = compatible_layer() + layer.get_reference_pose.side_effect = KeyError("missing") + + with pytest.raises(ValueError, match="reference"): + _SomaBodyEvaluator(layer) + + def test_evaluator_propagates_validity_through_ancestors(): evaluator = evaluator_with_two_branches() controls = np.ones(77, dtype=bool) @@ -96,3 +172,61 @@ def test_dense_adapter_rejects_unsorted_or_duplicate_joint_identifiers(): _dense_joint_rotations([head, hips], 77) with pytest.raises(ValueError, match="sorted and unique"): _dense_joint_rotations([head, head], 77) + + +def test_dense_rotations_normalize_usable_values_and_invalidate_bad_values(): + entries = [ + SomaBodyJointRotation(SomaBodyJoint.HIPS, Quaternion()), + SomaBodyJointRotation(SomaBodyJoint.SPINE1, Quaternion(np.nan, 0.0, 0.0, 1.0)), + SomaBodyJointRotation(SomaBodyJoint.SPINE2, Quaternion(0.0, 0.0, 0.0, 2.0)), + SomaBodyJointRotation(SomaBodyJoint.CHEST, Quaternion(0.0, 0.0, 0.0, -2.0)), + SomaBodyJointRotation(SomaBodyJoint.NECK1, Quaternion(0.0, 0.0, 0.0, 1e-11)), + SomaBodyJointRotation(SomaBodyJoint.NECK2, Quaternion(0.0, 0.0, 0.0, 1e-12)), + ] + + rotations, valid = _dense_joint_rotations(entries, 77) + + np.testing.assert_array_equal(valid[:6], [False, False, True, True, True, False]) + np.testing.assert_array_equal(rotations[:2], np.tile([0, 0, 0, 1], (2, 1))) + np.testing.assert_allclose(rotations[2], [0, 0, 0, 1]) + np.testing.assert_allclose(rotations[3], [0, 0, 0, -1]) + np.testing.assert_allclose(rotations[4], [0, 0, 0, 1]) + + +def test_dense_poses_normalize_or_invalidate_each_entry(): + entries = [ + SomaBodyJointPose( + SomaBodyJoint.HIPS, + Pose(Point(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 2.0)), + ), + SomaBodyJointPose( + SomaBodyJoint.SPINE1, + Pose(Point(np.inf, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ), + SomaBodyJointPose( + SomaBodyJoint.SPINE2, + Pose(Point(0.0, 0.0, 0.0), Quaternion()), + ), + ] + + positions, orientations, valid = _dense_joint_poses(entries, 77) + + np.testing.assert_array_equal(valid[:3], [1, 0, 0]) + np.testing.assert_array_equal(positions[0], [1, 2, 3]) + np.testing.assert_allclose(orientations[0], [0, 0, 0, 1]) + assert np.isfinite(positions).all() + assert np.isfinite(orientations).all() + + +@pytest.mark.parametrize("bad_value", [np.nan, np.inf, -np.inf]) +def test_nonfinite_translation_is_unavailable(bad_value): + data = MagicMock( + joint_rotations=None, + global_translation=MagicMock(x=bad_value, y=0.0, z=0.0), + global_translation_is_valid=True, + ) + + _, _, translation, translation_valid = _SomaBodyEvaluator._pose_inputs(data) + + assert not translation_valid + np.testing.assert_array_equal(translation, np.zeros(3, dtype=np.float32)) diff --git a/tests/python/core/retargeting_engine/test_soma_body_source.py b/tests/python/core/retargeting_engine/test_soma_body_source.py index b393c98c47..d1adbbe6b3 100644 --- a/tests/python/core/retargeting_engine/test_soma_body_source.py +++ b/tests/python/core/retargeting_engine/test_soma_body_source.py @@ -45,8 +45,19 @@ def pose(): def fake_layer(): layer = MagicMock() - layer.public_joint_names = ("Root", *(f"Joint{index}" for index in range(77))) + layer.public_joint_names = ( + "Root", + *( + name + for name, joint in sorted( + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ), + ) layer.output_joint_parent_ids = np.array([0, 0, *range(1, 77)]) + layer.output_unit = MagicMock(meters_per_unit=1.0) + layer.get_reference_pose.return_value = np.tile(np.eye(3), (78, 1, 1)) return layer @@ -147,6 +158,40 @@ def test_joint_pose_source_maps_received_payload_without_fk(): assert evaluated[SomaBodyInputIndex.JOINT_VALID].all() +def test_joint_pose_source_invalidates_bad_numeric_entries(): + soma_source = SomaBodySource( + "body", + "vendor.soma", + representation=SomaBodyRepresentation.JOINT_POSES, + ) + raw = SomaBodyJointPoses( + [ + SomaBodyJointPose( + SomaBodyJoint.HIPS, + Pose(Point(1, 2, 3), Quaternion(0, 0, 0, -2)), + ), + SomaBodyJointPose( + SomaBodyJoint.SPINE1, + Pose(Point(np.nan, 0, 0), Quaternion(0, 0, 0, 1)), + ), + SomaBodyJointPose( + SomaBodyJoint.SPINE2, + Pose(Point(0, 0, 0), Quaternion()), + ), + ] + ) + inputs = payload_group(soma_source.input_spec()["deviceio_soma_body"], raw) + + evaluated = soma_source({"deviceio_soma_body": inputs})[SomaBodySource.BODY] + + np.testing.assert_array_equal( + evaluated[SomaBodyInputIndex.JOINT_VALID][:3], [1, 0, 0] + ) + np.testing.assert_allclose( + evaluated[SomaBodyInputIndex.JOINT_ORIENTATIONS][0], [0, 0, 0, -1] + ) + + def test_absent_sample_clears_reused_output(): soma_source = source() soma_source._evaluator.evaluate = MagicMock( diff --git a/tests/python/core/retargeting_engine/test_soma_hand_source.py b/tests/python/core/retargeting_engine/test_soma_hand_source.py index b0d8f5a016..f0880a68a6 100644 --- a/tests/python/core/retargeting_engine/test_soma_hand_source.py +++ b/tests/python/core/retargeting_engine/test_soma_hand_source.py @@ -18,6 +18,9 @@ ) from isaaccapture.retargeting_engine.interface.tensor_group import TensorGroup from isaaccapture.retargeting_engine.tensor_types import SomaHandInputIndex +from isaaccapture.retargeting_engine.utilities.soma_hand_evaluator import ( + _SomaHandEvaluator, +) from isaaccapture.schema import ( Point, Pose, @@ -31,10 +34,57 @@ ) -def fake_layer(): +def hand_joint_names(): + return tuple( + name + for name, joint in sorted( + SomaHandJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ) + + +def fake_layer(hand_type="left"): layer = MagicMock() - layer.rig_data = {"joint_names": [f"Joint{index}" for index in range(25)]} - layer.joint_parent_ids = np.array([0, 0, 1, 2, 3, *range(20)])[:25] + prefix = f"{hand_type.capitalize()}Hand" + layer.rig_data = { + "joint_names": [ + prefix if index == 0 else f"{prefix}{name.replace('_', '')}" + for index, name in enumerate(hand_joint_names()) + ] + } + layer.joint_parent_ids = np.array( + [ + 0, + 0, + 1, + 2, + 3, + 0, + 5, + 6, + 7, + 8, + 0, + 10, + 11, + 12, + 13, + 0, + 15, + 16, + 17, + 18, + 0, + 20, + 21, + 22, + 23, + ] + ) + layer.hand_type = hand_type + layer.output_unit = MagicMock(meters_per_unit=1.0) + layer.get_reference_pose.return_value = np.tile(np.eye(3), (25, 1, 1)) return layer @@ -65,8 +115,10 @@ def payload_group(group_type, data): def test_rotation_source_evaluates_received_payload(): - source = SomaHandSource("left", "vendor.left", SomaHandedness.LEFT, fake_layer()) + layer = fake_layer() + source = SomaHandSource("left", "vendor.left", SomaHandedness.LEFT, layer) assert isinstance(source.get_tracker(), SomaHandJointRotationsTracker) + layer.get_reference_pose.assert_called_once_with(version="v0.3.1") raw = rotations() expected = ( np.ones((25, 3), dtype=np.float32), @@ -117,6 +169,42 @@ def test_joint_pose_source_maps_without_fk(): assert evaluated[SomaHandInputIndex.JOINT_VALID].all() +def test_joint_pose_source_invalidates_bad_numeric_entries(): + source = SomaHandSource( + "left", + "vendor.left", + SomaHandedness.LEFT, + representation=SomaHandRepresentation.JOINT_POSES, + ) + raw = SomaHandJointPoses( + [ + SomaHandJointPose( + SomaHandJoint.WRIST, + Pose(Point(1, 2, 3), Quaternion(0, 0, 0, -2)), + ), + SomaHandJointPose( + SomaHandJoint.THUMB1, + Pose(Point(np.nan, 0, 0), Quaternion(0, 0, 0, 1)), + ), + SomaHandJointPose( + SomaHandJoint.THUMB2, + Pose(Point(0, 0, 0), Quaternion()), + ), + ], + SomaHandedness.LEFT, + ) + inputs = payload_group(source.input_spec()["deviceio_soma_hand"], raw) + + evaluated = source({"deviceio_soma_hand": inputs})[SomaHandSource.HAND] + + np.testing.assert_array_equal( + evaluated[SomaHandInputIndex.JOINT_VALID][:3], [1, 0, 0] + ) + np.testing.assert_allclose( + evaluated[SomaHandInputIndex.JOINT_ORIENTATIONS][0], [0, 0, 0, -1] + ) + + def test_payload_handedness_must_match_collection(): source = SomaHandSource( "left", @@ -150,3 +238,81 @@ def test_rotation_source_requires_layer_and_valid_side(): SomaHandedness.UNSPECIFIED, fake_layer(), ) + + +def test_rotation_source_rejects_layer_for_other_hand(): + with pytest.raises(ValueError, match="does not match"): + SomaHandSource( + "left", + "vendor.left", + SomaHandedness.LEFT, + fake_layer("right"), + ) + + +def test_rotation_source_rejects_wrong_hand_hierarchy(): + layer = fake_layer() + layer.joint_parent_ids[2] = 0 + + with pytest.raises(ValueError, match="hierarchy"): + SomaHandSource( + "left", + "vendor.left", + SomaHandedness.LEFT, + layer, + ) + + +@pytest.mark.parametrize( + "change,error", + [ + ( + lambda layer: layer.rig_data["joint_names"].reverse(), + "joint order", + ), + ( + lambda layer: setattr(layer.output_unit, "meters_per_unit", 0.01), + "meters", + ), + ( + lambda layer: setattr( + layer.get_reference_pose, "side_effect", KeyError("missing") + ), + "reference", + ), + ], +) +def test_rotation_source_rejects_incompatible_hand_contract(change, error): + layer = fake_layer() + change(layer) + + with pytest.raises(ValueError, match=error): + SomaHandSource( + "left", + "vendor.left", + SomaHandedness.LEFT, + layer, + ) + + +def test_hand_rotation_inputs_normalize_and_invalidate_numeric_values(): + payload = SomaHandJointRotations( + [ + SomaHandJointRotation(SomaHandJoint.WRIST, Quaternion()), + SomaHandJointRotation(SomaHandJoint.THUMB1, Quaternion(0, 0, 0, 2)), + SomaHandJointRotation(SomaHandJoint.THUMB2, Quaternion(0, 0, 0, -2)), + ], + Point(np.inf, 0, 0), + True, + SomaHandedness.LEFT, + ) + + rotations, valid, translation, translation_valid = _SomaHandEvaluator._pose_inputs( + payload + ) + + np.testing.assert_array_equal(valid[:3], [False, True, True]) + np.testing.assert_allclose(rotations[1], [0, 0, 0, 1]) + np.testing.assert_allclose(rotations[2], [0, 0, 0, -1]) + assert not translation_valid + np.testing.assert_array_equal(translation, np.zeros(3, dtype=np.float32)) diff --git a/tests/python/examples/deviceio_live_view/test_soma_viewer.py b/tests/python/examples/deviceio_live_view/test_soma_viewer.py index de488a168c..c1b0a71b38 100644 --- a/tests/python/examples/deviceio_live_view/test_soma_viewer.py +++ b/tests/python/examples/deviceio_live_view/test_soma_viewer.py @@ -72,8 +72,19 @@ def fake_layer(): layer = MagicMock() - layer.public_joint_names = ("Root", *(f"Joint{index}" for index in range(77))) + layer.public_joint_names = ( + "Root", + *( + name + for name, joint in sorted( + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" + ), + ) layer.output_joint_parent_ids = np.array([0, 0, *range(1, 77)]) + layer.output_unit = MagicMock(meters_per_unit=1.0) + layer.get_reference_pose.return_value = np.tile(np.eye(3), (78, 1, 1)) return layer diff --git a/tests/python/examples/soma_hand_publisher/test_publisher.py b/tests/python/examples/soma_hand_publisher/test_publisher.py index 2af8824110..9222d18b38 100644 --- a/tests/python/examples/soma_hand_publisher/test_publisher.py +++ b/tests/python/examples/soma_hand_publisher/test_publisher.py @@ -72,7 +72,7 @@ def test_demo_profiles_match_upstream_hand_fk(soma_assets, request): payload = soma_joint_rotations( frame["rotations"][0], frame["translation"][0], handedness ) - positions, _, valid = _SomaHandEvaluator(hands[side]).evaluate(payload) + positions, _, valid = _SomaHandEvaluator(hands[side], side).evaluate(payload) np.testing.assert_allclose(positions, frame["positions"][0], atol=1e-5) np.testing.assert_array_equal(valid, np.ones(25, dtype=np.uint8)) evaluated = soma_joint_poses( From 5803f716ff5ecf52677baba96702dffa1c1764ff Mon Sep 17 00:00:00 2001 From: Michael Haselton Date: Wed, 7 Oct 2026 15:13:33 -0400 Subject: [PATCH 09/12] fix(soma): simplify publisher setup and harden pose handling Resolve demo assets through SOMA-X and remove --data-root from both publishers. Add a first-use loading notice and simplify launch docs. Compare handedness by value and avoid float32 overflow when normalizing quaternions. Add focused regression coverage for these fixes and automatic publisher asset resolution. Signed-off-by: Michael Haselton --- docs/source/device/soma.rst | 10 +-- examples/soma_body_publisher/README.md | 6 +- .../soma_body_publisher/publisher.py | 13 ++-- examples/soma_hand_publisher/README.md | 7 +- .../soma_hand_publisher/publisher.py | 13 ++-- src/core/schema/python/soma_common_bindings.h | 1 - .../deviceio_source_nodes/soma_hand_source.py | 3 +- .../utilities/soma_body_evaluator.py | 6 +- .../test_soma_body_evaluator.py | 18 +++++ .../test_soma_hand_source.py | 14 ++++ .../soma_body_publisher/test_publisher.py | 30 ++++++-- .../soma_hand_publisher/test_publisher.py | 72 +++++++++++++++++++ 12 files changed, 160 insertions(+), 33 deletions(-) diff --git a/docs/source/device/soma.rst b/docs/source/device/soma.rst index 2051715f4c..9f0e97e1e8 100644 --- a/docs/source/device/soma.rst +++ b/docs/source/device/soma.rst @@ -100,10 +100,7 @@ build their native pushers: --target isaacteleop_python soma_body_pusher soma_hand_pusher \ -j4 -The example packages install ``py-soma-x==0.3.1``. SOMA-X resolves the model -assets needed by rotation consumers. The demo publishers additionally require a -matching SOMA-X asset directory containing ``example_animation.npy``. The hand -publisher also requires ``SOMAHand.npz``. +The example packages install ``py-soma-x==0.3.1``. Run the body example -------------------- @@ -125,7 +122,6 @@ publisher: source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ - --data-root /path/to/SOMA-X/assets \ --pusher ./build/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher \ --loop @@ -151,7 +147,6 @@ publisher: source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ - --data-root /path/to/SOMA-X/assets \ --pusher ./build/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher \ --loop @@ -238,8 +233,7 @@ The viewer receives no skeleton ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Confirm that the publisher and viewer use the same representation and -collection names. For a demo publisher, also confirm that its asset directory -matches SOMA-X v0.3.1 and contains the required animation and model files. +collection names. See also -------- diff --git a/examples/soma_body_publisher/README.md b/examples/soma_body_publisher/README.md index 037e7a50fd..92868a7ce1 100644 --- a/examples/soma_body_publisher/README.md +++ b/examples/soma_body_publisher/README.md @@ -37,7 +37,6 @@ Start a SOMA consumer such as `deviceio_live_view`, connect its client, then run ```bash source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ - --data-root /path/to/SOMA-X/assets \ --pusher ./build/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher \ --loop ``` @@ -47,6 +46,5 @@ add `--body-representation joint-poses`. Configure the consumer with the same representation. The two modes publish `soma_body_joint_rotations` and `soma_body_joint_poses`, respectively. -The assets directory must contain `example_animation.npy` and the neutral SOMA -identity assets. Add `--validate-only` to process every bundled frame through -the FlatBuffer verifier without starting OpenXR or publishing data. +Add `--validate-only` to process every bundled frame through the FlatBuffer +verifier without starting OpenXR or publishing data. diff --git a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py index 229bb8b2a9..1dcd7af2ff 100644 --- a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py +++ b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py @@ -37,11 +37,17 @@ ) -def create_layer(data_root: Path): +def create_layer(data_root: Path | None = None): """Create the neutral SOMA-X v0.3.1 model used by the bundled motion.""" import soma import torch + # The v0.3.1 body-layer fallback imports a nonexistent soma.body.assets. + if data_root is None: + logger.info( + "Loading SOMA assets. First use may download files and take a while." + ) + data_root = soma.get_assets_dir() layer = soma.SOMALayer( data_root=str(data_root), identity_model_type="soma", @@ -133,7 +139,6 @@ def soma_joint_poses(positions, orientations): def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--data-root", required=True, type=Path) parser.add_argument( "--pusher", required=True, type=Path, help="Built soma_body_pusher executable" ) @@ -153,8 +158,8 @@ def main(argv: list[str]) -> int: args = parser.parse_args(argv[1:]) if not math.isfinite(args.rate) or args.rate <= 0: parser.error("rate must be finite and positive") - layer = create_layer(args.data_root) - q, t, rotations = demo_controls(args.data_root / "example_animation.npy", layer) + layer = create_layer() + q, t, rotations = demo_controls(layer.data_root / "example_animation.npy", layer) if args.body_representation == "joint-poses": positions, orientations = evaluate_demo_controls(layer, rotations, t) frame_values = positions, orientations diff --git a/examples/soma_hand_publisher/README.md b/examples/soma_hand_publisher/README.md index 351b9599c5..31a6d414cb 100644 --- a/examples/soma_hand_publisher/README.md +++ b/examples/soma_hand_publisher/README.md @@ -42,7 +42,6 @@ uv run --no-sync python -m isaaccapture_examples.deviceio_live_view \ source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ - --data-root /path/to/SOMA-X-v0.3.1-assets \ --pusher ./build/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher \ --loop ``` @@ -52,7 +51,5 @@ Joint rotations are the default. To publish evaluated joint poses, add `--soma-hand-representation joint-poses` to the viewer. The two modes publish `soma_hand_joint_rotations` and `soma_hand_joint_poses`, respectively. -The assets must include `SOMAHand.npz`, `example_animation.npy`, and the neutral -SOMA identity assets from the v0.3.1 asset release. Add `--validate-only` to -process every paired frame through the native FlatBuffer verifier without -starting OpenXR. +Add `--validate-only` to process every paired frame through the native +FlatBuffer verifier without starting OpenXR. diff --git a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py index 8d538237b4..ffdf26a97d 100644 --- a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py +++ b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py @@ -38,11 +38,17 @@ ) -def create_layers(data_root: Path): +def create_layers(data_root: Path | None = None): """Create the neutral full-body and hand layers used by the bundled motion.""" import soma import torch + # The v0.3.1 body-layer fallback imports a nonexistent soma.body.assets. + if data_root is None: + logger.info( + "Loading SOMA assets. First use may download files and take a while." + ) + data_root = soma.get_assets_dir() body = soma.SOMALayer( data_root=str(data_root), identity_model_type="soma", @@ -173,7 +179,6 @@ def _payload(frame, index: int, representation: str, handedness): def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument("--data-root", required=True, type=Path) parser.add_argument( "--pusher", required=True, type=Path, help="Built soma_hand_pusher executable" ) @@ -194,9 +199,9 @@ def main(argv: list[str]) -> int: if not math.isfinite(args.rate) or args.rate <= 0: parser.error("rate must be finite and positive") - body, hand_layers = create_layers(args.data_root) + body, hand_layers = create_layers() frames = demo_hand_frames( - args.data_root / "example_animation.npy", body, hand_layers + body.data_root / "example_animation.npy", body, hand_layers ) command = [ str(args.pusher.resolve()), diff --git a/src/core/schema/python/soma_common_bindings.h b/src/core/schema/python/soma_common_bindings.h index 7113f6b158..175dc8d6c8 100644 --- a/src/core/schema/python/soma_common_bindings.h +++ b/src/core/schema/python/soma_common_bindings.h @@ -17,7 +17,6 @@ inline void bind_soma_common(py::module& m) .value("UNSPECIFIED", SomaHandedness_UNSPECIFIED) .value("LEFT", SomaHandedness_LEFT) .value("RIGHT", SomaHandedness_RIGHT); - } } // namespace core diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py index b318d3479b..b851501d9d 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py @@ -63,7 +63,8 @@ def __init__( else: self._tracker = SomaHandJointPosesTracker(collection_id) self._input_type = DeviceIOSomaHandJointPosesTracked() - expected_hand_type = "left" if handedness is SomaHandedness.LEFT else "right" + # Pybind enum values read from buffers are not singleton objects. + expected_hand_type = "left" if handedness == SomaHandedness.LEFT else "right" self._evaluator = ( _SomaHandEvaluator(layer, expected_hand_type) if layer is not None else None ) diff --git a/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py index 87e3bf5220..5ab1c3161e 100644 --- a/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py @@ -28,10 +28,12 @@ def _normalize_quaternion(value: Any) -> tuple[np.ndarray, bool]: quaternion = np.asarray(value, dtype=np.float32) if quaternion.shape != (4,) or not np.isfinite(quaternion).all(): return np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), False - norm = float(np.linalg.norm(quaternion.astype(np.float64))) + quaternion64 = quaternion.astype(np.float64) + norm = float(np.linalg.norm(quaternion64)) if norm <= _QUATERNION_EPS: return np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), False - return quaternion / np.float32(norm), True + # Finite float32 components can have a norm larger than float32 can represent. + return (quaternion64 / norm).astype(np.float32), True def _dense_joint_rotations( diff --git a/tests/python/core/retargeting_engine/test_soma_body_evaluator.py b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py index 27612daa7b..ebfb6b69b5 100644 --- a/tests/python/core/retargeting_engine/test_soma_body_evaluator.py +++ b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py @@ -193,6 +193,24 @@ def test_dense_rotations_normalize_usable_values_and_invalidate_bad_values(): np.testing.assert_allclose(rotations[4], [0, 0, 0, 1]) +@pytest.mark.parametrize("sign", [-1.0, 1.0]) +def test_dense_adapters_normalize_large_finite_quaternions(sign): + quaternion = Quaternion(*np.full(4, sign * 2e38, dtype=np.float32)) + rotation_entry = SomaBodyJointRotation(SomaBodyJoint.HIPS, quaternion) + pose_entry = SomaBodyJointPose(SomaBodyJoint.HIPS, Pose(Point(1, 2, 3), quaternion)) + + with np.errstate(over="raise", invalid="raise"): + rotations, rotation_valid = _dense_joint_rotations([rotation_entry], 77) + _, orientations, pose_valid = _dense_joint_poses([pose_entry], 77) + + assert rotation_valid[0] + assert pose_valid[0] + np.testing.assert_allclose(rotations[0], np.full(4, sign * 0.5)) + np.testing.assert_allclose(orientations[0], rotations[0]) + assert rotations.dtype == np.float32 + assert orientations.dtype == np.float32 + + def test_dense_poses_normalize_or_invalidate_each_entry(): entries = [ SomaBodyJointPose( diff --git a/tests/python/core/retargeting_engine/test_soma_hand_source.py b/tests/python/core/retargeting_engine/test_soma_hand_source.py index f0880a68a6..591368398f 100644 --- a/tests/python/core/retargeting_engine/test_soma_hand_source.py +++ b/tests/python/core/retargeting_engine/test_soma_hand_source.py @@ -142,6 +142,20 @@ def test_rotation_source_evaluates_received_payload(): ) +@pytest.mark.parametrize( + "handedness,hand_type", + [(SomaHandedness.LEFT, "left"), (SomaHandedness.RIGHT, "right")], +) +def test_rotation_source_accepts_payload_handedness(handedness, hand_type): + payload = rotations(handedness) + layer = fake_layer(hand_type) + + source = SomaHandSource("hand", "vendor.hand", payload.handedness, layer) + + assert source.handedness == handedness + assert source._evaluator.layer is layer + + def test_joint_pose_source_maps_without_fk(): source = SomaHandSource( "left", diff --git a/tests/python/examples/soma_body_publisher/test_publisher.py b/tests/python/examples/soma_body_publisher/test_publisher.py index ff5d900f22..9d1c9f8083 100644 --- a/tests/python/examples/soma_body_publisher/test_publisher.py +++ b/tests/python/examples/soma_body_publisher/test_publisher.py @@ -4,6 +4,9 @@ import signal import struct import subprocess +import sys +from types import SimpleNamespace +from unittest.mock import MagicMock import numpy as np import pytest @@ -44,22 +47,41 @@ def test_nested_soma_serialization_uses_payload_root(): assert evaluated_record.data.to_bytes() == evaluated.to_bytes() +def test_layer_uses_upstream_asset_cache(monkeypatch, tmp_path): + soma = MagicMock() + soma.get_assets_dir.return_value = tmp_path + layer = soma.SOMALayer.return_value + layer.public_joint_names = [ + "Root", + *[joint.name.replace("_", "") for joint in publisher.BODY_JOINTS], + ] + layer.data_root = tmp_path + monkeypatch.setitem(sys.modules, "soma", soma) + monkeypatch.setitem(sys.modules, "torch", MagicMock()) + + assert create_layer() is layer + + soma.get_assets_dir.assert_called_once_with() + assert soma.SOMALayer.call_args.kwargs["data_root"] == str(tmp_path) + layer.prepare_identity.assert_called_once() + + @pytest.mark.parametrize("exit_code", [0, -signal.SIGINT, 1]) def test_publisher_interrupt_preserves_real_failures(monkeypatch, tmp_path, exit_code): - from unittest.mock import MagicMock - process = MagicMock() process.__enter__.return_value = process process.stdin.write.side_effect = KeyboardInterrupt process.wait.return_value = exit_code monkeypatch.setattr(publisher.subprocess, "Popen", lambda *args, **kwargs: process) - monkeypatch.setattr(publisher, "create_layer", lambda _: None) + monkeypatch.setattr( + publisher, "create_layer", lambda: SimpleNamespace(data_root=tmp_path) + ) monkeypatch.setattr( publisher, "demo_controls", lambda *args: (np.tile([0, 0, 0, 1], (1, 77, 1)), [[0, 0, 0]], None), ) - args = ["soma_body_publisher", "--data-root", str(tmp_path), "--pusher", "pusher"] + args = ["soma_body_publisher", "--pusher", "pusher"] if exit_code == 1: with pytest.raises(RuntimeError, match="pusher failed"): publisher.main(args) diff --git a/tests/python/examples/soma_hand_publisher/test_publisher.py b/tests/python/examples/soma_hand_publisher/test_publisher.py index 9222d18b38..3faa72fc85 100644 --- a/tests/python/examples/soma_hand_publisher/test_publisher.py +++ b/tests/python/examples/soma_hand_publisher/test_publisher.py @@ -3,8 +3,12 @@ import struct import subprocess +import sys +from types import SimpleNamespace +from unittest.mock import MagicMock import numpy as np +import pytest from isaaccapture.retargeting_engine.utilities.soma_body_evaluator import ( _dense_joint_poses, @@ -14,6 +18,7 @@ _SomaHandEvaluator, ) from isaaccapture.schema import SomaHandedness +from isaaccapture_examples.soma_hand_publisher import publisher from isaaccapture_examples.soma_hand_publisher.publisher import ( create_layers, demo_hand_frames, @@ -22,6 +27,73 @@ ) +def test_layers_resolve_assets_without_explicit_path(monkeypatch, tmp_path): + soma = MagicMock() + soma.get_assets_dir.return_value = tmp_path + monkeypatch.setitem(sys.modules, "soma", soma) + monkeypatch.setitem(sys.modules, "torch", MagicMock()) + + body, hands = create_layers() + + assert soma.SOMALayer.call_args.kwargs["data_root"] == str(tmp_path) + assert [call.kwargs["hand_type"] for call in soma.SOMAHandLayer.call_args_list] == [ + "left", + "right", + ] + assert all( + call.kwargs["data_root"] == str(tmp_path) + for call in soma.SOMAHandLayer.call_args_list + ) + body.prepare_identity.assert_called_once() + assert set(hands) == {"left", "right"} + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +def test_publisher_uses_resolved_demo_without_asset_argument( + monkeypatch, tmp_path, representation +): + body = SimpleNamespace(data_root=tmp_path) + layers = MagicMock(return_value=(body, {})) + frame = { + "rotations": np.tile([0, 0, 0, 1], (1, 25, 1)), + "translation": np.zeros((1, 3)), + "positions": np.zeros((1, 25, 3)), + "orientations": np.tile([0, 0, 0, 1], (1, 25, 1)), + } + demo = MagicMock(return_value={"left": frame, "right": frame}) + process = MagicMock() + process.__enter__.return_value = process + process.wait.return_value = 0 + popen = MagicMock(return_value=process) + monkeypatch.setattr(publisher, "create_layers", layers) + monkeypatch.setattr(publisher, "demo_hand_frames", demo) + monkeypatch.setattr(publisher.subprocess, "Popen", popen) + + assert ( + publisher.main( + [ + "soma_hand_publisher", + "--pusher", + "pusher", + "--hand-representation", + representation, + "--validate-only", + ] + ) + == 0 + ) + + layers.assert_called_once_with() + demo.assert_called_once_with(tmp_path / "example_animation.npy", body, {}) + assert popen.call_args.args[0][1:] == [ + "--hand-representation", + representation, + "--validate-only", + ] + assert process.stdin.write.call_count == 3 + process.stdin.close.assert_called_once() + + def test_hand_payload_constructors_preserve_side_and_arrays(): rotations = np.tile([0.0, 0.0, 0.0, 1.0], (25, 1)) rotation_payload = soma_joint_rotations(rotations, [1, 2, 3], SomaHandedness.LEFT) From d6607be79b554839c36ad804a838f87c9abc74be Mon Sep 17 00:00:00 2001 From: Michael Haselton Date: Wed, 7 Oct 2026 17:13:22 -0400 Subject: [PATCH 10/12] fix(soma): annotate body and hand tracker unions Declare both rotation and evaluated-pose tracker types so mypy passes against generated native stubs. Signed-off-by: Michael Haselton --- .../deviceio_source_nodes/soma_body_source.py | 2 ++ .../deviceio_source_nodes/soma_hand_source.py | 1 + 2 files changed, 3 insertions(+) diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py index 84dfdd0d4c..4ed07611b6 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py @@ -54,6 +54,8 @@ def __init__( if not collection_id: raise ValueError("SOMA body collection_id must not be empty") self.representation = SomaBodyRepresentation(representation) + # Without generated native stubs, mypy can miss tracker type mismatches. + self._tracker: SomaBodyJointRotationsTracker | SomaBodyJointPosesTracker if self.representation is SomaBodyRepresentation.JOINT_ROTATIONS: if layer is None: raise ValueError("joint-rotations requires a prepared SOMA layer") diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py index b851501d9d..e4dbb83b10 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py @@ -55,6 +55,7 @@ def __init__( raise ValueError("SOMA hand source requires LEFT or RIGHT handedness") self.handedness = handedness self.representation = SomaHandRepresentation(representation) + self._tracker: SomaHandJointRotationsTracker | SomaHandJointPosesTracker if self.representation is SomaHandRepresentation.JOINT_ROTATIONS: if layer is None: raise ValueError("joint-rotations requires a prepared SOMA hand layer") From 89a93a1ae5663dc2066678b67115fc56ad5ca7e4 Mon Sep 17 00:00:00 2001 From: Michael Haselton Date: Wed, 7 Oct 2026 17:58:12 -0400 Subject: [PATCH 11/12] refactor(soma): publish demos in-process with TensorPushTracker - Use the existing Python tracker API for body and hand publishing. - Remove C++ demo senders, stdin framing, and the --pusher option. - Preserve schemas, collections, and representation selection. - Use session timestamps and retain paired hand publishing. - Update publisher tests, build configuration, and launch documentation. Signed-off-by: Michael Haselton --- docs/source/device/soma.rst | 10 +- examples/soma_body_publisher/CMakeLists.txt | 2 - examples/soma_body_publisher/README.md | 17 +- .../cpp/soma_body_pusher/CMakeLists.txt | 15 -- .../cpp/soma_body_pusher/soma_body_pusher.cpp | 150 ----------- .../soma_body_publisher/publisher.py | 67 +++-- examples/soma_hand_publisher/CMakeLists.txt | 2 - examples/soma_hand_publisher/README.md | 17 +- .../cpp/soma_hand_pusher/CMakeLists.txt | 15 -- .../cpp/soma_hand_pusher/soma_hand_pusher.cpp | 182 ------------- .../soma_hand_publisher/publisher.py | 78 +++--- .../soma_body_publisher/CMakeLists.txt | 1 - .../examples/soma_body_publisher/conftest.py | 1 - .../soma_body_publisher/test_publisher.py | 239 +++++++++++------- .../soma_hand_publisher/CMakeLists.txt | 1 - .../examples/soma_hand_publisher/conftest.py | 3 - .../soma_hand_publisher/test_publisher.py | 220 ++++++++++------ 17 files changed, 401 insertions(+), 619 deletions(-) delete mode 100644 examples/soma_body_publisher/cpp/soma_body_pusher/CMakeLists.txt delete mode 100644 examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp delete mode 100644 examples/soma_hand_publisher/cpp/soma_hand_pusher/CMakeLists.txt delete mode 100644 examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp diff --git a/docs/source/device/soma.rst b/docs/source/device/soma.rst index 9f0e97e1e8..ae7299cfef 100644 --- a/docs/source/device/soma.rst +++ b/docs/source/device/soma.rst @@ -86,8 +86,7 @@ Build the examples Start with a configured Isaac Teleop checkout. See :doc:`../getting_started/build_from_source/index` for the complete build -prerequisites. Install the live viewer and both deterministic publishers, then -build their native pushers: +prerequisites. Install the live viewer and both deterministic publishers: .. code-block:: bash @@ -97,10 +96,13 @@ build their native pushers: .venv/bin/cmake -S . -B build .venv/bin/cmake --build build \ - --target isaacteleop_python soma_body_pusher soma_hand_pusher \ + --target isaacteleop_python \ -j4 The example packages install ``py-soma-x==0.3.1``. +The publishers use the Python-accessible ``TensorPushTracker`` and its native +``SchemaPusher`` transport; no separate sender executable is needed. Demo samples +use the session's monotonic clock for both timestamps. Run the body example -------------------- @@ -122,7 +124,6 @@ publisher: source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ - --pusher ./build/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher \ --loop These commands use ``joint-rotations``. To publish evaluated joint poses, add @@ -147,7 +148,6 @@ publisher: source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ - --pusher ./build/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher \ --loop These commands use ``joint-rotations``. To publish evaluated joint poses, add diff --git a/examples/soma_body_publisher/CMakeLists.txt b/examples/soma_body_publisher/CMakeLists.txt index ff899a019b..4d205725bf 100644 --- a/examples/soma_body_publisher/CMakeLists.txt +++ b/examples/soma_body_publisher/CMakeLists.txt @@ -3,7 +3,5 @@ cmake_minimum_required(VERSION 3.20) -add_subdirectory(cpp/soma_body_pusher) - include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) install_python_example(DESTINATION examples/soma_body_publisher) diff --git a/examples/soma_body_publisher/README.md b/examples/soma_body_publisher/README.md index 92868a7ce1..ee5652ee70 100644 --- a/examples/soma_body_publisher/README.md +++ b/examples/soma_body_publisher/README.md @@ -10,10 +10,9 @@ FlatBuffer and `SchemaPusher`. This is a deterministic input generator for testing SOMA consumers. It is not a vendor plugin or vendor network protocol. The Python process converts the bundled motion into the pinned SOMA-X v0.3.1 -pose contract and writes framed FlatBuffers to the native `soma_body_pusher`. -It can publish joint rotations for downstream FK or pre-evaluated joint poses. -The native process validates the selected payload and publishes the -`soma_body_demo` tensor collection. +pose contract and publishes through `TensorPushTracker`, which wraps +`SchemaPusher`. It can publish joint rotations for downstream FK or +pre-evaluated joint poses on the `soma_body_demo` tensor collection. Each payload uses explicitly keyed SOMA joints. The bundled motion provides all 77 joints; a partial producer can omit joints it does not provide. @@ -21,15 +20,14 @@ Each payload uses explicitly keyed SOMA joints. The bundled motion provides all ```text SOMA-X example_animation.npy -> Python SOMA body encoder - -> soma_body_pusher + -> TensorPushTracker -> SchemaPusher collection soma_body_demo ``` -Install the example and build the native pusher: +Install the example: ```bash uv pip install -e ./examples/soma_body_publisher -cmake --build ./build --target soma_body_pusher ``` Start a SOMA consumer such as `deviceio_live_view`, connect its client, then run: @@ -37,7 +35,6 @@ Start a SOMA consumer such as `deviceio_live_view`, connect its client, then run ```bash source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ - --pusher ./build/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher \ --loop ``` @@ -46,5 +43,5 @@ add `--body-representation joint-poses`. Configure the consumer with the same representation. The two modes publish `soma_body_joint_rotations` and `soma_body_joint_poses`, respectively. -Add `--validate-only` to process every bundled frame through the FlatBuffer -verifier without starting OpenXR or publishing data. +Add `--validate-only` to encode every bundled frame and check transport capacity +without starting OpenXR or publishing data. diff --git a/examples/soma_body_publisher/cpp/soma_body_pusher/CMakeLists.txt b/examples/soma_body_publisher/cpp/soma_body_pusher/CMakeLists.txt deleted file mode 100644 index e4cd8a57de..0000000000 --- a/examples/soma_body_publisher/cpp/soma_body_pusher/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -find_package(Threads REQUIRED) - -add_executable(soma_body_pusher soma_body_pusher.cpp) -target_link_libraries(soma_body_pusher PRIVATE - pusherio::pusherio - oxr::oxr_core - isaacteleop_schema - Threads::Threads -) -install(TARGETS soma_body_pusher - RUNTIME DESTINATION examples/soma_body_publisher/cpp -) diff --git a/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp b/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp deleted file mode 100644 index ffa4e73fbe..0000000000 --- a/examples/soma_body_publisher/cpp/soma_body_pusher/soma_body_pusher.cpp +++ /dev/null @@ -1,150 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace -{ - -enum class BodyRepresentation -{ - JointRotations, - JointPoses, -}; - -struct Arguments -{ - bool validate_only = false; - BodyRepresentation representation = BodyRepresentation::JointRotations; -}; - -Arguments parse_arguments(int argc, char** argv) -{ - Arguments arguments; - for (int index = 1; index < argc; ++index) - { - const std::string argument = argv[index]; - if (argument == "--validate-only") - { - arguments.validate_only = true; - } - else if (argument == "--body-representation" && index + 1 < argc) - { - const std::string value = argv[++index]; - if (value == "joint-rotations") - { - arguments.representation = BodyRepresentation::JointRotations; - } - else if (value == "joint-poses") - { - arguments.representation = BodyRepresentation::JointPoses; - } - else - { - throw std::invalid_argument("Unknown SOMA body representation: " + value); - } - } - else - { - throw std::invalid_argument("Unknown or incomplete argument: " + argument); - } - } - return arguments; -} - -uint64_t little_endian(const uint8_t* bytes, size_t size) -{ - uint64_t value = 0; - for (size_t i = 0; i < size; ++i) - { - value |= static_cast(bytes[i]) << (8 * i); - } - return value; -} - -} // namespace - -int main(int argc, char** argv) -{ - if (argc == 2 && std::string(argv[1]) == "--help") - { - std::cout << "Usage: soma_body_pusher [--body-representation joint-rotations|joint-poses] " - "[--validate-only]\n" - "Reads framed SOMA body FlatBuffers from stdin.\n"; - return 0; - } - auto logger = isaaccapture::Logger::get("isaaccapture.examples.soma_body_pusher"); - try - { - const auto arguments = parse_arguments(argc, argv); - const bool joint_poses = arguments.representation == BodyRepresentation::JointPoses; - const size_t max_flatbuffer_size = joint_poses ? 4096 : 2048; - const char* tensor_identifier = joint_poses ? "soma_body_joint_poses" : "soma_body_joint_rotations"; - std::unique_ptr session; - std::unique_ptr pusher; - size_t frames = 0; - std::array header{}; - while (std::cin.read(reinterpret_cast(header.data()), header.size())) - { - // Local pipe framing only: byte count + clip-relative device time, both little-endian. - const auto size = little_endian(header.data(), 4); - const auto raw_time = little_endian(header.data() + 4, 8); - if (size == 0 || size > max_flatbuffer_size || raw_time > INT64_MAX) - { - throw std::invalid_argument("Invalid SOMA body demo packet header"); - } - std::vector bytes(size); - if (!std::cin.read(reinterpret_cast(bytes.data()), bytes.size())) - { - throw std::invalid_argument("Truncated SOMA body demo payload"); - } - flatbuffers::Verifier verifier(bytes.data(), bytes.size()); - const bool valid = joint_poses ? verifier.VerifyBuffer(nullptr) : - verifier.VerifyBuffer(nullptr); - if (!valid) - { - throw std::invalid_argument("Invalid SOMA body FlatBuffer"); - } - if (!arguments.validate_only) - { - if (!pusher) - { - session = std::make_unique( - "SomaBodyDemoPublisher", core::SchemaPusher::get_required_extensions(), false); - pusher = std::make_unique( - session->get_handles(), core::SchemaPusherConfig{ .collection_id = "soma_body_demo", - .max_flatbuffer_size = max_flatbuffer_size, - .tensor_identifier = tensor_identifier, - .localized_name = "SOMA Body Demo" }); - } - pusher->push_buffer( - bytes.data(), bytes.size(), core::os_monotonic_now_ns(), static_cast(raw_time)); - } - ++frames; - } - if (std::cin.gcount() != 0) - { - throw std::invalid_argument("Truncated SOMA body demo packet header"); - } - logger->info("{} {} SOMA body demo frames", arguments.validate_only ? "Validated" : "Published", frames); - return 0; - } - catch (const std::exception& error) - { - logger->error("{}", error.what()); - return 1; - } -} diff --git a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py index 1dcd7af2ff..f6eae04f17 100644 --- a/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py +++ b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py @@ -6,15 +6,16 @@ import argparse import logging import math -import signal -import struct -import subprocess import sys import time +from contextlib import ExitStack from pathlib import Path import numpy as np +from isaaccapture.deviceio_session import DeviceIOSession +from isaaccapture.deviceio_trackers import TensorPushTracker +from isaaccapture.oxr import OpenXRSession from isaaccapture.schema import ( Point, Pose, @@ -139,9 +140,6 @@ def soma_joint_poses(positions, orientations): def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument( - "--pusher", required=True, type=Path, help="Built soma_body_pusher executable" - ) parser.add_argument("--rate", type=float, default=30.0) parser.add_argument("--loop", action="store_true") parser.add_argument( @@ -153,7 +151,7 @@ def main(argv: list[str]) -> int: parser.add_argument( "--validate-only", action="store_true", - help="Validate FBS packets without OpenXR", + help="Encode demo frames and check transport capacity without OpenXR", ) args = parser.parse_args(argv[1:]) if not math.isfinite(args.rate) or args.rate <= 0: @@ -165,16 +163,29 @@ def main(argv: list[str]) -> int: frame_values = positions, orientations else: frame_values = q, t - command = [ - str(args.pusher.resolve()), - "--body-representation", - args.body_representation, - ] + (["--validate-only"] if args.validate_only else []) - with subprocess.Popen(command, stdin=subprocess.PIPE) as process: - interrupted = False - try: + joint_poses = args.body_representation == "joint-poses" + max_payload_size = 4096 if joint_poses else 2048 + frame = 0 + try: + with ExitStack() as stack: + session = None + if not args.validate_only: + tracker = TensorPushTracker( + "soma_body_demo", + "soma_body_joint_poses" + if joint_poses + else "soma_body_joint_rotations", + max_payload_size, + ) + trackers = [tracker] + extensions = DeviceIOSession.get_required_extensions(trackers) + oxr_session = stack.enter_context( + OpenXRSession("SomaBodyDemoPublisher", extensions) + ) + session = stack.enter_context( + DeviceIOSession.run(trackers, oxr_session.get_handles()) + ) start = time.monotonic() - frame = 0 while True: for first, second in zip(*frame_values): remaining = start + frame / args.rate - time.monotonic() @@ -184,21 +195,21 @@ def main(argv: list[str]) -> int: payload = soma_joint_poses(first, second).to_bytes() else: payload = soma_joint_rotations(first, second).to_bytes() - process.stdin.write( - struct.pack(" max_payload_size: + raise ValueError("SOMA body payload exceeds transport capacity") + if session is not None: + session.update() + tracker.push(session, payload) frame += 1 if not args.loop or args.validate_only: break - except KeyboardInterrupt: - interrupted = True - finally: - process.stdin.close() - exit_code = process.wait() - if exit_code != 0 and not (interrupted and exit_code == -signal.SIGINT): - raise RuntimeError("SOMA body pusher failed; see its runtime diagnostics") + except KeyboardInterrupt: + pass + logger.info( + "%s %d SOMA body demo frames", + "Encoded" if args.validate_only else "Published", + frame, + ) return 0 diff --git a/examples/soma_hand_publisher/CMakeLists.txt b/examples/soma_hand_publisher/CMakeLists.txt index 1417a90271..f80bf09a98 100644 --- a/examples/soma_hand_publisher/CMakeLists.txt +++ b/examples/soma_hand_publisher/CMakeLists.txt @@ -3,7 +3,5 @@ cmake_minimum_required(VERSION 3.20) -add_subdirectory(cpp/soma_hand_pusher) - include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) install_python_example(DESTINATION examples/soma_hand_publisher) diff --git a/examples/soma_hand_publisher/README.md b/examples/soma_hand_publisher/README.md index 31a6d414cb..115994afed 100644 --- a/examples/soma_hand_publisher/README.md +++ b/examples/soma_hand_publisher/README.md @@ -10,10 +10,9 @@ This is a deterministic input generator for testing SOMA hand consumers. It is not a vendor plugin or vendor network protocol. The Python process expresses both hands through the pinned SOMA-X v0.3.1 -`SOMAHandLayer`, then writes paired FlatBuffers to `soma_hand_pusher`. The -native process validates each payload and publishes independent -`soma_hand_left_demo` and `soma_hand_right_demo` collections on one OpenXR -session. +`SOMAHandLayer`, then publishes through `TensorPushTracker`, which wraps +`SchemaPusher`. Independent `soma_hand_left_demo` and `soma_hand_right_demo` +collections share one OpenXR session. Each payload uses explicitly keyed SOMA joints. The bundled motion provides all 25 joints per hand; a partial producer can omit joints it does not provide. @@ -21,15 +20,14 @@ Each payload uses explicitly keyed SOMA joints. The bundled motion provides all ```text SOMA-X example_animation.npy -> left/right SOMAHandLayer controls and evaluated poses - -> soma_hand_pusher + -> left/right TensorPushTracker -> left/right SchemaPusher collections ``` -Install the example and build the native pusher: +Install the example: ```bash uv pip install -e ./examples/soma_hand_publisher -cmake --build ./build --target soma_hand_pusher ``` Start the DeviceIO viewer with SOMA hands, connect its client, then run the @@ -42,7 +40,6 @@ uv run --no-sync python -m isaaccapture_examples.deviceio_live_view \ source ~/.cloudxr/run/cloudxr.env uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ - --pusher ./build/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher \ --loop ``` @@ -51,5 +48,5 @@ Joint rotations are the default. To publish evaluated joint poses, add `--soma-hand-representation joint-poses` to the viewer. The two modes publish `soma_hand_joint_rotations` and `soma_hand_joint_poses`, respectively. -Add `--validate-only` to process every paired frame through the native -FlatBuffer verifier without starting OpenXR. +Add `--validate-only` to encode every paired frame and check transport capacity +without starting OpenXR. diff --git a/examples/soma_hand_publisher/cpp/soma_hand_pusher/CMakeLists.txt b/examples/soma_hand_publisher/cpp/soma_hand_pusher/CMakeLists.txt deleted file mode 100644 index cfead1828b..0000000000 --- a/examples/soma_hand_publisher/cpp/soma_hand_pusher/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -find_package(Threads REQUIRED) - -add_executable(soma_hand_pusher soma_hand_pusher.cpp) -target_link_libraries(soma_hand_pusher PRIVATE - pusherio::pusherio - oxr::oxr_core - isaacteleop_schema - Threads::Threads -) -install(TARGETS soma_hand_pusher - RUNTIME DESTINATION examples/soma_hand_publisher/cpp -) diff --git a/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp b/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp deleted file mode 100644 index c11847fc6d..0000000000 --- a/examples/soma_hand_publisher/cpp/soma_hand_pusher/soma_hand_pusher.cpp +++ /dev/null @@ -1,182 +0,0 @@ -// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -// SPDX-License-Identifier: Apache-2.0 - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace -{ - -enum class HandRepresentation -{ - JointRotations, - JointPoses, -}; - -struct Arguments -{ - bool validate_only = false; - HandRepresentation representation = HandRepresentation::JointRotations; -}; - -Arguments parse_arguments(int argc, char** argv) -{ - Arguments arguments; - for (int index = 1; index < argc; ++index) - { - const std::string argument = argv[index]; - if (argument == "--validate-only") - { - arguments.validate_only = true; - } - else if (argument == "--hand-representation" && index + 1 < argc) - { - const std::string value = argv[++index]; - if (value == "joint-rotations") - { - arguments.representation = HandRepresentation::JointRotations; - } - else if (value == "joint-poses") - { - arguments.representation = HandRepresentation::JointPoses; - } - else - { - throw std::invalid_argument("Unknown SOMA hand representation: " + value); - } - } - else - { - throw std::invalid_argument("Unknown or incomplete argument: " + argument); - } - } - return arguments; -} - -uint64_t little_endian(const uint8_t* bytes, size_t size) -{ - uint64_t value = 0; - for (size_t index = 0; index < size; ++index) - { - value |= static_cast(bytes[index]) << (8 * index); - } - return value; -} - -template -const Table* verify(const std::vector& bytes) -{ - flatbuffers::Verifier verifier(bytes.data(), bytes.size()); - if (!verifier.VerifyBuffer
(nullptr)) - { - throw std::invalid_argument("Invalid SOMA hand FlatBuffer"); - } - return flatbuffers::GetRoot
(bytes.data()); -} - -} // namespace - -int main(int argc, char** argv) -{ - if (argc == 2 && std::string(argv[1]) == "--help") - { - std::cout << "Usage: soma_hand_pusher [--hand-representation joint-rotations|joint-poses] " - "[--validate-only]\n" - "Reads paired, framed SOMA hand FlatBuffers from stdin.\n"; - return 0; - } - auto logger = isaaccapture::Logger::get("isaaccapture.examples.soma_hand_pusher"); - try - { - const auto arguments = parse_arguments(argc, argv); - const bool joint_poses = arguments.representation == HandRepresentation::JointPoses; - const size_t max_flatbuffer_size = joint_poses ? 2048 : 1024; - const char* tensor_identifier = joint_poses ? "soma_hand_joint_poses" : "soma_hand_joint_rotations"; - std::unique_ptr session; - std::unique_ptr left_pusher; - std::unique_ptr right_pusher; - size_t frames = 0; - std::array header{}; - while (std::cin.read(reinterpret_cast(header.data()), header.size())) - { - const auto left_size = little_endian(header.data(), 4); - const auto right_size = little_endian(header.data() + 4, 4); - const auto raw_time = little_endian(header.data() + 8, 8); - if (left_size == 0 || left_size > max_flatbuffer_size || right_size == 0 || - right_size > max_flatbuffer_size || raw_time > INT64_MAX) - { - throw std::invalid_argument("Invalid SOMA hand demo packet header"); - } - std::vector left_bytes(left_size); - std::vector right_bytes(right_size); - if (!std::cin.read(reinterpret_cast(left_bytes.data()), left_bytes.size()) || - !std::cin.read(reinterpret_cast(right_bytes.data()), right_bytes.size())) - { - throw std::invalid_argument("Truncated SOMA hand demo payload"); - } - - core::SomaHandedness left_handedness; - core::SomaHandedness right_handedness; - if (joint_poses) - { - left_handedness = verify(left_bytes)->handedness(); - right_handedness = verify(right_bytes)->handedness(); - } - else - { - left_handedness = verify(left_bytes)->handedness(); - right_handedness = verify(right_bytes)->handedness(); - } - if (left_handedness != core::SomaHandedness_LEFT || right_handedness != core::SomaHandedness_RIGHT) - { - throw std::invalid_argument("SOMA hand demo payload handedness does not match its collection"); - } - - if (!arguments.validate_only) - { - if (!left_pusher) - { - session = std::make_unique( - "SomaHandDemoPublisher", core::SchemaPusher::get_required_extensions(), false); - left_pusher = std::make_unique( - session->get_handles(), core::SchemaPusherConfig{ .collection_id = "soma_hand_left_demo", - .max_flatbuffer_size = max_flatbuffer_size, - .tensor_identifier = tensor_identifier, - .localized_name = "SOMA Left Hand Demo" }); - right_pusher = std::make_unique( - session->get_handles(), core::SchemaPusherConfig{ .collection_id = "soma_hand_right_demo", - .max_flatbuffer_size = max_flatbuffer_size, - .tensor_identifier = tensor_identifier, - .localized_name = "SOMA Right Hand Demo" }); - } - const auto now = core::os_monotonic_now_ns(); - left_pusher->push_buffer(left_bytes.data(), left_bytes.size(), now, static_cast(raw_time)); - right_pusher->push_buffer(right_bytes.data(), right_bytes.size(), now, static_cast(raw_time)); - } - ++frames; - } - if (std::cin.gcount() != 0) - { - throw std::invalid_argument("Truncated SOMA hand demo packet header"); - } - logger->info("{} {} paired SOMA hand demo frames", arguments.validate_only ? "Validated" : "Published", frames); - return 0; - } - catch (const std::exception& error) - { - logger->error("{}", error.what()); - return 1; - } -} diff --git a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py index ffdf26a97d..a7e8e329bf 100644 --- a/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py +++ b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py @@ -6,15 +6,16 @@ import argparse import logging import math -import signal -import struct -import subprocess import sys import time +from contextlib import ExitStack from pathlib import Path import numpy as np +from isaaccapture.deviceio_session import DeviceIOSession +from isaaccapture.deviceio_trackers import TensorPushTracker +from isaaccapture.oxr import OpenXRSession from isaaccapture.schema import ( Point, Pose, @@ -179,9 +180,6 @@ def _payload(frame, index: int, representation: str, handedness): def main(argv: list[str]) -> int: parser = argparse.ArgumentParser(description=__doc__) - parser.add_argument( - "--pusher", required=True, type=Path, help="Built soma_hand_pusher executable" - ) parser.add_argument("--rate", type=float, default=30.0) parser.add_argument("--loop", action="store_true") parser.add_argument( @@ -193,7 +191,7 @@ def main(argv: list[str]) -> int: parser.add_argument( "--validate-only", action="store_true", - help="Validate FBS packets without OpenXR", + help="Encode demo frames and check transport capacity without OpenXR", ) args = parser.parse_args(argv[1:]) if not math.isfinite(args.rate) or args.rate <= 0: @@ -203,16 +201,33 @@ def main(argv: list[str]) -> int: frames = demo_hand_frames( body.data_root / "example_animation.npy", body, hand_layers ) - command = [ - str(args.pusher.resolve()), - "--hand-representation", - args.hand_representation, - ] + (["--validate-only"] if args.validate_only else []) - with subprocess.Popen(command, stdin=subprocess.PIPE) as process: - interrupted = False - try: + joint_poses = args.hand_representation == "joint-poses" + max_payload_size = 2048 if joint_poses else 1024 + frame_number = 0 + try: + with ExitStack() as stack: + session = None + if not args.validate_only: + tensor_identifier = ( + "soma_hand_joint_poses" + if joint_poses + else "soma_hand_joint_rotations" + ) + left_tracker = TensorPushTracker( + "soma_hand_left_demo", tensor_identifier, max_payload_size + ) + right_tracker = TensorPushTracker( + "soma_hand_right_demo", tensor_identifier, max_payload_size + ) + trackers = [left_tracker, right_tracker] + extensions = DeviceIOSession.get_required_extensions(trackers) + oxr_session = stack.enter_context( + OpenXRSession("SomaHandDemoPublisher", extensions) + ) + session = stack.enter_context( + DeviceIOSession.run(trackers, oxr_session.get_handles()) + ) start = time.monotonic() - frame_number = 0 while True: for index in range(len(frames["left"]["positions"])): remaining = start + frame_number / args.rate - time.monotonic() @@ -230,27 +245,22 @@ def main(argv: list[str]) -> int: args.hand_representation, SomaHandedness.RIGHT, ) - process.stdin.write( - struct.pack( - " max_payload_size or len(right) > max_payload_size: + raise ValueError("SOMA hand payload exceeds transport capacity") + if session is not None: + session.update() + left_tracker.push(session, left) + right_tracker.push(session, right) frame_number += 1 if not args.loop or args.validate_only: break - except KeyboardInterrupt: - interrupted = True - finally: - process.stdin.close() - exit_code = process.wait() - if exit_code != 0 and not (interrupted and exit_code == -signal.SIGINT): - raise RuntimeError("SOMA hand pusher failed; see its runtime diagnostics") + except KeyboardInterrupt: + pass + logger.info( + "%s %d paired SOMA hand demo frames", + "Encoded" if args.validate_only else "Published", + frame_number, + ) return 0 diff --git a/tests/python/examples/soma_body_publisher/CMakeLists.txt b/tests/python/examples/soma_body_publisher/CMakeLists.txt index fa33402f98..ed5bb89e88 100644 --- a/tests/python/examples/soma_body_publisher/CMakeLists.txt +++ b/tests/python/examples/soma_body_publisher/CMakeLists.txt @@ -12,7 +12,6 @@ foreach(test_file ${TEST_FILES}) NAME "soma_body_publisher_${test_name}" COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev pytest -v --tb=short "${test_file}" - --soma-pusher $ WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" ) set_tests_properties("soma_body_publisher_${test_name}" PROPERTIES diff --git a/tests/python/examples/soma_body_publisher/conftest.py b/tests/python/examples/soma_body_publisher/conftest.py index a432ae052a..4e5db394a4 100644 --- a/tests/python/examples/soma_body_publisher/conftest.py +++ b/tests/python/examples/soma_body_publisher/conftest.py @@ -15,7 +15,6 @@ def pytest_addoption(parser): parser.addoption("--soma-assets", type=Path, help="SOMA-X assets directory") - parser.addoption("--soma-pusher", type=Path, help="Built soma_body_pusher") @pytest.fixture(scope="module") diff --git a/tests/python/examples/soma_body_publisher/test_publisher.py b/tests/python/examples/soma_body_publisher/test_publisher.py index 9d1c9f8083..ff0fbc05fc 100644 --- a/tests/python/examples/soma_body_publisher/test_publisher.py +++ b/tests/python/examples/soma_body_publisher/test_publisher.py @@ -1,9 +1,6 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -import signal -import struct -import subprocess import sys from types import SimpleNamespace from unittest.mock import MagicMock @@ -66,72 +63,161 @@ def test_layer_uses_upstream_asset_cache(monkeypatch, tmp_path): layer.prepare_identity.assert_called_once() -@pytest.mark.parametrize("exit_code", [0, -signal.SIGINT, 1]) -def test_publisher_interrupt_preserves_real_failures(monkeypatch, tmp_path, exit_code): - process = MagicMock() - process.__enter__.return_value = process - process.stdin.write.side_effect = KeyboardInterrupt - process.wait.return_value = exit_code - monkeypatch.setattr(publisher.subprocess, "Popen", lambda *args, **kwargs: process) +@pytest.fixture +def demo(monkeypatch, tmp_path): monkeypatch.setattr( publisher, "create_layer", lambda: SimpleNamespace(data_root=tmp_path) ) monkeypatch.setattr( publisher, "demo_controls", - lambda *args: (np.tile([0, 0, 0, 1], (1, 77, 1)), [[0, 0, 0]], None), + lambda *args: (np.tile([0, 0, 0, 1], (2, 77, 1)), [[0, 0, 0], [1, 2, 3]], None), ) - args = ["soma_body_publisher", "--pusher", "pusher"] - if exit_code == 1: - with pytest.raises(RuntimeError, match="pusher failed"): - publisher.main(args) + monkeypatch.setattr( + publisher, + "evaluate_demo_controls", + lambda *args: (np.zeros((2, 77, 3)), np.tile([0, 0, 0, 1], (2, 77, 1))), + ) + session = MagicMock() + session.__enter__.return_value = session + oxr_session = MagicMock() + oxr_session.__enter__.return_value = oxr_session + deviceio = MagicMock() + deviceio.run.return_value = session + oxr = MagicMock(return_value=oxr_session) + tracker = MagicMock() + tracker_factory = MagicMock(return_value=tracker) + monkeypatch.setattr(publisher, "DeviceIOSession", deviceio) + monkeypatch.setattr(publisher, "OpenXRSession", oxr) + monkeypatch.setattr(publisher, "TensorPushTracker", tracker_factory) + monkeypatch.setattr(publisher.time, "monotonic", lambda: 0.0) + sleep = MagicMock() + monkeypatch.setattr(publisher.time, "sleep", sleep) + return SimpleNamespace( + session=session, + oxr_session=oxr_session, + deviceio=deviceio, + oxr=oxr, + tracker=tracker, + tracker_factory=tracker_factory, + sleep=sleep, + ) + + +@pytest.mark.parametrize( + "representation,capacity", [("joint-rotations", 2048), ("joint-poses", 4096)] +) +def test_publisher_pushes_selected_schema_in_process(demo, representation, capacity): + assert publisher.main(["publisher", "--body-representation", representation]) == 0 + demo.tracker_factory.assert_called_once_with( + "soma_body_demo", "soma_body_" + representation.replace("-", "_"), capacity + ) + demo.deviceio.get_required_extensions.assert_called_once_with([demo.tracker]) + demo.oxr.assert_called_once_with( + "SomaBodyDemoPublisher", demo.deviceio.get_required_extensions.return_value + ) + demo.deviceio.run.assert_called_once_with( + [demo.tracker], demo.oxr_session.get_handles.return_value + ) + assert demo.session.update.call_count == 2 + assert demo.tracker.push.call_count == 2 + if representation == "joint-rotations": + expected = soma_joint_rotations( + np.tile([0, 0, 0, 1], (77, 1)), [1, 2, 3] + ).to_bytes() else: - assert publisher.main(args) == 0 - process.stdin.close.assert_called_once() + expected = soma_joint_poses( + np.zeros((77, 3)), np.tile([0, 0, 0, 1], (77, 1)) + ).to_bytes() + demo.tracker.push.assert_called_with(demo.session, expected) + demo.sleep.assert_called_once_with(1 / 30) + demo.session.__exit__.assert_called_once() + demo.oxr_session.__exit__.assert_called_once() + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +def test_validate_only_does_not_open_runtime_or_loop(demo, representation): + assert ( + publisher.main( + [ + "publisher", + "--body-representation", + representation, + "--validate-only", + "--loop", + ] + ) + == 0 + ) + demo.tracker_factory.assert_not_called() + demo.deviceio.run.assert_not_called() + demo.oxr.assert_not_called() + demo.sleep.assert_not_called() + + +@pytest.mark.parametrize("error", [KeyboardInterrupt, RuntimeError]) +def test_publisher_closes_sessions_and_preserves_push_failures(demo, error): + demo.tracker.push.side_effect = error("push stopped") + if error is KeyboardInterrupt: + assert publisher.main(["publisher"]) == 0 + else: + with pytest.raises(RuntimeError, match="push stopped"): + publisher.main(["publisher"]) + demo.session.__exit__.assert_called_once() + demo.oxr_session.__exit__.assert_called_once() + + +def test_session_initialization_failure_closes_openxr(demo): + demo.deviceio.run.side_effect = RuntimeError("session failed") + with pytest.raises(RuntimeError, match="session failed"): + publisher.main(["publisher"]) + demo.oxr_session.__exit__.assert_called_once() + demo.tracker.push.assert_not_called() + + +def test_publisher_loop_repeats_clip_until_interrupted(demo): + demo.tracker.push.side_effect = [None, None, KeyboardInterrupt] + assert publisher.main(["publisher", "--loop"]) == 0 + assert demo.tracker.push.call_count == 3 + assert ( + demo.tracker.push.call_args_list[0].args[1] + == demo.tracker.push.call_args_list[2].args[1] + ) @pytest.mark.parametrize( - "packet", - [b"x", struct.pack(" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" ) set_tests_properties("soma_hand_publisher_${test_name}" PROPERTIES diff --git a/tests/python/examples/soma_hand_publisher/conftest.py b/tests/python/examples/soma_hand_publisher/conftest.py index 8373623d92..03612477c1 100644 --- a/tests/python/examples/soma_hand_publisher/conftest.py +++ b/tests/python/examples/soma_hand_publisher/conftest.py @@ -15,9 +15,6 @@ def pytest_addoption(parser): parser.addoption("--soma-assets", type=Path, help="SOMA-X assets directory") - parser.addoption( - "--soma-hand-pusher", type=Path, help="Built soma_hand_pusher executable" - ) @pytest.fixture(scope="module") diff --git a/tests/python/examples/soma_hand_publisher/test_publisher.py b/tests/python/examples/soma_hand_publisher/test_publisher.py index 3faa72fc85..4d2a2c5a21 100644 --- a/tests/python/examples/soma_hand_publisher/test_publisher.py +++ b/tests/python/examples/soma_hand_publisher/test_publisher.py @@ -1,8 +1,6 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -import struct -import subprocess import sys from types import SimpleNamespace from unittest.mock import MagicMock @@ -48,50 +46,169 @@ def test_layers_resolve_assets_without_explicit_path(monkeypatch, tmp_path): assert set(hands) == {"left", "right"} -@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) -def test_publisher_uses_resolved_demo_without_asset_argument( - monkeypatch, tmp_path, representation -): +@pytest.fixture +def demo(monkeypatch, tmp_path): body = SimpleNamespace(data_root=tmp_path) layers = MagicMock(return_value=(body, {})) frame = { - "rotations": np.tile([0, 0, 0, 1], (1, 25, 1)), - "translation": np.zeros((1, 3)), - "positions": np.zeros((1, 25, 3)), - "orientations": np.tile([0, 0, 0, 1], (1, 25, 1)), + "rotations": np.tile([0, 0, 0, 1], (2, 25, 1)), + "translation": np.zeros((2, 3)), + "positions": np.zeros((2, 25, 3)), + "orientations": np.tile([0, 0, 0, 1], (2, 25, 1)), } - demo = MagicMock(return_value={"left": frame, "right": frame}) - process = MagicMock() - process.__enter__.return_value = process - process.wait.return_value = 0 - popen = MagicMock(return_value=process) + frames = MagicMock(return_value={"left": frame, "right": frame}) + session = MagicMock() + session.__enter__.return_value = session + oxr_session = MagicMock() + oxr_session.__enter__.return_value = oxr_session + deviceio = MagicMock() + deviceio.run.return_value = session + oxr = MagicMock(return_value=oxr_session) + left, right = MagicMock(), MagicMock() + tracker_factory = MagicMock(side_effect=[left, right]) monkeypatch.setattr(publisher, "create_layers", layers) - monkeypatch.setattr(publisher, "demo_hand_frames", demo) - monkeypatch.setattr(publisher.subprocess, "Popen", popen) + monkeypatch.setattr(publisher, "demo_hand_frames", frames) + monkeypatch.setattr(publisher, "DeviceIOSession", deviceio) + monkeypatch.setattr(publisher, "OpenXRSession", oxr) + monkeypatch.setattr(publisher, "TensorPushTracker", tracker_factory) + monkeypatch.setattr(publisher.time, "monotonic", lambda: 0.0) + sleep = MagicMock() + monkeypatch.setattr(publisher.time, "sleep", sleep) + return SimpleNamespace( + session=session, + oxr_session=oxr_session, + deviceio=deviceio, + oxr=oxr, + left=left, + right=right, + tracker_factory=tracker_factory, + sleep=sleep, + layers=layers, + frames=frames, + body=body, + frame=frame, + ) + +@pytest.mark.parametrize( + "representation,capacity", [("joint-rotations", 1024), ("joint-poses", 2048)] +) +def test_publisher_pushes_each_hand_through_one_session(demo, representation, capacity): assert ( publisher.main( [ "soma_hand_publisher", - "--pusher", - "pusher", "--hand-representation", representation, - "--validate-only", ] ) == 0 ) - layers.assert_called_once_with() - demo.assert_called_once_with(tmp_path / "example_animation.npy", body, {}) - assert popen.call_args.args[0][1:] == [ - "--hand-representation", - representation, - "--validate-only", + demo.layers.assert_called_once_with() + demo.frames.assert_called_once_with( + demo.body.data_root / "example_animation.npy", demo.body, {} + ) + identifier = "soma_hand_" + representation.replace("-", "_") + assert [call.args for call in demo.tracker_factory.call_args_list] == [ + ("soma_hand_left_demo", identifier, capacity), + ("soma_hand_right_demo", identifier, capacity), ] - assert process.stdin.write.call_count == 3 - process.stdin.close.assert_called_once() + demo.deviceio.get_required_extensions.assert_called_once_with( + [demo.left, demo.right] + ) + demo.oxr.assert_called_once_with( + "SomaHandDemoPublisher", demo.deviceio.get_required_extensions.return_value + ) + demo.deviceio.run.assert_called_once_with( + [demo.left, demo.right], demo.oxr_session.get_handles.return_value + ) + assert demo.session.update.call_count == 2 + for tracker, handedness in ( + (demo.left, SomaHandedness.LEFT), + (demo.right, SomaHandedness.RIGHT), + ): + assert tracker.push.call_count == 2 + tracker.push.assert_called_with( + demo.session, publisher._payload(demo.frame, 1, representation, handedness) + ) + demo.sleep.assert_called_once_with(1 / 30) + demo.session.__exit__.assert_called_once() + demo.oxr_session.__exit__.assert_called_once() + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +def test_validate_only_does_not_open_runtime_or_loop(demo, representation): + assert ( + publisher.main( + [ + "publisher", + "--hand-representation", + representation, + "--validate-only", + "--loop", + ] + ) + == 0 + ) + demo.tracker_factory.assert_not_called() + demo.deviceio.run.assert_not_called() + demo.oxr.assert_not_called() + demo.sleep.assert_not_called() + + +@pytest.mark.parametrize("error", [KeyboardInterrupt, RuntimeError]) +def test_publisher_closes_sessions_and_preserves_push_failures(demo, error): + demo.right.push.side_effect = error("push stopped") + if error is KeyboardInterrupt: + assert publisher.main(["publisher"]) == 0 + else: + with pytest.raises(RuntimeError, match="push stopped"): + publisher.main(["publisher"]) + demo.session.__exit__.assert_called_once() + demo.oxr_session.__exit__.assert_called_once() + + +def test_session_initialization_failure_closes_openxr(demo): + demo.deviceio.run.side_effect = RuntimeError("session failed") + with pytest.raises(RuntimeError, match="session failed"): + publisher.main(["publisher"]) + demo.oxr_session.__exit__.assert_called_once() + demo.left.push.assert_not_called() + demo.right.push.assert_not_called() + + +def test_publisher_loop_repeats_clip_until_interrupted(demo): + demo.right.push.side_effect = [None, None, KeyboardInterrupt] + assert publisher.main(["publisher", "--loop"]) == 0 + assert demo.right.push.call_count == 3 + assert ( + demo.right.push.call_args_list[0].args[1] + == demo.right.push.call_args_list[2].args[1] + ) + + +@pytest.mark.parametrize( + "representation,capacity", [("joint-rotations", 1024), ("joint-poses", 2048)] +) +def test_validate_only_rejects_oversized_payload( + demo, monkeypatch, representation, capacity +): + monkeypatch.setattr(publisher, "_payload", lambda *args: bytes(capacity + 1)) + with pytest.raises(ValueError, match="exceeds transport capacity"): + publisher.main( + ["publisher", "--hand-representation", representation, "--validate-only"] + ) + + +@pytest.mark.parametrize("rate", ["0", "-1", "nan", "inf"]) +def test_invalid_rate_is_rejected_before_loading_model(monkeypatch, rate): + layers = MagicMock() + monkeypatch.setattr(publisher, "create_layers", layers) + with pytest.raises(SystemExit) as error: + publisher.main(["publisher", "--rate", rate]) + assert error.value.code == 2 + layers.assert_not_called() def test_hand_payload_constructors_preserve_side_and_arrays(): @@ -112,27 +229,7 @@ def test_hand_payload_constructors_preserve_side_and_arrays(): assert pose_valid.all() -def test_pusher_rejects_collection_handedness_mismatch(request): - executable = request.config.getoption("--soma-hand-pusher") - if executable is None: - return - rotations = np.tile([0.0, 0.0, 0.0, 1.0], (25, 1)) - wrong_left = soma_joint_rotations( - rotations, [0, 0, 0], SomaHandedness.RIGHT - ).to_bytes() - right = soma_joint_rotations(rotations, [0, 0, 0], SomaHandedness.RIGHT).to_bytes() - packet = struct.pack(" Date: Thu, 8 Oct 2026 17:20:52 -0400 Subject: [PATCH 12/12] docs: group general device guides before SOMA Signed-off-by: Michael Haselton --- docs/source/index.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/index.rst b/docs/source/index.rst index bbf1aa717f..87f853aaba 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -61,8 +61,8 @@ Table of Contents :caption: Devices device/trackers - device/soma device/add_device + device/soma device/joint_space device/body_tracking device/haptic_feedback