diff --git a/docs/source/device/soma.rst b/docs/source/device/soma.rst new file mode 100644 index 0000000000..ae7299cfef --- /dev/null +++ b/docs/source/device/soma.rst @@ -0,0 +1,244 @@ +.. 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 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 +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. + +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 +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 +------------------ + +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: + +.. 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 \ + -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 +-------------------- + +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 + +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 \ + --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 + +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 \ + --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 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. + +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. + +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/device/trackers.rst b/docs/source/device/trackers.rst index b5568bb8e7..faec8789c3 100644 --- a/docs/source/device/trackers.rst +++ b/docs/source/device/trackers.rst @@ -24,6 +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:`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: @@ -269,6 +273,109 @@ 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 trackers +~~~~~~~~~~~~~~~~~~~~~ + +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 +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 ``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 +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.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``, ``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_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` + +SomaHandJointRotationsTracker and SomaHandJointPosesTracker +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Reads either of the two SOMA hand transport profiles defined and verified against SOMA-X v0.3.1. +``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 ``SomaBodyJointRotations``. + +When a producer has already evaluated forward kinematics, it can instead publish +``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``. + +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.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``, ``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_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/index.rst b/docs/source/index.rst index 31cda57dd8..87f853aaba 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -62,6 +62,7 @@ Table of Contents device/trackers device/add_device + device/soma device/joint_space device/body_tracking device/haptic_feedback diff --git a/docs/source/references/generated_trackers.rst b/docs/source/references/generated_trackers.rst index 6f916b9ec0..796675edd2 100644 --- a/docs/source/references/generated_trackers.rst +++ b/docs/source/references/generated_trackers.rst @@ -89,6 +89,18 @@ Generated today * - ``se3_tracker`` - ``Se3Tracker`` - Overrides ``class``; the default would give ``Se3TrackerTracker`` + * - ``soma_body_joint_rotations`` + - ``SomaBodyJointRotationsTracker`` + - 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`` + - Keyed evaluated SOMA positions and orientations for up to 77 public joints + * - ``soma_hand_joint_rotations`` + - ``SomaHandJointRotationsTracker`` + - Keyed SOMA hand rotations for up to 25 controls; defined against SOMA-X v0.3.1 + * - ``soma_hand_joint_poses`` + - ``SomaHandJointPosesTracker`` + - 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/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 63649e7f85..ec8783659c 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 ``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`. Available Retargeters --------------------- diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 57cadc7c90..9b96fea7f9 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -17,6 +17,8 @@ if(BUILD_EXAMPLES) add_subdirectory(native_openxr) 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 5fce01d879..949f86aaa0 100644 --- a/examples/deviceio_live_view/README.md +++ b/examples/deviceio_live_view/README.md @@ -12,10 +12,102 @@ 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 +SomaBodyJointRotations -> tracker -> upstream FK --+ + +-> SomaBodyInput -> viewer +SomaBodyJointPoses ----> tracker -> direct map ----+ +``` + +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 +``` + +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`. 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. +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 `SomaBodyJointRotations` or +`SomaBodyJointPoses`. The raw tracker remains the transport and recording +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 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. + +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. +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 +SomaHandJointRotations -> tracker -> upstream FK --+ + +-> SomaHandInput -> viewer +SomaHandJointPoses ----> tracker -> direct map -------+ +``` + +```bash +uv run --no-sync python -m isaaccapture_examples.deviceio_live_view --accept-eula \ + --hand-schema soma +``` + +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`. + +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: + +```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..48ba0083f2 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/body_pipeline.py @@ -0,0 +1,57 @@ +# 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 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) -> BodySchema: + return ( + BodySchema(body_schema) + if body_schema is not None + else BodySchema.FULL_BODY_POSE + ) + + +def create_body_view_pipeline( + *, + 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) + 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 + + 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 new file mode 100644 index 0000000000..1621dd933f --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/deviceio_pipeline.py @@ -0,0 +1,42 @@ +# 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, + HeadSource, +) +from isaaccapture.retargeting_engine.interface import OutputCombiner + +from .body_pipeline import BodyViewPipeline, create_body_view_pipeline +from .hand_pipeline import HandViewPipeline, create_hand_view_pipeline + + +def build_all_human_pipeline( + *, + body_schema=None, + soma_body_collection_id="soma_body_demo", + soma_body_representation="joint-rotations", + body: BodyViewPipeline | None = None, + hands: HandViewPipeline | None = None, +): + """Wire every human-related DeviceIO source into one pipeline.""" + head = HeadSource(name="head") + controllers = ControllersSource(name="controllers") + selected_body = body or create_body_view_pipeline( + body_schema=body_schema, + soma_body_collection_id=soma_body_collection_id, + soma_body_representation=soma_body_representation, + ) + selected_hands = hands or create_hand_view_pipeline() + return OutputCombiner( + { + "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), + "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..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 @@ -1,36 +1,29 @@ # 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``. +Rendering helpers: ``BODY_BONES``, ``controller_state``. """ 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 +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 # --------------------------------------------------------------------------- # Color palette shared across all viz scripts @@ -110,109 +103,14 @@ 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. -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) -> 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: @@ -277,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: @@ -428,22 +328,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 +352,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 +369,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 +418,22 @@ 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, + hand_layout: HandViewLayout = OPENXR_HAND_LAYOUT, + ): self._ground = ground - self.hand_left = HandViz(server, "hand_left", LEFT_COLOR) - self.hand_right = HandViz(server, "hand_right", RIGHT_COLOR) + self._body_layout = body_layout + 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) - 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: @@ -530,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: @@ -551,34 +469,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 +509,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/hand_pipeline.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py new file mode 100644 index 0000000000..3c5589a131 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/hand_pipeline.py @@ -0,0 +1,58 @@ +# 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 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( + *, + 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) + from .soma_hand import create_soma_hand_pipeline + + return create_soma_hand_pipeline( + 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 c50022d352..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 @@ -24,15 +24,20 @@ 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, + 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: parser = argparse.ArgumentParser(description=__doc__) @@ -42,15 +47,69 @@ 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)", + ) + parser.add_argument( + "--soma-body-collection-id", + default="soma_body_demo", + help="SOMA body 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", + ) + 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:]) + try: + body_schema = resolve_body_schema(args.body_schema) + body = create_body_view_pipeline( + 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( + 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)) + 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 +118,20 @@ 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, 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() 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 +139,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/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_body.py b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py new file mode 100644 index 0000000000..551e99913b --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_body.py @@ -0,0 +1,176 @@ +# 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 SomaBodyJoint + +from .body_pipeline import BodyViewLayout, BodyViewPipeline + + +# 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", + 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( + 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 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)) + ) + return layer + + +def create_soma_body_pipeline( + 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, + layer, + representation=representation, + ) + return BodyViewPipeline( + output=source.output(SomaBodySource.BODY), + 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 new file mode 100644 index 0000000000..e9ff49c901 --- /dev/null +++ b/examples/deviceio_live_view/python/isaaccapture_examples/deviceio_live_view/soma_hand.py @@ -0,0 +1,116 @@ +# 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 SomaHandedness, SomaHandJoint + +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( + SomaHandJoint.__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(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, + 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 FBS") + with torch.no_grad(): + layer.prepare_identity( + torch.zeros((1, layer.identity_model.num_identity_coeffs)) + ) + return layer + + +def create_soma_hand_pipeline( + left_collection_id: str, + right_collection_id: str, + representation: SomaHandRepresentation, +) -> HandViewPipeline: + layers = (None, None) + if representation is SomaHandRepresentation.JOINT_ROTATIONS: + layers = (create_layer("left"), create_layer("right")) + left = SomaHandSource( + "hand_left", + left_collection_id, + SomaHandedness.LEFT, + layers[0], + representation=representation, + ) + right = SomaHandSource( + "hand_right", + right_collection_id, + SomaHandedness.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_body_publisher/CMakeLists.txt b/examples/soma_body_publisher/CMakeLists.txt new file mode 100644 index 0000000000..4d205725bf --- /dev/null +++ b/examples/soma_body_publisher/CMakeLists.txt @@ -0,0 +1,7 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +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..ee5652ee70 --- /dev/null +++ b/examples/soma_body_publisher/README.md @@ -0,0 +1,47 @@ + + +# 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 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. + +```text +SOMA-X example_animation.npy + -> Python SOMA body encoder + -> TensorPushTracker + -> SchemaPusher collection soma_body_demo +``` + +Install the example: + +```bash +uv pip install -e ./examples/soma_body_publisher +``` + +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 \ + --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` and +`soma_body_joint_poses`, respectively. + +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/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..f6eae04f17 --- /dev/null +++ b/examples/soma_body_publisher/python/isaaccapture_examples/soma_body_publisher/publisher.py @@ -0,0 +1,217 @@ +# 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 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, + Quaternion, + SomaBodyJointPose, + SomaBodyJointPoses, + 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 | 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", + 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( + 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 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 = [ + SomaBodyJointRotation(joint, Quaternion(*quaternion)) + for joint, quaternion in zip(BODY_JOINTS, quaternions, strict=True) + ] + return SomaBodyJointRotations(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 = [ + SomaBodyJointPose( + joint, + Pose(Point(*position), Quaternion(*orientation)), + ) + for joint, position, orientation in zip( + BODY_JOINTS, positions, orientations, strict=True + ) + ] + return SomaBodyJointPoses(joints) + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + 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="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: + parser.error("rate must be finite and positive") + 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 + else: + frame_values = q, t + 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() + 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() + if len(payload) > 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: + pass + logger.info( + "%s %d SOMA body demo frames", + "Encoded" if args.validate_only else "Published", + frame, + ) + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/examples/soma_hand_publisher/CMakeLists.txt b/examples/soma_hand_publisher/CMakeLists.txt new file mode 100644 index 0000000000..f80bf09a98 --- /dev/null +++ b/examples/soma_hand_publisher/CMakeLists.txt @@ -0,0 +1,7 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +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..115994afed --- /dev/null +++ b/examples/soma_hand_publisher/README.md @@ -0,0 +1,52 @@ + + +# 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 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. + +```text +SOMA-X example_animation.npy + -> left/right SOMAHandLayer controls and evaluated poses + -> left/right TensorPushTracker + -> left/right SchemaPusher collections +``` + +Install the example: + +```bash +uv pip install -e ./examples/soma_hand_publisher +``` + +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 + +source ~/.cloudxr/run/cloudxr.env +uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ + --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` and `soma_hand_joint_poses`, respectively. + +Add `--validate-only` to encode every paired frame and check transport capacity +without starting OpenXR. 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..a7e8e329bf --- /dev/null +++ b/examples/soma_hand_publisher/python/isaaccapture_examples/soma_hand_publisher/publisher.py @@ -0,0 +1,268 @@ +# 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 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, + Quaternion, + SomaHandedness, + SomaHandJoint, + SomaHandJointPose, + SomaHandJointPoses, + 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 | 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", + 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 = [ + 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 = [ + SomaHandJointPose(joint, Pose(Point(*position), Quaternion(*orientation))) + for joint, position, orientation in zip( + HAND_JOINTS, positions, orientations, strict=True + ) + ] + return SomaHandJointPoses(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("--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="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: + parser.error("rate must be finite and positive") + + body, hand_layers = create_layers() + frames = demo_hand_frames( + body.data_root / "example_animation.npy", body, hand_layers + ) + 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() + 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, + SomaHandedness.LEFT, + ) + right = _payload( + frames["right"], + index, + args.hand_representation, + SomaHandedness.RIGHT, + ) + if len(left) > 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: + pass + logger.info( + "%s %d paired SOMA hand demo frames", + "Encoded" if args.validate_only else "Published", + frame_number, + ) + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/src/core/deviceio_trackers/trackers.toml b/src/core/deviceio_trackers/trackers.toml index 47256e382c..4bb4783995 100644 --- a/src/core/deviceio_trackers/trackers.toml +++ b/src/core/deviceio_trackers/trackers.toml @@ -28,6 +28,38 @@ class = "Se3Tracker" max_flatbuffer_size = 256 facade_tensor_constant = true +[[tracker]] +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" +table = "SomaBodyJointPoses" +schema = "soma_body_joint_poses" +class = "SomaBodyJointPosesTracker" +traits = "SomaBodyJointPosesRecordingTraits" +max_flatbuffer_size = 4096 + +[[tracker]] +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" +table = "SomaHandJointPoses" +schema = "soma_hand_joint_poses" +class = "SomaHandJointPosesTracker" +traits = "SomaHandJointPosesRecordingTraits" +max_flatbuffer_size = 2048 + [[tracker]] name = "oglo_tactile" table = "OgloGloveSample" diff --git a/src/core/schema/fbs/soma_body_common.fbs b/src/core/schema/fbs/soma_body_common.fbs new file mode 100644 index 0000000000..7ad8ddebed --- /dev/null +++ b/src/core/schema/fbs/soma_body_common.fbs @@ -0,0 +1,89 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +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 transported. +// Hips is index 0, matching SOMALayer's 77-entry public pose input. +enum SomaBodyJoint : uint8 { + HIPS = 0, + SPINE1 = 1, + SPINE2 = 2, + CHEST = 3, + NECK1 = 4, + NECK2 = 5, + HEAD = 6, + HEAD_END = 7, + JAW = 8, + LEFT_EYE = 9, + RIGHT_EYE = 10, + LEFT_SHOULDER = 11, + LEFT_ARM = 12, + LEFT_FORE_ARM = 13, + LEFT_HAND = 14, + LEFT_HAND_THUMB1 = 15, + LEFT_HAND_THUMB2 = 16, + LEFT_HAND_THUMB3 = 17, + LEFT_HAND_THUMB_END = 18, + LEFT_HAND_INDEX1 = 19, + LEFT_HAND_INDEX2 = 20, + LEFT_HAND_INDEX3 = 21, + LEFT_HAND_INDEX4 = 22, + LEFT_HAND_INDEX_END = 23, + LEFT_HAND_MIDDLE1 = 24, + LEFT_HAND_MIDDLE2 = 25, + LEFT_HAND_MIDDLE3 = 26, + LEFT_HAND_MIDDLE4 = 27, + LEFT_HAND_MIDDLE_END = 28, + LEFT_HAND_RING1 = 29, + LEFT_HAND_RING2 = 30, + LEFT_HAND_RING3 = 31, + LEFT_HAND_RING4 = 32, + LEFT_HAND_RING_END = 33, + LEFT_HAND_PINKY1 = 34, + LEFT_HAND_PINKY2 = 35, + LEFT_HAND_PINKY3 = 36, + LEFT_HAND_PINKY4 = 37, + LEFT_HAND_PINKY_END = 38, + RIGHT_SHOULDER = 39, + RIGHT_ARM = 40, + RIGHT_FORE_ARM = 41, + RIGHT_HAND = 42, + RIGHT_HAND_THUMB1 = 43, + RIGHT_HAND_THUMB2 = 44, + RIGHT_HAND_THUMB3 = 45, + RIGHT_HAND_THUMB_END = 46, + RIGHT_HAND_INDEX1 = 47, + RIGHT_HAND_INDEX2 = 48, + RIGHT_HAND_INDEX3 = 49, + RIGHT_HAND_INDEX4 = 50, + RIGHT_HAND_INDEX_END = 51, + RIGHT_HAND_MIDDLE1 = 52, + RIGHT_HAND_MIDDLE2 = 53, + RIGHT_HAND_MIDDLE3 = 54, + RIGHT_HAND_MIDDLE4 = 55, + RIGHT_HAND_MIDDLE_END = 56, + RIGHT_HAND_RING1 = 57, + RIGHT_HAND_RING2 = 58, + RIGHT_HAND_RING3 = 59, + RIGHT_HAND_RING4 = 60, + RIGHT_HAND_RING_END = 61, + RIGHT_HAND_PINKY1 = 62, + RIGHT_HAND_PINKY2 = 63, + RIGHT_HAND_PINKY3 = 64, + RIGHT_HAND_PINKY4 = 65, + RIGHT_HAND_PINKY_END = 66, + LEFT_LEG = 67, + LEFT_SHIN = 68, + LEFT_FOOT = 69, + LEFT_TOE_BASE = 70, + LEFT_TOE_END = 71, + RIGHT_LEG = 72, + RIGHT_SHIN = 73, + RIGHT_FOOT = 74, + RIGHT_TOE_BASE = 75, + RIGHT_TOE_END = 76, + + NUM_JOINTS = 77, +} diff --git a/src/core/schema/fbs/soma_body_joint_poses.fbs b/src/core/schema/fbs/soma_body_joint_poses.fbs new file mode 100644 index 0000000000..e7348c9313 --- /dev/null +++ b/src/core/schema/fbs/soma_body_joint_poses.fbs @@ -0,0 +1,33 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "pose.fbs"; +include "soma_body_common.fbs"; +include "timestamp.fbs"; + +namespace core; + +struct SomaBodyJointPose { + joint: SomaBodyJoint (key); + pose: Pose; +} + +// 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 SomaBodyJointPoses { + // 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 { + data: SomaBodyJointPoses (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type SomaBodyJointPosesRecord; diff --git a/src/core/schema/fbs/soma_body_joint_rotations.fbs b/src/core/schema/fbs/soma_body_joint_rotations.fbs new file mode 100644 index 0000000000..56ee78f11d --- /dev/null +++ b/src/core/schema/fbs/soma_body_joint_rotations.fbs @@ -0,0 +1,44 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "point.fbs"; +include "soma_body_common.fbs"; +include "quaternion.fbs"; +include "timestamp.fbs"; + +namespace core; + +// One explicitly identified SOMA body rotation. +struct SomaBodyJointRotation { + joint: SomaBodyJoint (key); + rotation: Quaternion; +} + +// 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. +// Producers must convert historical, custom, or vendor-native references before +// publishing. Hips is global in the reference frame; all remaining rotations are +// joint-local relative to that fixed reference pose. A distinct vendor-native +// 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 SomaBodyJointRotations { + // 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. + global_translation: Point (id: 1); + global_translation_is_valid: bool (id: 2); +} + +table SomaBodyJointRotationsRecord { + data: SomaBodyJointRotations (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type SomaBodyJointRotationsRecord; diff --git a/src/core/schema/fbs/soma_common.fbs b/src/core/schema/fbs/soma_common.fbs new file mode 100644 index 0000000000..fea03b88be --- /dev/null +++ b/src/core/schema/fbs/soma_common.fbs @@ -0,0 +1,11 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +namespace core; + +// Hand side shared by the SOMA pose transport schemas. +enum SomaHandedness : uint8 { + UNSPECIFIED = 0, + LEFT = 1, + RIGHT = 2, +} diff --git a/src/core/schema/fbs/soma_hand_joint_poses.fbs b/src/core/schema/fbs/soma_hand_joint_poses.fbs new file mode 100644 index 0000000000..1df71e7678 --- /dev/null +++ b/src/core/schema/fbs/soma_hand_joint_poses.fbs @@ -0,0 +1,35 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "pose.fbs"; +include "soma_common.fbs"; +include "soma_hand_joint_rotations.fbs"; +include "timestamp.fbs"; + +namespace core; + +struct SomaHandJointPose { + joint: SomaHandJoint (key); + pose: Pose; +} + +// 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 SomaHandJointPoses { + // 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); +} + +table SomaHandJointPosesRecord { + data: SomaHandJointPoses (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type SomaHandJointPosesRecord; diff --git a/src/core/schema/fbs/soma_hand_joint_rotations.fbs b/src/core/schema/fbs/soma_hand_joint_rotations.fbs new file mode 100644 index 0000000000..65741c98ba --- /dev/null +++ b/src/core/schema/fbs/soma_hand_joint_rotations.fbs @@ -0,0 +1,76 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "point.fbs"; +include "quaternion.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 SomaHandJoint : uint8 { + WRIST = 0, + THUMB1 = 1, + THUMB2 = 2, + THUMB3 = 3, + THUMB_END = 4, + INDEX1 = 5, + INDEX2 = 6, + INDEX3 = 7, + INDEX4 = 8, + INDEX_END = 9, + MIDDLE1 = 10, + MIDDLE2 = 11, + MIDDLE3 = 12, + MIDDLE4 = 13, + MIDDLE_END = 14, + RING1 = 15, + RING2 = 16, + RING3 = 17, + RING4 = 18, + RING_END = 19, + PINKY1 = 20, + PINKY2 = 21, + PINKY3 = 22, + PINKY4 = 23, + PINKY_END = 24, + + NUM_JOINTS = 25, +} + +// One explicitly identified SOMA hand rotation. +struct SomaHandJointRotation { + joint: SomaHandJoint (key); + rotation: Quaternion; +} + +// 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. +// Producers must convert historical, custom, or vendor-native references before +// publishing. Wrist is global in the reference frame; finger rotations are +// 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 SomaHandJointRotations { + // 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. + global_translation: Point (id: 1); + global_translation_is_valid: bool (id: 2); + handedness: SomaHandedness = UNSPECIFIED (id: 3); +} + +table SomaHandJointRotationsRecord { + data: SomaHandJointRotations (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type SomaHandJointRotationsRecord; diff --git a/src/core/schema/golden/soma_body_common.bfbs b/src/core/schema/golden/soma_body_common.bfbs new file mode 100644 index 0000000000..5a51d63c6a Binary files /dev/null and b/src/core/schema/golden/soma_body_common.bfbs differ diff --git a/src/core/schema/golden/soma_body_joint_poses.bfbs b/src/core/schema/golden/soma_body_joint_poses.bfbs new file mode 100644 index 0000000000..894b1cdfca Binary files /dev/null and b/src/core/schema/golden/soma_body_joint_poses.bfbs differ diff --git a/src/core/schema/golden/soma_body_joint_rotations.bfbs b/src/core/schema/golden/soma_body_joint_rotations.bfbs new file mode 100644 index 0000000000..d28a0ed8a3 Binary files /dev/null and b/src/core/schema/golden/soma_body_joint_rotations.bfbs differ diff --git a/src/core/schema/golden/soma_common.bfbs b/src/core/schema/golden/soma_common.bfbs new file mode 100644 index 0000000000..7a9eb4d4de Binary files /dev/null and b/src/core/schema/golden/soma_common.bfbs differ diff --git a/src/core/schema/golden/soma_hand_joint_poses.bfbs b/src/core/schema/golden/soma_hand_joint_poses.bfbs new file mode 100644 index 0000000000..25119933db Binary files /dev/null and b/src/core/schema/golden/soma_hand_joint_poses.bfbs differ 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 0000000000..f14fa7eea3 Binary files /dev/null and b/src/core/schema/golden/soma_hand_joint_rotations.bfbs differ diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index 8c1f7eebe1..387cc3f1e3 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -19,6 +19,11 @@ pybind11_add_module(schema_py schema_array_views.h schema_serialized.h se3_tracker_bindings.h + soma_common_bindings.h + soma_body_joint_poses_bindings.h + soma_body_joint_rotations_bindings.h + soma_hand_joint_poses_bindings.h + soma_hand_joint_rotations_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..fa3691e353 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -21,6 +21,11 @@ #include "plugin_device_status_bindings.h" #include "pose_bindings.h" #include "se3_tracker_bindings.h" +#include "soma_body_joint_poses_bindings.h" +#include "soma_body_joint_rotations_bindings.h" +#include "soma_common_bindings.h" +#include "soma_hand_joint_poses_bindings.h" +#include "soma_hand_joint_rotations_bindings.h" #include "steering_wheel_bindings.h" #include "timestamp_bindings.h" @@ -81,4 +86,11 @@ PYBIND11_MODULE(_schema, m) // Bind full body types (BodyJointPose, BodyJoints structs, FullBodyPose table). core::bind_full_body(m); + + // Bind shared SOMA primitives, then the body and hand pose contracts. + core::bind_soma_common(m); + core::bind_soma_body_joint_rotations(m); + core::bind_soma_body_joint_poses(m); + core::bind_soma_hand_joint_rotations(m); + core::bind_soma_hand_joint_poses(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_bindings.h b/src/core/schema/python/soma_body_joint_poses_bindings.h new file mode 100644 index 0000000000..ade6ed58cf --- /dev/null +++ b/src/core/schema/python/soma_body_joint_poses_bindings.h @@ -0,0 +1,84 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_serialized.h" + +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_soma_body_joint_poses(py::module& m) +{ + 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("__repr__", [](const SomaBodyJointPose& self) + { return "SomaBodyJointPose(joint=" + std::string(EnumNameSomaBodyJoint(self.joint())) + ")"; }); + + serialized_class(m, "SomaBodyJointPoses", "Encoded evaluated SOMA body poses.") + .def(py::init( + [](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::move(joint_poses); + return pack(native); + }), + 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"); +} + +} // 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..55924716f4 --- /dev/null +++ b/src/core/schema/python/soma_body_joint_rotations_bindings.h @@ -0,0 +1,173 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_serialized.h" + +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +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, "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( + [](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::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") = std::vector{}, py::arg("global_translation") = Point(), + py::arg("global_translation_is_valid") = false) + .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( + "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_common_bindings.h b/src/core/schema/python/soma_common_bindings.h new file mode 100644 index 0000000000..175dc8d6c8 --- /dev/null +++ b/src/core/schema/python/soma_common_bindings.h @@ -0,0 +1,22 @@ +// 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); +} + +} // 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 new file mode 100644 index 0000000000..2a5ea85954 --- /dev/null +++ b/src/core/schema/python/soma_hand_joint_poses_bindings.h @@ -0,0 +1,87 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_serialized.h" + +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_soma_hand_joint_poses(py::module& m) +{ + 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("__repr__", [](const SomaHandJointPose& self) + { return "SomaHandJointPose(joint=" + std::string(EnumNameSomaHandJoint(self.joint())) + ")"; }); + + serialized_class(m, "SomaHandJointPoses", "Encoded evaluated SOMA hand poses.") + .def(py::init( + [](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::move(joint_poses); + native.handedness = handedness; + return pack(native); + }), + 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"); +} + +} // 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..ceeda65be2 --- /dev/null +++ b/src/core/schema/python/soma_hand_joint_rotations_bindings.h @@ -0,0 +1,122 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_serialized.h" + +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +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, "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( + [](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::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") = std::vector{}, py::arg("global_translation") = Point(), + py::arg("global_translation_is_valid") = false, py::arg("handedness") = SomaHandedness_UNSPECIFIED) + .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)) + .def_property_readonly("handedness", field(&SomaHandJointRotations::handedness)); + + bind_record( + m, "SomaHandJointRotationsRecord", "SomaHandJointRotations"); +} + +} // 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..203860a39f 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,8 @@ 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 .soma_hand_source import SomaHandRepresentation, SomaHandSource from .message_channel_source import MessageChannelSource from .message_channel_sink import MessageChannelSink from .message_channel_config import ( @@ -41,6 +43,10 @@ KeyboardOutputTrackedType, JointStateOutputTrackedType, FullBodyPoseTrackedType, + SomaBodyJointRotationsTrackedType, + SomaBodyJointPosesTrackedType, + SomaHandJointRotationsTrackedType, + SomaHandJointPosesTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, @@ -48,6 +54,10 @@ DeviceIOKeyboardOutputTracked, DeviceIOJointStateOutputTracked, DeviceIOFullBodyPoseTracked, + DeviceIOSomaBodyJointRotationsTracked, + DeviceIOSomaBodyJointPosesTracked, + DeviceIOSomaHandJointRotationsTracked, + DeviceIOSomaHandJointPosesTracked, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, MessageChannelStatusType, @@ -74,6 +84,10 @@ "FakeKeyEventSource", "JointStateSource", "FullBodySource", + "SomaBodySource", + "SomaBodyRepresentation", + "SomaHandSource", + "SomaHandRepresentation", "MessageChannelSource", "MessageChannelSink", "MessageChannelConfig", @@ -87,6 +101,10 @@ "KeyboardOutputTrackedType", "JointStateOutputTrackedType", "FullBodyPoseTrackedType", + "SomaBodyJointRotationsTrackedType", + "SomaBodyJointPosesTrackedType", + "SomaHandJointRotationsTrackedType", + "SomaHandJointPosesTrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", "MessageChannelStatusType", @@ -97,6 +115,10 @@ "DeviceIOKeyboardOutputTracked", "DeviceIOJointStateOutputTracked", "DeviceIOFullBodyPoseTracked", + "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 5cebd9a090..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,6 +22,10 @@ KeyboardOutput, JointStateOutput, FullBodyPose, + SomaBodyJointPoses, + SomaBodyJointRotations, + SomaHandJointPoses, + SomaHandJointRotations, MessageChannelMessagesTracked, ) @@ -115,6 +119,30 @@ class FullBodyPoseTrackedType(_PayloadTensorType): _payload_cls = FullBodyPose +class SomaBodyJointRotationsTrackedType(_PayloadTensorType): + """SomaBodyJointRotations payload from a generated SOMA body tracker.""" + + _payload_cls = SomaBodyJointRotations + + +class SomaBodyJointPosesTrackedType(_PayloadTensorType): + """SomaBodyJointPoses payload from a generated SOMA body tracker.""" + + _payload_cls = SomaBodyJointPoses + + +class SomaHandJointRotationsTrackedType(_PayloadTensorType): + """SomaHandJointRotations payload from a generated SOMA hand tracker.""" + + _payload_cls = SomaHandJointRotations + + +class SomaHandJointPosesTrackedType(_PayloadTensorType): + """SomaHandJointPoses payload from a generated SOMA hand tracker.""" + + _payload_cls = SomaHandJointPoses + + class MessageChannelMessagesTrackedType(_RequiredPayloadTensorType): """MessageChannelMessagesTracked batch from DeviceIO MessageChannelTracker.""" @@ -215,6 +243,38 @@ def DeviceIOFullBodyPoseTracked() -> TensorGroupType: ) +def DeviceIOSomaBodyJointRotationsTracked() -> TensorGroupType: + """SOMA body controls, or None when the tracker is inactive.""" + return TensorGroupType( + "deviceio_soma_body_joint_rotations", + [SomaBodyJointRotationsTrackedType("soma_body_tracked")], + ) + + +def DeviceIOSomaBodyJointPosesTracked() -> TensorGroupType: + """Evaluated SOMA body poses, or None when the tracker is inactive.""" + return TensorGroupType( + "deviceio_soma_body_joint_poses", + [SomaBodyJointPosesTrackedType("soma_body_tracked")], + ) + + +def DeviceIOSomaHandJointRotationsTracked() -> TensorGroupType: + """SOMA hand controls, or None when the tracker is inactive.""" + return TensorGroupType( + "deviceio_soma_hand_joint_rotations", + [SomaHandJointRotationsTrackedType("soma_hand_tracked")], + ) + + +def DeviceIOSomaHandJointPosesTracked() -> TensorGroupType: + """Evaluated SOMA hand poses, or None when the tracker is inactive.""" + return TensorGroupType( + "deviceio_soma_hand_joint_poses", + [SomaHandJointPosesTrackedType("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_body_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py new file mode 100644 index 0000000000..4ed07611b6 --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_body_source.py @@ -0,0 +1,100 @@ +# 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 + + +from isaaccapture.deviceio_trackers import ( + SomaBodyJointPosesTracker, + SomaBodyJointRotationsTracker, +) + +from .deviceio_tensor_types import ( + DeviceIOSomaBodyJointPosesTracked, + DeviceIOSomaBodyJointRotationsTracked, +) +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 _dense_joint_poses, _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 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) + # 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") + self._tracker = SomaBodyJointRotationsTracker(collection_id) + self._input_type = DeviceIOSomaBodyJointRotationsTracked() + else: + 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 () + 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) + else: + 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 new file mode 100644 index 0000000000..e4dbb83b10 --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/soma_hand_source.py @@ -0,0 +1,107 @@ +# 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 + + +from isaaccapture.deviceio_trackers import ( + SomaHandJointPosesTracker, + SomaHandJointRotationsTracker, +) +from isaaccapture.schema import SomaHandedness + +from .deviceio_tensor_types import ( + DeviceIOSomaHandJointPosesTracked, + DeviceIOSomaHandJointRotationsTracked, +) +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 +from ..utilities.soma_body_evaluator import _dense_joint_poses + +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 hand collection into evaluated joint poses.""" + + HAND = "soma_hand" + + def __init__( + self, + name: str, + collection_id: str, + handedness: SomaHandedness, + 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 (SomaHandedness.LEFT, SomaHandedness.RIGHT): + 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") + self._tracker = SomaHandJointRotationsTracker(collection_id) + self._input_type = DeviceIOSomaHandJointRotationsTracked() + else: + self._tracker = SomaHandJointPosesTracker(collection_id) + self._input_type = DeviceIOSomaHandJointPosesTracked() + # 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 + ) + 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) + else: + 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/tensor_types/__init__.py b/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py index 5406ef949c..514b057465 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py @@ -9,13 +9,17 @@ from .ndarray_types import NDArrayType, DLDeviceType, DLDataType from .standard_types import ( HandInput, + SomaHandInput, HeadInput, ControllerInput, FullBodyInput, + SomaBodyInput, TransformMatrix, Generic3AxisPedalInput, NUM_HAND_JOINTS, + NUM_SOMA_HAND_JOINTS, NUM_BODY_JOINTS, + NUM_SOMA_BODY_JOINTS, RobotHandJoints, ) from .tactile_types import ( @@ -30,10 +34,12 @@ ) from .indices import ( HandInputIndex, + SomaHandInputIndex, HeadInputIndex, ControllerInputIndex, Generic3AxisPedalInputIndex, FullBodyInputIndex, + SomaBodyInputIndex, HandJointIndex, BodyJointIndex, FingerIndex, @@ -50,13 +56,17 @@ "DLDataType", # Standard types "HandInput", + "SomaHandInput", "HeadInput", "ControllerInput", "FullBodyInput", + "SomaBodyInput", "TransformMatrix", "Generic3AxisPedalInput", "NUM_HAND_JOINTS", + "NUM_SOMA_HAND_JOINTS", "NUM_BODY_JOINTS", + "NUM_SOMA_BODY_JOINTS", "RobotHandJoints", # Tactile / haptic types "TactileVector", @@ -69,10 +79,12 @@ "NUM_END_EFFECTOR_FORCE_AXES", # Indices "HandInputIndex", + "SomaHandInputIndex", "HeadInputIndex", "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..40a954b7ac 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py @@ -17,10 +17,12 @@ from enum import IntEnum from .standard_types import ( HandInput, + SomaHandInput, HeadInput, ControllerInput, Generic3AxisPedalInput, FullBodyInput, + SomaBodyInput, ) @@ -37,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_" @@ -47,6 +52,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..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,7 +17,9 @@ # 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 # ============================================================================ # Hand Tracking Types @@ -85,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 # ============================================================================ @@ -253,6 +282,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..5ab1c3161e --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_body_evaluator.py @@ -0,0 +1,168 @@ +# 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 + +from .soma_contract import resolve_soma_body_contract + + +_QUATERNION_EPS = 1e-12 + + +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 _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 + 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 + # Finite float32 components can have a norm larger than float32 can represent. + return (quaternion64 / norm).astype(np.float32), True + + +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], provided[joint] = _normalize_quaternion( + (rotation.x, rotation.y, rotation.z, rotation.w) + ) + 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 + position = np.asarray( + (pose.position.x, pose.position.y, pose.position.z), dtype=np.float32 + ) + 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) + + 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( + (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]: + rotations, control_valid = _dense_joint_rotations(data.joint_rotations, 77) + + 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]: + 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, + reference_pose=self.reference_pose, + ) + 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/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 new file mode 100644 index 0000000000..e9189e43e0 --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/utilities/soma_hand_evaluator.py @@ -0,0 +1,74 @@ +# 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 _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, 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) + + 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( + (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]: + rotations, control_valid = _dense_joint_rotations(data.joint_rotations, 25) + + 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]: + 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, + reference_pose=self.reference_pose, + ) + 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 3c488108bf..768ba08704 100644 --- a/src/python/isaaccapture/schema/__init__.py +++ b/src/python/isaaccapture/schema/__init__.py @@ -84,6 +84,26 @@ BodyJoints, FullBodyPose, FullBodyPoseRecord, + # Shared SOMA types. + SomaHandedness, + # SOMA body joint-rotation types. + SomaBodyJoint, + SomaBodyJointRotation, + SomaBodyJointRotations, + SomaBodyJointRotationsRecord, + # SOMA evaluated body joint-pose types. + SomaBodyJointPose, + SomaBodyJointPoses, + SomaBodyJointPosesRecord, + # SOMA hand joint-rotation types. + SomaHandJoint, + SomaHandJointRotation, + SomaHandJointRotations, + SomaHandJointRotationsRecord, + # SOMA evaluated hand joint-pose types. + SomaHandJointPose, + SomaHandJointPoses, + SomaHandJointPosesRecord, ) # Deprecated aliases, resolved lazily via __getattr__ so accessing them emits a @@ -189,4 +209,24 @@ def __getattr__(name: str): "BodyJoints", "FullBodyPose", "FullBodyPoseRecord", + # Shared SOMA types. + "SomaHandedness", + # SOMA body joint-rotation types. + "SomaBodyJoint", + "SomaBodyJointRotation", + "SomaBodyJointRotations", + "SomaBodyJointRotationsRecord", + # SOMA evaluated body joint-pose types. + "SomaBodyJointPose", + "SomaBodyJointPoses", + "SomaBodyJointPosesRecord", + # SOMA hand joint-rotation types. + "SomaHandJoint", + "SomaHandJointRotation", + "SomaHandJointRotations", + "SomaHandJointRotationsRecord", + # SOMA evaluated hand joint-pose types. + "SomaHandJointPose", + "SomaHandJointPoses", + "SomaHandJointPosesRecord", ] 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..46290c8e45 100644 --- a/tests/cpp/core/schema/CMakeLists.txt +++ b/tests/cpp/core/schema/CMakeLists.txt @@ -16,6 +16,10 @@ add_executable(schema_tests test_joint_se3_pose.cpp test_se3_tracker.cpp test_serialized.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.cpp b/tests/cpp/core/schema/test_soma_body_joint_poses.cpp new file mode 100644 index 0000000000..7ea55f69f2 --- /dev/null +++ b/tests/cpp/core/schema/test_soma_body_joint_poses.cpp @@ -0,0 +1,50 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include + +#include + +namespace +{ + +constexpr flatbuffers::voffset_t vt(flatbuffers::voffset_t field) +{ + return (field + 2) * 2; +} + +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::SomaBodyJointPose) == 32); + +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.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)); + flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); + REQUIRE(verifier.VerifyBuffer(nullptr)); + + const auto* decoded = flatbuffers::GetRoot(builder.GetBufferPointer()); + REQUIRE(decoded->joint_poses() != nullptr); + 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(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 new file mode 100644 index 0000000000..9bcac7971c --- /dev/null +++ b/tests/cpp/core/schema/test_soma_body_joint_rotations.cpp @@ -0,0 +1,108 @@ +// 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 + +#define VT(field) (field + 2) * 2 +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(sizeof(core::SomaBodyJointRotation) == 20); + +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 rotations have explicit joint identifiers", "[soma_body_joint_rotations][struct]") +{ + const core::SomaBodyJointRotation head(core::SomaBodyJoint_HEAD, core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)); + + 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 as a sparse keyed vector", "[soma_body_joint_rotations][flatbuffers]") +{ + core::SomaBodyJointRotationsT pose; + 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; + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaBodyJointRotations::Pack(builder, &pose)); + + 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()->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(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 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.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); + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaBodyJointRotationsRecord::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); + 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)); + 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(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); + CHECK(decoded->timestamp()->sample_time_raw_device_clock() == 300); +} diff --git a/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp b/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp new file mode 100644 index 0000000000..deb1bbecfe --- /dev/null +++ b/tests/cpp/core/schema/test_soma_hand_joint_poses.cpp @@ -0,0 +1,50 @@ +// 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::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(std::is_trivially_copyable_v); +static_assert(sizeof(core::SomaHandJointPose) == 32); + +TEST_CASE("SOMA hand joint poses round trip as a sparse keyed vector", "[soma_hand_joint_poses][flatbuffers]") +{ + core::SomaHandJointPosesT pose; + 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; + builder.Finish(core::SomaHandJointPoses::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); + 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(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 new file mode 100644 index 0000000000..98dc24d110 --- /dev/null +++ b/tests/cpp/core/schema/test_soma_hand_joint_rotations.cpp @@ -0,0 +1,113 @@ +// 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 + +#define VT(field) (field + 2) * 2 +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_trivially_copyable_v); +static_assert(sizeof(core::SomaHandJointRotation) == 20); + +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 rotations have explicit joint identifiers", "[soma_hand_joint_rotations][struct]") +{ + const core::SomaHandJointRotation wrist(core::SomaHandJoint_WRIST, core::Quaternion(0.0f, 0.0f, 0.6f, 0.8f)); + + 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 as a sparse keyed vector", "[soma_hand_joint_rotations][flatbuffers]") +{ + core::SomaHandJointRotationsT pose; + 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; + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaHandJointRotations::Pack(builder, &pose)); + + 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()->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(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 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.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; + record.timestamp = std::make_shared(100, 200, 300); + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::SomaHandJointRotationsRecord::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); + 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)); + 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(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); + 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..ebfb6b69b5 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_soma_body_evaluator.py @@ -0,0 +1,250 @@ +# 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.retargeting_engine.utilities.soma_body_evaluator import ( + _dense_joint_poses, + _dense_joint_rotations, + _SomaBodyEvaluator, +) +from isaaccapture.retargeting_engine.utilities.soma_contract import ( + SOMA_REFERENCE_VERSION, +) +from isaaccapture.schema import ( + Point, + Pose, + Quaternion, + SomaBodyJoint, + SomaBodyJointPose, + SomaBodyJointRotation, +) + + +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", *body_joint_names()) + layer.output_joint_parent_ids = np.array([0, 0, 1, 1, *range(3, 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 + + +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] == ("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) + 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=[ + SomaBodyJointRotation(SomaBodyJoint(index), Quaternion(*rotation)) + for index, rotation in enumerate(rotations) + ], + 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() + ) + + +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) + + +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]) + + +@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( + 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 new file mode 100644 index 0000000000..d1adbbe6b3 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_soma_body_source.py @@ -0,0 +1,243 @@ +# 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 ( + SomaBodyJointPosesTracker, + SomaBodyJointRotationsTracker, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + DeviceIOFullBodyPoseTracked, + DeviceIOSomaBodyJointPosesTracked, + DeviceIOSomaBodyJointRotationsTracked, + 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, + Pose, + Quaternion, + SomaBodyJoint, + SomaBodyJointPose, + SomaBodyJointPoses, + SomaBodyJointRotation, + SomaBodyJointRotations, +) +from isaaccapture.teleop_session_manager.helpers import _get_trackers_from_pipeline + + +def pose(): + joints = [ + SomaBodyJointRotation(SomaBodyJoint(index), Quaternion(0, 0, 0, 1)) + for index in range(77) + ] + return SomaBodyJointRotations(joints, Point(1, 2, 3), True) + + +def fake_layer(): + layer = MagicMock() + 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 + + +def source(): + return SomaBodySource("body", "vendor.soma", fake_layer()) + + +def evaluated_pose(): + 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) + + +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(), 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"] + 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(), SomaBodyJointPosesTracker) + 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] + + 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_POSITIONS], positions + ) + 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( + 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 = DeviceIOSomaBodyJointRotationsTracked() + soma_poses = DeviceIOSomaBodyJointPosesTracked() + fullbody = DeviceIOFullBodyPoseTracked() + with pytest.raises(ValueError, match="type mismatch"): + soma.check_compatibility(fullbody) + 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="SomaBodyJointPoses"): + 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/retargeting_engine/test_soma_hand_source.py b/tests/python/core/retargeting_engine/test_soma_hand_source.py new file mode 100644 index 0000000000..591368398f --- /dev/null +++ b/tests/python/core/retargeting_engine/test_soma_hand_source.py @@ -0,0 +1,332 @@ +# 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 ( + SomaHandJointPosesTracker, + SomaHandJointRotationsTracker, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + DeviceIOSomaHandJointPosesTracked, + DeviceIOSomaHandJointRotationsTracked, + SomaHandRepresentation, + SomaHandSource, +) +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, + Quaternion, + SomaHandedness, + SomaHandJoint, + SomaHandJointPose, + SomaHandJointPoses, + SomaHandJointRotation, + SomaHandJointRotations, +) + + +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() + 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 + + +def rotations(handedness=SomaHandedness.LEFT): + 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): + 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) + + +def payload_group(group_type, data): + group = TensorGroup(group_type) + group[0] = data + return group + + +def test_rotation_source_evaluates_received_payload(): + 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), + 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] + ) + + +@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", + "vendor.left", + SomaHandedness.LEFT, + representation=SomaHandRepresentation.JOINT_POSES, + ) + assert isinstance(source.get_tracker(), SomaHandJointPosesTracker) + 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 + 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_POSITIONS], expected_positions + ) + 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", + "vendor.left", + SomaHandedness.LEFT, + representation=SomaHandRepresentation.JOINT_POSES, + ) + inputs = payload_group( + 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 = DeviceIOSomaHandJointRotationsTracked() + pose_type = DeviceIOSomaHandJointPosesTracked() + with pytest.raises(ValueError, match="type mismatch"): + rotation_type.check_compatibility(pose_type) + 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", SomaHandedness.LEFT) + with pytest.raises(ValueError, match="LEFT or RIGHT"): + SomaHandSource( + "left", + "vendor.left", + 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/core/schema/test_soma_body_joint_poses.py b/tests/python/core/schema/test_soma_body_joint_poses.py new file mode 100644 index 0000000000..0b42d7e39f --- /dev/null +++ b/tests/python/core/schema/test_soma_body_joint_poses.py @@ -0,0 +1,60 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import pytest + +from isaaccapture.schema import ( + DeviceDataTimestamp, + Point, + Pose, + Quaternion, + SomaBodyJoint, + SomaBodyJointPose, + SomaBodyJointPoses, + SomaBodyJointPosesRecord, +) + + +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)), + ) + ] + ) + + 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_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(): + payload = SomaBodyJointPoses( + [ + SomaBodyJointPose( + SomaBodyJoint.RIGHT_TOE_END, + Pose(Point(1.0, 2.0, 3.0), Quaternion(0.0, 0.0, 0.0, 1.0)), + ) + ] + ) + 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 new file mode 100644 index 0000000000..0536724b37 --- /dev/null +++ b/tests/python/core/schema/test_soma_body_joint_rotations.py @@ -0,0 +1,88 @@ +# 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, + Point, + Quaternion, + SomaBodyJoint, + SomaBodyJointRotation, + SomaBodyJointRotations, + SomaBodyJointRotationsRecord, +) + + +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_rotation_has_explicit_joint_identifier(): + entry = SomaBodyJointRotation(SomaBodyJoint.HEAD, Quaternion(0.0, 0.0, 0.6, 0.8)) + + 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) + + +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 [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_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]) +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) + 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, + ) + 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 new file mode 100644 index 0000000000..f4ab28a30b --- /dev/null +++ b/tests/python/core/schema/test_soma_hand_joint_poses.py @@ -0,0 +1,65 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import pytest + +from isaaccapture.schema import ( + DeviceDataTimestamp, + Point, + Pose, + Quaternion, + SomaHandedness, + SomaHandJoint, + SomaHandJointPose, + SomaHandJointPoses, + SomaHandJointPosesRecord, +) + + +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 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 + + +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_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.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 new file mode 100644 index 0000000000..e253028a9f --- /dev/null +++ b/tests/python/core/schema/test_soma_hand_joint_rotations.py @@ -0,0 +1,71 @@ +# 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, + Point, + Quaternion, + SomaHandedness, + SomaHandJoint, + SomaHandJointRotation, + SomaHandJointRotations, + SomaHandJointRotationsRecord, +) + + +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, + ) + + 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_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]) + + +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, + ) + record = SomaHandJointRotationsRecord(payload, DeviceDataTimestamp(100, 200, 300)) + + wrist = record.data.lookup(SomaHandJoint.WRIST) + np.testing.assert_array_equal( + [wrist.rotation.x, wrist.rotation.y, wrist.rotation.z, wrist.rotation.w], + quaternion, + ) + assert record.data.handedness == SomaHandedness.RIGHT + assert record.timestamp.sample_time_local_common_clock == 200 diff --git a/tests/python/examples/CMakeLists.txt b/tests/python/examples/CMakeLists.txt index 6d2c6b5a4b..a0cc7b40cc 100644 --- a/tests/python/examples/CMakeLists.txt +++ b/tests/python/examples/CMakeLists.txt @@ -2,3 +2,6 @@ # SPDX-License-Identifier: Apache-2.0 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/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..c1b0a71b38 --- /dev/null +++ b/tests/python/examples/deviceio_live_view/test_soma_viewer.py @@ -0,0 +1,526 @@ +# 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 ( + SomaBodyJointPosesTracker, + SomaBodyJointRotationsTracker, + SomaHandJointPosesTracker, + SomaHandJointRotationsTracker, +) +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 +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, + Pose, + Quaternion, + SomaBodyJoint, + SomaBodyJointPose, + SomaBodyJointPoses, + SomaBodyJointRotation, + SomaBodyJointRotations, + SomaHandedness, + SomaHandJoint, + SomaHandJointPose, + SomaHandJointPoses, + SomaHandJointRotation, + SomaHandJointRotations, +) +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, +) +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(): + layer = MagicMock() + 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 + + +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 = [ + 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 = [ + 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 = [ + SomaHandJointRotation(SomaHandJoint(index), Quaternion(*rotation)) + for index, rotation in enumerate(rotations) + ] + return SomaHandJointRotations(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, hand_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", + } + + +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( + body_schema=BodySchema.SOMA, + soma_body_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(), SomaBodyJointRotationsTracker) + 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): + 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( + body_schema=BodySchema.SOMA, + soma_body_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(), SomaBodyJointPosesTracker) + tracker_factory.assert_called_once_with("vendor_body") + create.assert_not_called() + + +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(), SomaHandJointPosesTracker) + 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, SomaHandedness.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): + create = MagicMock(side_effect=AssertionError("SOMA must remain optional")) + monkeypatch.setattr(soma_body, "create_layer", create) + 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): + 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") + 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,expected,collection", + [ + (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, schema, 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) + 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] + args += ["--soma-body-collection-id", collection] + assert live_deviceio.main(args) == 0 + select.assert_called_once_with( + body_schema=expected, + soma_body_collection_id=collection, + soma_body_representation="joint-rotations", + ) + select_hands.assert_called_once_with( + 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 + + +@pytest.mark.parametrize( + "args,message", + [ + (["--body-schema", "unknown"], "invalid choice"), + (["--soma-data-root", "/unused"], "unrecognized arguments"), + ( + [ + "--body-schema", + "soma", + "--soma-body-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", 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(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() + 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], omitted={10} + ) + 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_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 = {} + 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", SomaHandedness.LEFT), + ("hand_right", SomaHandedness.RIGHT), + ): + source = sources[name] + assert isinstance(source.get_tracker(), SomaHandJointRotationsTracker) + 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(monkeypatch): + create = MagicMock(side_effect=AssertionError("joint poses must not load SOMA")) + monkeypatch.setattr(soma_body, "create_layer", create) + body = create_body_view_pipeline( + 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() + assert source._evaluator is None + 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) + create.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..ed5bb89e88 --- /dev/null +++ b/tests/python/examples/soma_body_publisher/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 "soma_body_publisher_${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("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..4e5db394a4 --- /dev/null +++ b/tests/python/examples/soma_body_publisher/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" / "soma_body_publisher" / "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/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..ff0fbc05fc --- /dev/null +++ b/tests/python/examples/soma_body_publisher/test_publisher.py @@ -0,0 +1,285 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +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, + _SomaBodyEvaluator, +) +from isaaccapture.schema import ( + DeviceDataTimestamp, + SomaBodyJointPosesRecord, + SomaBodyJointRotationsRecord, +) +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 = SomaBodyJointRotationsRecord(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 = SomaBodyJointPosesRecord( + evaluated, DeviceDataTimestamp(10, 20, 30) + ) + 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.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], (2, 77, 1)), [[0, 0, 0], [1, 2, 3]], None), + ) + 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: + 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( + "representation,capacity", [("joint-rotations", 2048), ("joint-poses", 4096)] +) +def test_validate_only_rejects_oversized_payload( + demo, monkeypatch, representation, capacity +): + encoder = ( + "soma_joint_poses" + if representation == "joint-poses" + else "soma_joint_rotations" + ) + monkeypatch.setattr( + publisher, + encoder, + lambda *args: SimpleNamespace(to_bytes=lambda: bytes(capacity + 1)), + ) + with pytest.raises(ValueError, match="exceeds transport capacity"): + publisher.main( + ["publisher", "--body-representation", representation, "--validate-only"] + ) + + +@pytest.mark.parametrize("rate", ["0", "-1", "nan", "inf"]) +def test_invalid_rate_is_rejected_before_loading_model(monkeypatch, rate): + layer = MagicMock() + monkeypatch.setattr(publisher, "create_layer", layer) + with pytest.raises(SystemExit) as error: + publisher.main(["publisher", "--rate", rate]) + assert error.value.code == 2 + layer.assert_not_called() + + +def test_demo_to_fbs_to_native_soma_matches_upstream(soma_assets): + import torch + from soma.geometry.transforms import ( + matrix_to_quaternion_xyzw, + quaternion_xyzw_to_matrix, + ) + + evaluator = _SomaBodyEvaluator(create_layer(soma_assets)) + q, t, matrices = demo_controls( + soma_assets / "example_animation.npy", evaluator.layer + ) + torch.testing.assert_close( + quaternion_xyzw_to_matrix(torch.from_numpy(q)), matrices, atol=1e-4, rtol=1e-4 + ) + with torch.no_grad(): + expected = evaluator.layer.pose( + matrices[:1], + transl=torch.from_numpy(t[:1]), + pose2rot=False, + fk_only=True, + apply_correctives=False, + ) + payload = soma_joint_rotations(q[0], t[0]) + 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( + expected["transforms"][0, 1:, :3, :3] + ) + actual_rotations = quaternion_xyzw_to_matrix(torch.from_numpy(orientations.copy())) + expected_rotations = quaternion_xyzw_to_matrix(expected_orientations) + torch.testing.assert_close( + actual_rotations, expected_rotations, atol=1e-4, rtol=1e-4 + ) + np.testing.assert_array_equal(valid, np.ones(77, dtype=np.uint8)) + + evaluated_positions, evaluated_orientations = evaluate_demo_controls( + evaluator.layer, matrices, t + ) + evaluated_payload = soma_joint_poses( + evaluated_positions[0], evaluated_orientations[0] + ) + 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(sparse_orientations)), + actual_rotations, + atol=1e-4, + rtol=1e-4, + ) + + flipped_positions, _, _ = evaluator.evaluate(soma_joint_rotations(-q[0], t[0])) + np.testing.assert_allclose(flipped_positions, positions, atol=1e-6) + + for rotations, translation in zip(q, t, strict=True): + assert len(soma_joint_rotations(rotations, translation).to_bytes()) <= 2048 + for frame_positions, frame_orientations in zip( + evaluated_positions, evaluated_orientations, strict=True + ): + assert ( + len(soma_joint_poses(frame_positions, frame_orientations).to_bytes()) + <= 4096 + ) 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..069e58d004 --- /dev/null +++ b/tests/python/examples/soma_hand_publisher/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 "soma_hand_publisher_${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("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..03612477c1 --- /dev/null +++ b/tests/python/examples/soma_hand_publisher/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" / "soma_hand_publisher" / "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/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..4d2a2c5a21 --- /dev/null +++ b/tests/python/examples/soma_hand_publisher/test_publisher.py @@ -0,0 +1,276 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +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, + _dense_joint_rotations, +) +from isaaccapture.retargeting_engine.utilities.soma_hand_evaluator import ( + _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, + soma_joint_poses, + soma_joint_rotations, +) + + +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.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], (2, 25, 1)), + "translation": np.zeros((2, 3)), + "positions": np.zeros((2, 25, 3)), + "orientations": np.tile([0, 0, 0, 1], (2, 25, 1)), + } + 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", 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", + "--hand-representation", + representation, + ] + ) + == 0 + ) + + 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), + ] + 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(): + rotations = np.tile([0.0, 0.0, 0.0, 1.0], (25, 1)) + rotation_payload = soma_joint_rotations(rotations, [1, 2, 3], SomaHandedness.LEFT) + assert rotation_payload.handedness == SomaHandedness.LEFT + dense_rotations, rotation_valid = _dense_joint_rotations( + rotation_payload.joint_rotations, 25 + ) + np.testing.assert_array_equal(dense_rotations, rotations) + assert rotation_valid.all() + + positions = np.arange(75, dtype=np.float32).reshape(25, 3) + pose_payload = soma_joint_poses(positions, rotations, SomaHandedness.RIGHT) + assert pose_payload.handedness == SomaHandedness.RIGHT + dense_positions, _, pose_valid = _dense_joint_poses(pose_payload.joint_poses, 25) + np.testing.assert_array_equal(dense_positions, positions) + assert pose_valid.all() + + +def test_demo_profiles_match_upstream_hand_fk(soma_assets): + body, hands = create_layers(soma_assets) + frames = demo_hand_frames(soma_assets / "example_animation.npy", body, hands) + + for side, handedness in ( + ("left", SomaHandedness.LEFT), + ("right", SomaHandedness.RIGHT), + ): + frame = frames[side] + payload = soma_joint_rotations( + frame["rotations"][0], frame["translation"][0], handedness + ) + 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( + frame["positions"][0], frame["orientations"][0], handedness + ) + evaluated_positions, _, evaluated_valid = _dense_joint_poses( + evaluated.joint_poses, 25 + ) + np.testing.assert_allclose(evaluated_positions, positions, atol=1e-5) + assert evaluated_valid.all() + + for representation in ("joint-rotations", "joint-poses"): + capacity = 2048 if representation == "joint-poses" else 1024 + for index in range(len(frames["left"]["positions"])): + for side, handedness in ( + ("left", SomaHandedness.LEFT), + ("right", SomaHandedness.RIGHT), + ): + frame = frames[side] + if representation == "joint-poses": + payload = soma_joint_poses( + frame["positions"][index], + frame["orientations"][index], + handedness, + ) + else: + payload = soma_joint_rotations( + frame["rotations"][index], + frame["translation"][index], + handedness, + ) + assert len(payload.to_bytes()) <= capacity