From 7d5b500dfce1704faef581c0e2168ba995bdb32a Mon Sep 17 00:00:00 2001 From: Michael Haselton Date: Mon, 12 Oct 2026 11:30:02 -0400 Subject: [PATCH] feat(mcap): add SOMA body and hand recording, replay, and live preview Extend the existing body and hand commands with SOMA schema selection and support for joint-rotations and joint-poses. Preserve received FlatBuffer payloads in MCAP and render native SOMA skeletons through the existing sources. Share recording, replay, and visualization helpers while preserving legacy defaults and core replay behavior. Keep controllers opt-in for SOMA body workflows. Add round-trip, sparse-data, channel-selection, EOF, and repeatability tests, and document installation and usage. Signed-off-by: Michael Haselton --- docs/source/references/mcap_record_replay.rst | 140 +++++ examples/mcap_record_replay/README.md | 138 ++++- examples/mcap_record_replay/pyproject.toml | 3 +- .../mcap_record_replay/common.py | 325 ++++++++++- .../mcap_record_replay/live_full_body.py | 45 +- .../mcap_record_replay/live_hand.py | 35 +- .../mcap_record_replay/record_full_body.py | 39 +- .../mcap_record_replay/record_hand.py | 35 +- .../mcap_record_replay/replay_full_body.py | 72 ++- .../mcap_record_replay/replay_hand.py | 82 ++- .../mcap_record_replay/soma_body_common.py | 209 +++++++ .../mcap_record_replay/soma_hand_common.py | 171 ++++++ tests/python/examples/CMakeLists.txt | 1 + .../mcap_record_replay/CMakeLists.txt | 21 + .../examples/mcap_record_replay/conftest.py | 159 ++++++ .../mcap_record_replay/pyproject.toml | 13 + .../mcap_record_replay/test_soma_hand.py | 508 ++++++++++++++++++ .../mcap_record_replay/test_soma_mcap.py | 208 +++++++ .../mcap_record_replay/test_soma_viewer.py | 345 ++++++++++++ 19 files changed, 2450 insertions(+), 99 deletions(-) create mode 100644 examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/soma_body_common.py create mode 100644 examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/soma_hand_common.py create mode 100644 tests/python/examples/mcap_record_replay/CMakeLists.txt create mode 100644 tests/python/examples/mcap_record_replay/conftest.py create mode 100644 tests/python/examples/mcap_record_replay/pyproject.toml create mode 100644 tests/python/examples/mcap_record_replay/test_soma_hand.py create mode 100644 tests/python/examples/mcap_record_replay/test_soma_mcap.py create mode 100644 tests/python/examples/mcap_record_replay/test_soma_viewer.py diff --git a/docs/source/references/mcap_record_replay.rst b/docs/source/references/mcap_record_replay.rst index c23dad9c6c..cc3f2632d4 100644 --- a/docs/source/references/mcap_record_replay.rst +++ b/docs/source/references/mcap_record_replay.rst @@ -239,6 +239,146 @@ The ``record_controller`` / ``replay_controller`` and ``record_full_body`` / — without it, ``uv`` may resolve against the system Python and fail to match the lockfile. +SOMA body workflows +^^^^^^^^^^^^^^^^^^^ + +The body commands accept ``--body-schema full-body-pose`` (default) or +``--body-schema soma``. SOMA mode supports both ``SomaBodyJointRotations`` and +``SomaBodyJointPoses``. They render the native 77-joint body topology, including +available finger joints, without converting to ``FullBodyPose``. See +:doc:`/device/soma` for the transport contract. + +FullBodyPose mode includes controllers by default. SOMA mode uses only the +body stream, including available finger joints. Pass ``--controllers`` to +include controller recording, or ``--no-controllers`` to disable it for +FullBodyPose. Standalone hands use the separate hand commands; these examples +do not extract or fuse them with the body. + +Install the example and demo publisher into a configured development checkout: + +.. code-block:: bash + + uv pip install -e ./examples/mcap_record_replay + uv pip install -e ./examples/soma_body_publisher + +Start the live viewer and connect a CloudXR client. Live preview does not read +or write an MCAP file: + +.. code-block:: bash + + uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.live_full_body \ + --accept-eula --body-schema soma --soma-body-representation joint-rotations + +In a second terminal, start the existing publisher after the runtime is running: + +.. code-block:: bash + + source ~/.cloudxr/run/cloudxr.env + uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ + --body-representation joint-rotations --loop + +Record in another terminal, then stop the live viewer and publisher before +replaying on the same Viser port: + +.. code-block:: bash + + uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.record_full_body \ + 10 recordings/soma_body_joint-rotations_demo.mcap \ + --accept-eula --body-schema soma --soma-body-representation joint-rotations + + uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.replay_full_body \ + recordings/soma_body_joint-rotations_demo.mcap \ + --body-schema soma --soma-body-representation joint-rotations + +For evaluated poses, use ``--soma-body-representation joint-poses`` on the +viewer, recorder, and replay commands, and ``--body-representation joint-poses`` +on the publisher. Use a separate output file. Replay works without OpenXR. +As with the other viewers, ``--host`` and ``--port`` control browser access. + +The recorder stores received SOMA payloads unchanged, without FK. Viewing joint +rotations uses ``SomaBodySource`` and its existing FK helper against a prepared +neutral model. Viewing joint poses maps directly without loading SOMA-X assets +or running FK. Unavailable joints and connecting bones are hidden. + +``--soma-body-collection-id`` selects the publisher collection and MCAP channel +base. Its default, ``soma_body_demo``, matches the publisher's ``--collection-id``. +Replay reads +``/soma_body_joint_rotations_tracked`` or +``/soma_body_joint_poses_tracked``. If no file is supplied, it selects +the newest ``soma_body__*.mcap`` under ``./recordings/``. + +Native replay still advances one tracked sample per update. The viewer uses the +selected channel's average capture rate for pacing; ``--rate`` overrides it +(``0`` is unpaced). It does not resample or synchronize streams. FullBodyPose +and OpenXR hand replay retain their 60 Hz, duration-based playback. + +SOMA hand workflows +^^^^^^^^^^^^^^^^^^^ + +The hand commands accept ``--hand-schema openxr-hand-pose`` (default) or +``--hand-schema soma``. SOMA mode supports ``SomaHandJointRotations`` and +``SomaHandJointPoses``. Each left/right collection retains its anatomical +``hand_side`` and uses the native 25-joint Wrist/finger layout. These commands +do not fuse standalone hands with a body stream. + +Install the MCAP example and the existing hand publisher: + +.. code-block:: bash + + uv pip install -e ./examples/mcap_record_replay + uv pip install -e ./examples/soma_hand_publisher + +Start the viewer and connect a CloudXR client: + +.. code-block:: bash + + uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.live_hand \ + --accept-eula --hand-schema soma --soma-hand-representation joint-rotations + +After the runtime is running, start the publisher in a second terminal: + +.. code-block:: bash + + source ~/.cloudxr/run/cloudxr.env + uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ + --hand-representation joint-rotations --loop + +Record in another terminal, then stop the viewer and publisher before replaying +on the same Viser port: + +.. code-block:: bash + + uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.record_hand \ + 10 recordings/soma_hand_joint-rotations_demo.mcap \ + --accept-eula --hand-schema soma --soma-hand-representation joint-rotations + + uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.replay_hand \ + recordings/soma_hand_joint-rotations_demo.mcap \ + --hand-schema soma --soma-hand-representation joint-rotations + +For evaluated poses, use ``--soma-hand-representation joint-poses`` on the +viewer, recorder, and replay commands, and ``--hand-representation joint-poses`` +on the publisher. Use a separate output file. Live preview does not read or +write MCAP; replay does not require OpenXR. ``--host`` and ``--port`` control +browser access. + +Recording preserves received payloads without FK. Viewing rotations uses +``SomaHandSource`` FK against prepared neutral models. Joint poses map directly +without loading models. The viewer colors left green and right blue and hides +unavailable joints and bones. + +``--soma-left-hand-collection-id`` and ``--soma-right-hand-collection-id`` default +to ``soma_hand_left_demo`` and ``soma_hand_right_demo``, matching the publisher's +``--left-collection-id`` and ``--right-collection-id``. Replay reads each +collection's ``soma_hand_joint_rotations_tracked`` or +``soma_hand_joint_poses_tracked`` channel. With no file argument, it selects the +newest ``soma_hand__*.mcap`` under ``./recordings/``. + +The trackers retain independent, one-sample-per-update replay. Pacing defaults +to the busier selected hand's average capture rate; ``--rate`` overrides it. +Missing or exhausted hands are hidden, and playback repeats after the longer +channel finishes. No timestamp synchronization or resampling is added. + API Reference ------------- diff --git a/examples/mcap_record_replay/README.md b/examples/mcap_record_replay/README.md index 33d1cc9d4a..b0af4a23e2 100644 --- a/examples/mcap_record_replay/README.md +++ b/examples/mcap_record_replay/README.md @@ -29,14 +29,148 @@ keep a viewer local. Replay repeats until Ctrl+C; no loop flag is needed. | Channel | Live | Record | Replay | | --- | --- | --- | --- | -| Hands | `live_hand` | `record_hand` | `replay_hand` | +| Hands (OpenXR or SOMA) | `live_hand` | `record_hand` | `replay_hand` | | Controllers | `live_controller` | `record_controller` | `replay_controller` | -| Full body | `live_full_body` | `record_full_body` | `replay_full_body` | +| Full body (FullBodyPose or SOMA) | `live_full_body` | `record_full_body` | `replay_full_body` | | Raw hand joint SE3 poses | `live_joint_se3_pose` | `record_joint_se3_pose` | `replay_joint_se3_pose` | | VIVE SE3 trackers | — | `record_se3_vive` | `replay_se3_vive` | `record_*` takes an optional duration in seconds and an optional output path. Recording needs a live OpenXR runtime; replay needs only the file. +The body commands default to `--body-schema full-body-pose`, including +controllers. The hand commands default to `--hand-schema openxr-hand-pose`. +Select SOMA with `--body-schema soma` or `--hand-schema soma`. These options +and the `--soma-*` options below match the DeviceIO live viewer. + +SOMA body mode uses only the body stream by default, including any available +finger joints. Pass `--controllers` to include controller recording, or +`--no-controllers` to disable it for FullBodyPose. Standalone hand streams use +the separate hand commands; they are not extracted from or fused with the body. + +## SOMA body + +Install the consumer and the existing demo publisher into your configured +development environment: + +```bash +uv pip install -e ./examples/mcap_record_replay +uv pip install -e ./examples/soma_body_publisher +``` + +Start the live-only viewer, then connect a CloudXR client: + +```bash +uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.live_full_body \ + --accept-eula --body-schema soma --soma-body-representation joint-rotations +``` + +In another terminal, start the publisher after the runtime is running: + +```bash +source ~/.cloudxr/run/cloudxr.env +uv run --no-sync python -m isaaccapture_examples.soma_body_publisher \ + --body-representation joint-rotations --loop +``` + +Record the received SOMA data in a third terminal: + +```bash +uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.record_full_body \ + 10 recordings/soma_body_joint-rotations_demo.mcap \ + --accept-eula --body-schema soma --soma-body-representation joint-rotations +``` + +Stop the live viewer and publisher before replaying on the same Viser port. +Replay does not need a runtime or client: + +```bash +uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.replay_full_body \ + recordings/soma_body_joint-rotations_demo.mcap \ + --body-schema soma --soma-body-representation joint-rotations +``` + +For evaluated joint poses, use `--soma-body-representation joint-poses` on +the viewer, recorder, and replay commands, and `--body-representation joint-poses` +on the publisher. Use a separate output file, such as +`recordings/soma_body_joint-poses_demo.mcap`. Both paths render +SOMA's native 77-joint topology, including available finger joints. Unavailable +joints and their connecting bones are hidden. + +Recording stores the received schema unchanged. Joint rotations use the +existing `SomaBodySource` FK helper with a prepared neutral model when viewing; +joint poses map directly without loading a model or running FK. + +`--soma-body-collection-id` selects the input collection and MCAP channel base. +It defaults to `soma_body_demo`, matching the publisher's `--collection-id`. +Recordings contain +`/soma_body_joint_rotations` and its `_tracked` channel, or the +corresponding `soma_body_joint_poses` pair. Replay consumes the tracked channel. +Without a file argument, replay selects the newest recording matching its +representation under `./recordings/`. + +Replay advances one tracked sample per update. Viewer pacing defaults to the +selected channel's average capture rate; `--rate` overrides it (`0` is unpaced). +This does not resample or synchronize streams. FullBodyPose and OpenXR hand +replay retain their existing 60 Hz pacing and duration-based loop. + +## SOMA hands + +Install the existing hand demo publisher alongside the MCAP example: + +```bash +uv pip install -e ./examples/mcap_record_replay +uv pip install -e ./examples/soma_hand_publisher +``` + +Start the live-only viewer and connect a CloudXR client: + +```bash +uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.live_hand \ + --accept-eula --hand-schema soma --soma-hand-representation joint-rotations +``` + +After the runtime is running, start the publisher in a second terminal: + +```bash +source ~/.cloudxr/run/cloudxr.env +uv run --no-sync python -m isaaccapture_examples.soma_hand_publisher \ + --hand-representation joint-rotations --loop +``` + +Record in a third terminal. Stop the viewer and publisher before replaying on +the same Viser port: + +```bash +uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.record_hand \ + 10 recordings/soma_hand_joint-rotations_demo.mcap \ + --accept-eula --hand-schema soma --soma-hand-representation joint-rotations + +uv run --no-sync python -m isaaccapture_examples.mcap_record_replay.replay_hand \ + recordings/soma_hand_joint-rotations_demo.mcap \ + --hand-schema soma --soma-hand-representation joint-rotations +``` + +For evaluated poses, use `--soma-hand-representation joint-poses` on the +viewer, recorder, and replay commands, and `--hand-representation joint-poses` +on the publisher. Use a separate output file. The viewer renders each hand's +native 25-joint Wrist/finger skeleton, with left green and right blue. Joint rotations +use `SomaHandSource` FK against prepared neutral models; joint poses map directly +without loading models. Recording does not run FK in either mode. + +`--soma-left-hand-collection-id` and `--soma-right-hand-collection-id` default to +`soma_hand_left_demo` and `soma_hand_right_demo`, matching the publisher's +`--left-collection-id` and `--right-collection-id`. Each collection is recorded +separately with its anatomical `hand_side` intact. Replay reads its +`soma_hand_joint_rotations_tracked` or `soma_hand_joint_poses_tracked` channel. +Without a file argument, it selects the newest +`soma_hand__*.mcap` under `./recordings/`. + +Left and right trackers advance independently once per replay update, following +the existing native behavior. Viewer pacing defaults to the busier selected +hand's average capture rate; `--rate` overrides it. Missing or exhausted hands +are hidden, and each pass ends when the longer channel finishes. No timestamp +synchronization, resampling, or body/hand fusion is performed. + A C++ recorder lives in `cpp/`. Docs: `docs/source/references/mcap_record_replay.rst`. diff --git a/examples/mcap_record_replay/pyproject.toml b/examples/mcap_record_replay/pyproject.toml index d6bb76e974..03ca5db9b7 100644 --- a/examples/mcap_record_replay/pyproject.toml +++ b/examples/mcap_record_replay/pyproject.toml @@ -18,6 +18,7 @@ dependencies = [ "isaaccapture[cloudxr]", "mcap>=1.2.0", "numpy>=1.23.0", + "py-soma-x==0.3.1", "viser>=0.2.0", ] @@ -32,5 +33,5 @@ sources = ["python"] [tool.uv.sources] # Editable, so edits under src/python/ need no rebuild. Dropped from the # installed copy, which has no repo above it. -# TODO(#880): nothing runs this example in CI; neither path is covered. +# TODO(#880): the example install paths are not covered in CI. isaaccapture = { path = "../..", editable = true } diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/common.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/common.py index ec43b54ae1..76f4fd10ad 100644 --- a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/common.py +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/common.py @@ -12,9 +12,18 @@ Rendering helpers: ``HandJoints``, ``HAND_BONES``, ``BODY_BONES``. """ +import argparse +import time +from pathlib import Path + import numpy as np import viser +from mcap.reader import make_reader +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.deviceio_session import DeviceIOSession, McapRecordingConfig +from isaaccapture.deviceio import McapReplayConfig +from isaaccapture.oxr import OpenXRSession from isaaccapture.retargeting_engine.deviceio_source_nodes import ( ControllersSource, FullBodySource, @@ -41,11 +50,212 @@ from isaaccapture.retargeting_engine.tensor_types.indices import ( BodyJointIndex, ControllerInputIndex, + FullBodyInputIndex, ) from isaaccapture.retargeting_engine.tensor_types.ndarray_types import ( DLDataType, NDArrayType, ) +from isaaccapture.teleop_session_manager import ( + SessionMode, + TeleopSession, + TeleopSessionConfig, +) + + +def add_body_arguments(parser): + from .soma_body_common import add_body_arguments as add_soma_arguments + + parser.add_argument( + "--body-schema", choices=["full-body-pose", "soma"], default="full-body-pose" + ) + parser.add_argument( + "--controllers", + action=argparse.BooleanOptionalAction, + default=None, + help="Include controllers (default: enabled for FullBodyPose, disabled for SOMA)", + ) + add_soma_arguments(parser) + + +def validate_body_arguments(parser, args): + if args.body_schema == "soma" and not args.soma_body_collection_id: + parser.error("soma-body-collection-id must not be empty") + if args.controllers is None: + args.controllers = args.body_schema == "full-body-pose" + + +def add_hand_arguments(parser): + from .soma_hand_common import add_hand_arguments as add_soma_arguments + + parser.add_argument( + "--hand-schema", + choices=["openxr-hand-pose", "soma"], + default="openxr-hand-pose", + ) + add_soma_arguments(parser) + + +def validate_hand_arguments(parser, args): + if args.hand_schema == "soma": + from .soma_hand_common import collections + + try: + collections( + args.soma_left_hand_collection_id, args.soma_right_hand_collection_id + ) + except ValueError as error: + parser.error(str(error)) + + +def build_body_pipeline(args): + # Select the body independently; fingers in SOMA do not enable another hand source. + if args.body_schema == "soma": + from .soma_body_common import build_pipeline + + pipeline = build_pipeline( + args.soma_body_collection_id, args.soma_body_representation + ) + if args.controllers: + controllers = ControllersSource(name="controllers") + return OutputCombiner( + { + "soma_body": pipeline.output("soma_body"), + "controller_left": controllers.output(ControllersSource.LEFT), + "controller_right": controllers.output(ControllersSource.RIGHT), + } + ) + return pipeline + return build_full_body_pipeline(args.controllers) + + +def create_body_visualizer(args, server, ground=None): + if args.body_schema == "soma": + from .soma_body_common import SomaBodyViz + + return SomaBodyViz(server, ground) + return FullBodyViz(server, ground) + + +def update_body_visualizer(args, result, viz): + if args.body_schema == "soma": + return viz.update(result["soma_body"]) + body = result["full_body"] + if body.is_none: + viz.update(None, None) + return 0 + valid = np.asarray(body[FullBodyInputIndex.JOINT_VALID], dtype=np.uint8) + viz.update(np.asarray(body[FullBodyInputIndex.JOINT_POSITIONS]), valid) + return int(np.count_nonzero(valid)) + + +def build_selected_hand_pipeline(args): + if args.hand_schema == "soma": + from .soma_hand_common import build_pipeline, collections + + return build_pipeline( + collections( + args.soma_left_hand_collection_id, args.soma_right_hand_collection_id + ), + args.soma_hand_representation, + ) + return build_hand_pipeline() + + +def create_hand_visualizers(args, server, ground=None): + if args.hand_schema == "soma": + from .soma_hand_common import create_visualizers + + return create_visualizers(server, ground) + return { + "hand_left": HandViz(server, "hand_left", LEFT_COLOR), + "hand_right": HandViz(server, "hand_right", RIGHT_COLOR), + } + + +def update_hand_visualizers(args, result, visualizers): + valid = [] + for side in ("left", "right"): + viz = visualizers[f"hand_{side}"] + if args.hand_schema == "soma": + valid.append(bool(viz.update(result[f"hand_{side}"]))) + else: + available = bool(result[f"{side}_valid"][0]) + viz.update(np.asarray(result[f"{side}_positions"][0]), available) + valid.append(available) + return valid + + +def record_trackers(args, path, tracker_names): + """Record native inputs without preparing a model or evaluating a source graph.""" + trackers = [tracker for tracker, _ in tracker_names] + config = McapRecordingConfig(str(path), tracker_names) + extensions = DeviceIOSession.get_required_extensions(trackers) + try: + with CloudXRLauncher.launch_context(args): + with OpenXRSession("McapRecordExample", extensions) as oxr: + with DeviceIOSession.run( + trackers, oxr.get_handles(), config + ) as session: + start = time.monotonic() + frames = 0 + while time.monotonic() - start < args.duration: + session.update() + frames += 1 + if frames % 60 == 0: + print(f"[record] frame={frames}") + time.sleep(1 / 60) + except KeyboardInterrupt: + pass + + +def resolve_recording(path_arg, prefix): + if path_arg: + path = Path(path_arg) + if not path.is_file(): + raise FileNotFoundError(f"{path} does not exist") + return path + recordings = Path.cwd() / "recordings" + candidates = list(recordings.glob(f"{prefix}_*.mcap")) + if not candidates: + raise FileNotFoundError(f"No {prefix}_*.mcap in {recordings}") + return max(candidates, key=lambda path: path.stat().st_mtime) + + +def channel_info(path, topic, record_name): + """Return the selected channel's sample count and average capture rate.""" + count = 0 + first = last = None + with path.open("rb") as stream: + for schema, _, message in make_reader(stream).iter_messages( + topics=[topic], log_time_order=False + ): + if schema is None or schema.name != record_name: + raise ValueError(f"{topic}: expected {record_name}") + first = message.log_time if first is None else min(first, message.log_time) + last = message.log_time if last is None else max(last, message.log_time) + count += 1 + rate = (count - 1) * 1e9 / (last - first) if count > 1 and last > first else 60.0 + return count, rate + + +def replay_once(path, pipeline, frame_count, update, rate_hz): + config = TeleopSessionConfig( + app_name="McapReplayExample", + pipeline=pipeline, + mode=SessionMode.REPLAY, + mcap_config=McapReplayConfig(str(path)), + ) + # Native replay consumes one tracked record per step. None is not an EOF signal. + with TeleopSession(config) as session: + start = time.monotonic() + for frame in range(frame_count): + update(session.step()) + if rate_hz > 0 and frame + 1 < frame_count: + remaining = start + (frame + 1) / rate_hz - time.monotonic() + if remaining > 0: + time.sleep(remaining) + return frame_count # Duplicated verbatim in deviceio_live_view/deviceio_viser.py: each example @@ -204,16 +414,18 @@ def build_controller_pipeline(): ) -def build_full_body_pipeline(): - controllers = ControllersSource(name="controllers") +def build_full_body_pipeline(include_controllers=True): full_body = FullBodySource(name="full_body") - return OutputCombiner( - { - "controller_left": controllers.output(ControllersSource.LEFT), - "controller_right": controllers.output(ControllersSource.RIGHT), - "full_body": full_body.output(FullBodySource.FULL_BODY), - } - ) + outputs = {"full_body": full_body.output(FullBodySource.FULL_BODY)} + if include_controllers: + controllers = ControllersSource(name="controllers") + outputs.update( + { + "controller_left": controllers.output(ControllersSource.LEFT), + "controller_right": controllers.output(ControllersSource.RIGHT), + } + ) + return OutputCombiner(outputs) # PICO body-joint connectivity (parent → child) for skeleton rendering. @@ -302,10 +514,12 @@ def _bone_segments(positions: np.ndarray) -> np.ndarray: ).astype(np.float32) -def _valid_bone_segments(positions: np.ndarray, valid: np.ndarray) -> np.ndarray: - """Return (N, 2, 3) segment array for body bones whose both endpoints are valid.""" +def _valid_bone_segments( + positions: np.ndarray, valid: np.ndarray, bones=BODY_BONES +) -> np.ndarray: + """Return (N, 2, 3) segments whose endpoints are both available.""" 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: @@ -521,48 +735,105 @@ 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 SkeletonViz: + """Viser joint cloud and bones with a schema-specific layout and validity mask.""" - def __init__(self, server: viser.ViserServer, ground: "GroundGrid | None" = None): + def __init__( + self, + server, + ground=None, + *, + name="full_body", + joint_count=24, + bones=BODY_BONES, + color=TRACKED_COLOR, + hide_invalid=False, + ): self._ground = ground - self.color = np.array(TRACKED_COLOR, dtype=np.float32) - zero_pts = np.zeros((len(BODY_JOINT_NAMES), 3), dtype=np.float32) + self._joint_count = joint_count + self._bones = bones + self._hide_invalid = hide_invalid + self.color = np.array(color, dtype=np.float32) + zero_pts = np.zeros((0 if hide_invalid else joint_count, 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=f"/{name}/joints", points=zero_pts, - colors=np.tile(self.color, (len(BODY_JOINT_NAMES), 1)), + colors=np.tile(self.color, (len(zero_pts), 1)), point_size=0.01, ) self.bones = server.scene.add_line_segments( - name="/full_body/bones", + name=f"/{name}/bones", points=zero_segs, colors=np.zeros((0, 2, 3), dtype=np.float32), line_width=2.0, ) - def update(self, positions: np.ndarray | None, valid: np.ndarray | None) -> None: + def update(self, positions: np.ndarray | None, valid: np.ndarray | None) -> int: if positions is None or valid is None: - zero_pts = np.zeros((len(BODY_JOINT_NAMES), 3), dtype=np.float32) + zero_pts = np.zeros( + (0 if self._hide_invalid else self._joint_count, 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(zero_pts), 1)) self.bones.points = np.zeros((0, 2, 3), dtype=np.float32) self.bones.colors = np.zeros((0, 2, 3), dtype=np.float32) - return + return 0 positions = positions.astype(np.float32) valid_bool = valid.astype(bool) - self.points.points = positions + self.points.points = positions[valid_bool] if self._hide_invalid else positions point_colors = np.tile(self.color, (positions.shape[0], 1)) point_colors[~valid_bool] = INVALID_COLOR - self.points.colors = point_colors + self.points.colors = ( + point_colors[valid_bool] if self._hide_invalid else point_colors + ) - segs = _valid_bone_segments(positions, valid_bool) + segs = _valid_bone_segments(positions, valid_bool, self._bones) self.bones.points = segs self.bones.colors = np.tile(self.color, (segs.shape[0], 2, 1)) if self._ground is not None: self._ground.follow(positions, valid_bool) + return int(np.count_nonzero(valid_bool)) + + +class FullBodyViz(SkeletonViz): + """Render FullBodyPose's 24 joints, coloring unavailable joints red.""" + + +class JointInputViz(SkeletonViz): + """Render an optional evaluated joint input, hiding unavailable joints.""" + + def __init__( + self, + server, + name, + joint_count, + bones, + positions_index, + valid_index, + color, + ground=None, + ): + super().__init__( + server, + ground, + name=name, + joint_count=joint_count, + bones=bones, + color=color, + hide_invalid=True, + ) + self._positions_index = positions_index + self._valid_index = valid_index + + def update(self, data): + if data.is_none: + return super().update(None, None) + return super().update( + np.asarray(data[self._positions_index], dtype=np.float32), + np.asarray(data[self._valid_index], dtype=bool), + ) diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/live_full_body.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/live_full_body.py index 336c2239c6..e46aee2b78 100644 --- a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/live_full_body.py +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/live_full_body.py @@ -2,13 +2,13 @@ # SPDX-License-Identifier: Apache-2.0 """ -Visualize live OpenXR full-body pose tracking in real time with viser. +Visualize live FullBodyPose or SOMA body tracking in real time with viser. ``CloudXRLauncher`` starts the CloudXR runtime and WSS proxy automatically. Open the URL viser prints in a browser (binds all interfaces, so another machine can reach it at http://:8080) to see -the full PICO body skeleton — joints colored green when valid, red when lost — -updating live as you move. +the selected skeleton updating live. FullBodyPose renders the 24-joint PICO +layout; SOMA renders its native 77 joints and hides unavailable joints. Usage: python -m isaaccapture_examples.mcap_record_replay.live_full_body [--port 8080] [--host 127.0.0.1] [--accept-eula] @@ -22,14 +22,19 @@ import sys import time -import numpy as np import viser from isaaccapture.cloudxr import CloudXRLauncher -from isaaccapture.retargeting_engine.tensor_types.indices import FullBodyInputIndex from isaaccapture.teleop_session_manager import TeleopSession, TeleopSessionConfig -from .common import BODY_JOINT_NAMES, FullBodyViz, build_full_body_pipeline, setup_scene +from .common import ( + add_body_arguments, + validate_body_arguments, + build_body_pipeline, + create_body_visualizer, + update_body_visualizer, + setup_scene, +) def main(argv: list[str]) -> int: @@ -40,15 +45,19 @@ 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") + add_body_arguments(parser) CloudXRLauncher.add_launcher_arguments(parser) args = parser.parse_args(argv[1:]) + validate_body_arguments(parser, args) + + pipeline = build_body_pipeline(args) server = viser.ViserServer(host=args.host, port=args.port) ground = setup_scene(server) config = TeleopSessionConfig( app_name="LiveFullBodyExample", - pipeline=build_full_body_pipeline(), + pipeline=pipeline, ) with CloudXRLauncher.launch_context(args) as launcher: @@ -57,7 +66,7 @@ def main(argv: list[str]) -> int: print("[live] waiting for headset connection… (Ctrl+C to stop)") with TeleopSession(config) as session: - viz = FullBodyViz(server, ground) + viz = create_body_visualizer(args, server, ground) print( f"[live] viser listening on {args.host}:{args.port} " f"(http://localhost:{args.port})" @@ -65,31 +74,17 @@ def main(argv: list[str]) -> int: try: while True: result = session.step() - full_body = result["full_body"] - - if full_body.is_none: - viz.update(None, None) - n_valid = 0 - else: - positions = np.asarray( - full_body[FullBodyInputIndex.JOINT_POSITIONS], - dtype=np.float32, - ) - valid = np.asarray( - full_body[FullBodyInputIndex.JOINT_VALID], dtype=np.uint8 - ) - viz.update(positions, valid) - n_valid = int(np.count_nonzero(valid)) + n_valid = update_body_visualizer(args, result, viz) if session.frame_count % 60 == 0: print( - f"[live] frame={session.frame_count} " - f"joints={n_valid:02d}/{len(BODY_JOINT_NAMES)}" + f"[live] frame={session.frame_count} joints={n_valid:02d}" ) time.sleep(1 / 60) except KeyboardInterrupt: pass + server.stop() print("[live] stopped") return 0 diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/live_hand.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/live_hand.py index 3130c4f7c6..44e275d715 100644 --- a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/live_hand.py +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/live_hand.py @@ -2,7 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 """ -Visualize live OpenXR hand-tracking in real time with viser. +Visualize live OpenXR or SOMA hand tracking in real time with viser. ``CloudXRLauncher`` starts the CloudXR runtime and WSS proxy automatically. Open the URL viser prints in a browser (binds all interfaces, so another @@ -21,13 +21,19 @@ import sys import time -import numpy as np import viser from isaaccapture.cloudxr import CloudXRLauncher from isaaccapture.teleop_session_manager import TeleopSession, TeleopSessionConfig -from .common import HandViz, LEFT_COLOR, RIGHT_COLOR, build_hand_pipeline, setup_scene +from .common import ( + add_hand_arguments, + validate_hand_arguments, + build_selected_hand_pipeline, + create_hand_visualizers, + update_hand_visualizers, + setup_scene, +) def main(argv: list[str]) -> int: @@ -38,15 +44,19 @@ 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") + add_hand_arguments(parser) CloudXRLauncher.add_launcher_arguments(parser) args = parser.parse_args(argv[1:]) + validate_hand_arguments(parser, args) + + pipeline = build_selected_hand_pipeline(args) server = viser.ViserServer(host=args.host, port=args.port) - setup_scene(server) + ground = setup_scene(server) config = TeleopSessionConfig( app_name="LiveHandExample", - pipeline=build_hand_pipeline(), + pipeline=pipeline, ) with CloudXRLauncher.launch_context(args) as launcher: @@ -55,8 +65,7 @@ def main(argv: list[str]) -> int: print("[live] waiting for headset connection… (Ctrl+C to stop)") with TeleopSession(config) as session: - viz_left = HandViz(server, "hand_left", LEFT_COLOR) - viz_right = HandViz(server, "hand_right", RIGHT_COLOR) + visualizers = create_hand_visualizers(args, server, ground) print( f"[live] viser listening on {args.host}:{args.port} " f"(http://localhost:{args.port})" @@ -70,18 +79,9 @@ def main(argv: list[str]) -> int: _last_step_t = now result = session.step() - viz_left.update( - np.asarray(result["left_positions"][0]), - bool(result["left_valid"][0]), - ) - viz_right.update( - np.asarray(result["right_positions"][0]), - bool(result["right_valid"][0]), - ) + left, right = update_hand_visualizers(args, result, visualizers) if session.frame_count % 60 == 0: - left = bool(result["left_valid"][0]) - right = bool(result["right_valid"][0]) print( f"[live] frame={session.frame_count} " f"L={'Y' if left else '-'} R={'Y' if right else '-'} " @@ -92,6 +92,7 @@ def main(argv: list[str]) -> int: except KeyboardInterrupt: pass + server.stop() print("[live] stopped") return 0 diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/record_full_body.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/record_full_body.py index 479b6b318d..8f1ace7492 100644 --- a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/record_full_body.py +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/record_full_body.py @@ -2,12 +2,13 @@ # SPDX-License-Identifier: Apache-2.0 """ -Record a live OpenXR full-body tracking session to an MCAP file. +Record live FullBodyPose or SOMA body tracking to an MCAP file. ``CloudXRLauncher`` starts the CloudXR runtime and WSS proxy automatically — no separate terminal or pre-running headset daemon is needed. The pipeline in ``common.py`` wires ``FullBodySource`` and ``ControllersSource``, so ``TeleopSession`` records the ``full_body`` and ``controllers`` channels. +SOMA records raw body payloads without FK; controllers are opt-in. Usage: python -m isaaccapture_examples.mcap_record_replay.record_full_body [duration_seconds] [output.mcap] [--accept-eula] @@ -18,6 +19,7 @@ """ import argparse +import math import sys import time from datetime import datetime @@ -27,10 +29,17 @@ from isaaccapture.cloudxr import CloudXRLauncher from isaaccapture.deviceio import McapRecordingConfig +from isaaccapture.deviceio import ControllerTracker from isaaccapture.retargeting_engine.tensor_types.indices import FullBodyInputIndex from isaaccapture.teleop_session_manager import TeleopSession, TeleopSessionConfig -from .common import BODY_JOINT_NAMES, build_full_body_pipeline +from .common import ( + BODY_JOINT_NAMES, + add_body_arguments, + validate_body_arguments, + build_body_pipeline, + record_trackers, +) def main(argv: list[str]) -> int: @@ -39,8 +48,12 @@ def main(argv: list[str]) -> int: "duration", nargs="?", type=float, default=5.0, help="Recording duration (s)" ) parser.add_argument("output", nargs="?", help="Output .mcap path") + add_body_arguments(parser) CloudXRLauncher.add_launcher_arguments(parser) args = parser.parse_args(argv[1:]) + validate_body_arguments(parser, args) + if not math.isfinite(args.duration) or args.duration <= 0: + parser.error("duration must be finite and positive") duration_s: float = args.duration @@ -50,13 +63,31 @@ def main(argv: list[str]) -> int: else: out_dir = Path.cwd() / "recordings" out_dir.mkdir(exist_ok=True) - mcap_path = out_dir / f"full_body_{datetime.now():%Y%m%d_%H%M%S}.mcap" + prefix = ( + f"soma_body_{args.soma_body_representation}" + if args.body_schema == "soma" + else "full_body" + ) + mcap_path = out_dir / f"{prefix}_{datetime.now():%Y%m%d_%H%M%S}.mcap" print(f"[record] writing {mcap_path} for {duration_s:.1f}s") + if args.body_schema == "soma": + from .soma_body_common import create_tracker + + tracker = create_tracker( + args.soma_body_collection_id, args.soma_body_representation + ) + tracker_names = [(tracker, args.soma_body_collection_id)] + if args.controllers: + tracker_names.append((ControllerTracker(), "controllers")) + record_trackers(args, mcap_path, tracker_names) + print(f"[record] done: {mcap_path}") + return 0 + config = TeleopSessionConfig( app_name="McapFullBodyRecordExample", - pipeline=build_full_body_pipeline(), + pipeline=build_body_pipeline(args), mcap_config=McapRecordingConfig(str(mcap_path)), ) diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/record_hand.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/record_hand.py index b27c590ed1..54c8d70733 100644 --- a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/record_hand.py +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/record_hand.py @@ -2,12 +2,13 @@ # SPDX-License-Identifier: Apache-2.0 """ -Record a live OpenXR hand-tracking session to an MCAP file. +Record live OpenXR or SOMA hand tracking to an MCAP file. ``CloudXRLauncher`` starts the CloudXR runtime and WSS proxy automatically — no separate terminal or pre-running headset daemon is needed. The pipeline in ``common.py`` wires only ``HandsSource``, so ``TeleopSession`` records exactly the ``hands`` channel — no head, no controllers. +SOMA records its separate left/right collections without running FK. Usage: python -m isaaccapture_examples.mcap_record_replay.record_hand [duration_seconds] [output.mcap] [--accept-eula] @@ -18,6 +19,7 @@ """ import argparse +import math import sys import time from datetime import datetime @@ -27,7 +29,12 @@ from isaaccapture.deviceio import McapRecordingConfig from isaaccapture.teleop_session_manager import TeleopSession, TeleopSessionConfig -from .common import build_hand_pipeline +from .common import ( + add_hand_arguments, + validate_hand_arguments, + build_hand_pipeline, + record_trackers, +) def main(argv: list[str]) -> int: @@ -36,8 +43,12 @@ def main(argv: list[str]) -> int: "duration", nargs="?", type=float, default=5.0, help="Recording duration (s)" ) parser.add_argument("output", nargs="?", help="Output .mcap path") + add_hand_arguments(parser) CloudXRLauncher.add_launcher_arguments(parser) args = parser.parse_args(argv[1:]) + validate_hand_arguments(parser, args) + if not math.isfinite(args.duration) or args.duration <= 0: + parser.error("duration must be finite and positive") duration_s: float = args.duration @@ -47,10 +58,28 @@ def main(argv: list[str]) -> int: else: out_dir = Path.cwd() / "recordings" out_dir.mkdir(exist_ok=True) - mcap_path = out_dir / f"hands_{datetime.now():%Y%m%d_%H%M%S}.mcap" + prefix = ( + f"soma_hand_{args.soma_hand_representation}" + if args.hand_schema == "soma" + else "hands" + ) + mcap_path = out_dir / f"{prefix}_{datetime.now():%Y%m%d_%H%M%S}.mcap" print(f"[record] writing {mcap_path} for {duration_s:.1f}s") + if args.hand_schema == "soma": + from .soma_hand_common import collections, create_trackers + + ids = collections( + args.soma_left_hand_collection_id, args.soma_right_hand_collection_id + ) + trackers = create_trackers(ids, args.soma_hand_representation) + record_trackers( + args, mcap_path, [(tracker, ids[key]) for key, tracker in trackers.items()] + ) + print(f"[record] done: {mcap_path}") + return 0 + config = TeleopSessionConfig( app_name="McapHandRecordExample", pipeline=build_hand_pipeline(), diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_full_body.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_full_body.py index c6a44642af..38f2727512 100644 --- a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_full_body.py +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_full_body.py @@ -2,7 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 """ -Replay a recorded full-body MCAP file and visualize it with viser. +Replay recorded FullBodyPose or SOMA body data and visualize it with viser. ``mode=SessionMode.REPLAY`` skips all OpenXR initialization, so this runs headless on any machine. Open the URL viser prints (default @@ -13,12 +13,14 @@ If no path is given, the newest ``full_body_*.mcap`` under ``./recordings/`` is used. +SOMA mode selects ``soma_body__*.mcap`` instead. Replay repeats until Ctrl+C. See: https://nvidia.github.io/IsaacCapture/main/references/mcap_record_replay.html """ import argparse +import math import sys import time from pathlib import Path @@ -35,7 +37,19 @@ TeleopSessionConfig, ) -from .common import BODY_JOINT_NAMES, FullBodyViz, build_full_body_pipeline, setup_scene +from .common import ( + BODY_JOINT_NAMES, + FullBodyViz, + build_full_body_pipeline, + setup_scene, + add_body_arguments, + validate_body_arguments, + build_body_pipeline, + create_body_visualizer, + update_body_visualizer, + resolve_recording, + replay_once, +) def mcap_duration_s(path: Path) -> float: @@ -77,11 +91,12 @@ def run_once( mcap_path: Path, duration_s: float, viz: FullBodyViz, + include_controllers: bool = True, ) -> int: """Play the file once for ``duration_s`` wall-clock seconds. Returns frame count.""" config = TeleopSessionConfig( app_name="McapFullBodyReplayExample", - pipeline=build_full_body_pipeline(), + pipeline=build_full_body_pipeline(include_controllers), mode=SessionMode.REPLAY, mcap_config=McapReplayConfig(str(mcap_path)), ) @@ -127,27 +142,66 @@ 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") + add_body_arguments(parser) + parser.add_argument( + "--rate", + type=float, + help="SOMA playback Hz (default: average capture rate); 0 = unpaced", + ) args = parser.parse_args(argv[1:]) - - mcap_path = resolve_mcap(args.mcap) - duration_s = mcap_duration_s(mcap_path) + validate_body_arguments(parser, args) + if args.rate is not None: + if args.body_schema != "soma": + parser.error("--rate is only supported with --body-schema soma") + if not math.isfinite(args.rate) or args.rate < 0: + parser.error("rate must be finite and nonnegative") + + if args.body_schema == "soma": + from .soma_body_common import replay_info + + try: + mcap_path = resolve_recording( + args.mcap, f"soma_body_{args.soma_body_representation}" + ) + count, capture_rate = replay_info( + mcap_path, args.soma_body_collection_id, args.soma_body_representation + ) + except (FileNotFoundError, ValueError) as error: + parser.error(str(error)) + rate = capture_rate if args.rate is None else args.rate + pipeline = build_body_pipeline(args) + print(f"[replay] reading {mcap_path}: {count} samples at {rate:.1f} Hz") + else: + mcap_path = resolve_mcap(args.mcap) + duration_s = mcap_duration_s(mcap_path) + print(f"[replay] reading {mcap_path} (duration {duration_s:.2f}s)") server = viser.ViserServer(host=args.host, port=args.port) ground = setup_scene(server) - viz = FullBodyViz(server, ground) + viz = create_body_visualizer(args, server, ground) print( f"[replay] viser listening on {args.host}:{args.port} " f"(http://localhost:{args.port})" ) - print(f"[replay] reading {mcap_path} (duration {duration_s:.2f}s)") try: while True: - run_once(mcap_path, duration_s, viz) + if args.body_schema == "soma": + replay_once( + mcap_path, + pipeline, + count, + lambda result: update_body_visualizer(args, result, viz), + rate, + ) + else: + run_once(mcap_path, duration_s, viz, args.controllers) print("[replay] looping…") except KeyboardInterrupt: pass + finally: + server.stop() return 0 diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_hand.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_hand.py index a08ad8f36f..b990549276 100644 --- a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_hand.py +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/replay_hand.py @@ -2,7 +2,7 @@ # SPDX-License-Identifier: Apache-2.0 """ -Replay a recorded hand-tracking MCAP file and visualize it with viser. +Replay recorded OpenXR or SOMA hand tracking and visualize it with viser. ``mode=SessionMode.REPLAY`` skips all OpenXR initialization, so this runs headless on any machine. Open the URL viser prints (default @@ -13,12 +13,14 @@ If no path is given, the newest ``hands_*.mcap`` under ``./recordings/`` is used. +SOMA mode selects ``soma_hand__*.mcap`` instead. Replay repeats until Ctrl+C. See: https://nvidia.github.io/IsaacCapture/main/references/mcap_record_replay.html """ import argparse +import math import sys import time from pathlib import Path @@ -34,7 +36,18 @@ TeleopSessionConfig, ) -from .common import HandViz, LEFT_COLOR, RIGHT_COLOR, build_hand_pipeline, setup_scene +from .common import ( + HandViz, + build_hand_pipeline, + setup_scene, + add_hand_arguments, + validate_hand_arguments, + build_selected_hand_pipeline, + create_hand_visualizers, + update_hand_visualizers, + resolve_recording, + replay_once, +) def mcap_duration_s(path: Path) -> float: @@ -122,29 +135,76 @@ 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") + add_hand_arguments(parser) + parser.add_argument( + "--rate", + type=float, + help="SOMA playback Hz (default: busier hand's average capture rate); 0 = unpaced", + ) args = parser.parse_args(argv[1:]) - - mcap_path = resolve_mcap(args.mcap) - duration_s = mcap_duration_s(mcap_path) + validate_hand_arguments(parser, args) + if args.rate is not None: + if args.hand_schema != "soma": + parser.error("--rate is only supported with --hand-schema soma") + if not math.isfinite(args.rate) or args.rate < 0: + parser.error("rate must be finite and nonnegative") + + if args.hand_schema == "soma": + from .soma_hand_common import collections, replay_info + + ids = collections( + args.soma_left_hand_collection_id, args.soma_right_hand_collection_id + ) + try: + mcap_path = resolve_recording( + args.mcap, f"soma_hand_{args.soma_hand_representation}" + ) + counts, capture_rate = replay_info( + mcap_path, ids, args.soma_hand_representation + ) + except (FileNotFoundError, ValueError) as error: + parser.error(str(error)) + rate = capture_rate if args.rate is None else args.rate + pipeline = build_selected_hand_pipeline(args) + print( + f"[replay] reading {mcap_path}: left={counts['hand_left']} right={counts['hand_right']} at {rate:.1f} Hz" + ) + else: + mcap_path = resolve_mcap(args.mcap) + duration_s = mcap_duration_s(mcap_path) + print(f"[replay] reading {mcap_path} (duration {duration_s:.2f}s)") server = viser.ViserServer(host=args.host, port=args.port) - setup_scene(server) - - viz_left = HandViz(server, "hand_left", LEFT_COLOR) - viz_right = HandViz(server, "hand_right", RIGHT_COLOR) + visualizers = create_hand_visualizers(args, server, setup_scene(server)) print( f"[replay] viser listening on {args.host}:{args.port} " f"(http://localhost:{args.port})" ) - print(f"[replay] reading {mcap_path} (duration {duration_s:.2f}s)") try: while True: - run_once(mcap_path, duration_s, viz_left, viz_right) + if args.hand_schema == "soma": + # Each tracker advances independently; exhausted hands become absent. + replay_once( + mcap_path, + pipeline, + max(counts.values()), + lambda result: update_hand_visualizers(args, result, visualizers), + rate, + ) + else: + run_once( + mcap_path, + duration_s, + visualizers["hand_left"], + visualizers["hand_right"], + ) print("[replay] looping…") except KeyboardInterrupt: pass + finally: + server.stop() return 0 diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/soma_body_common.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/soma_body_common.py new file mode 100644 index 0000000000..9b47f18b9f --- /dev/null +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/soma_body_common.py @@ -0,0 +1,209 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""SOMA body pipeline and native skeleton rendering for this example.""" + +from isaaccapture.deviceio_trackers import ( + SomaBodyJointPosesTracker, + SomaBodyJointRotationsTracker, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + SomaBodyRepresentation, + SomaBodySource, +) +from isaaccapture.retargeting_engine.interface import OutputCombiner +from isaaccapture.retargeting_engine.tensor_types import SomaBodyInputIndex +from isaaccapture.schema import SomaBodyJoint + +from .common import TRACKED_COLOR, JointInputViz, channel_info + +DEFAULT_COLLECTION = "soma_body_demo" +JOINT_NAMES = tuple( + name + for name, joint in sorted( + SomaBodyJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" +) + +# Matches the DeviceIO viewer's SOMA-X v0.3.1 hierarchy, with virtual Root omitted. +# Each example is independently installable, so keep the layout local. +PARENT_IDS = ( + -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, +) +BONES = tuple((parent, child) for child, parent in enumerate(PARENT_IDS) if parent >= 0) + + +def add_body_arguments(parser): + parser.add_argument( + "--soma-body-representation", + choices=[representation.value for representation in SomaBodyRepresentation], + default=SomaBodyRepresentation.JOINT_ROTATIONS.value, + help="SOMA wire representation (default: joint-rotations)", + ) + parser.add_argument( + "--soma-body-collection-id", + default=DEFAULT_COLLECTION, + help="Publisher collection and MCAP channel base (default: soma_body_demo)", + ) + + +def create_tracker(collection_id, representation): + representation = SomaBodyRepresentation(representation) + if not collection_id: + raise ValueError("collection-id must not be empty") + tracker_type = ( + SomaBodyJointRotationsTracker + if representation is SomaBodyRepresentation.JOINT_ROTATIONS + else SomaBodyJointPosesTracker + ) + return tracker_type(collection_id) + + +def create_layer(): + import soma + import torch + + # py-soma-x 0.3.1's body-layer fallback imports nonexistent soma.body.assets. + layer = soma.SOMALayer( + data_root=str(soma.get_assets_dir()), + identity_model_type="soma", + device="cpu", + lod="low", + output_unit=soma.Unit.METERS, + enable_procedural_transforms=False, + correctives_model_path=None, + ) + parent_ids = tuple( + -1 if parent == 0 else int(parent) - 1 + for parent in layer.output_joint_parent_ids[1:] + ) + if parent_ids != 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 build_pipeline(collection_id, representation): + representation = SomaBodyRepresentation(representation) + layer = ( + create_layer() + if representation is SomaBodyRepresentation.JOINT_ROTATIONS + else None + ) + source = SomaBodySource( + collection_id, collection_id, layer, representation=representation + ) + return OutputCombiner({SomaBodySource.BODY: source.output(SomaBodySource.BODY)}) + + +def replay_info(path, collection_id, representation): + """Count the selected tracked channel and estimate its average capture rate.""" + suffix = "soma_body_" + representation.replace("-", "_") + topic = f"{collection_id}/{suffix}_tracked" + record_name = ( + "core.SomaBodyJointRotationsRecord" + if representation == "joint-rotations" + else "core.SomaBodyJointPosesRecord" + ) + count, rate = channel_info(path, topic, record_name) + if count == 0: + raise ValueError( + f"{path}: no samples on {topic}; check soma-body-collection-id and soma-body-representation" + ) + return count, rate + + +class SomaBodyViz(JointInputViz): + """Render available joints in SOMA's 77-joint order, including fingers.""" + + def __init__(self, server, ground=None): + super().__init__( + server, + "soma_body", + len(JOINT_NAMES), + BONES, + SomaBodyInputIndex.JOINT_POSITIONS, + SomaBodyInputIndex.JOINT_VALID, + TRACKED_COLOR, + ground, + ) diff --git a/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/soma_hand_common.py b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/soma_hand_common.py new file mode 100644 index 0000000000..caeb4974e4 --- /dev/null +++ b/examples/mcap_record_replay/python/isaaccapture_examples/mcap_record_replay/soma_hand_common.py @@ -0,0 +1,171 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Standalone SOMA hand sources and native layout for this example.""" + +from isaaccapture.deviceio_trackers import ( + SomaHandJointPosesTracker, + SomaHandJointRotationsTracker, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + SomaHandRepresentation, + SomaHandSource, +) +from isaaccapture.retargeting_engine.interface import OutputCombiner +from isaaccapture.retargeting_engine.tensor_types import SomaHandInputIndex +from isaaccapture.schema import SomaHandSide, SomaHandJoint + +from .common import LEFT_COLOR, RIGHT_COLOR, JointInputViz, channel_info + +DEFAULT_LEFT_COLLECTION = "soma_hand_left_demo" +DEFAULT_RIGHT_COLLECTION = "soma_hand_right_demo" +HAND_SIDES = {"hand_left": SomaHandSide.LEFT, "hand_right": SomaHandSide.RIGHT} +JOINT_NAMES = tuple( + name + for name, joint in sorted( + SomaHandJoint.__members__.items(), key=lambda item: int(item[1]) + ) + if name != "NUM_JOINTS" +) +# Matches the DeviceIO viewer's 25-joint Wrist/finger layout, without an OpenXR Palm. +BONES = ( + (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), +) + + +def add_hand_arguments(parser): + parser.add_argument( + "--soma-hand-representation", + choices=[profile.value for profile in SomaHandRepresentation], + default=SomaHandRepresentation.JOINT_ROTATIONS.value, + help="SOMA wire representation (default: joint-rotations)", + ) + parser.add_argument( + "--soma-left-hand-collection-id", + default=DEFAULT_LEFT_COLLECTION, + help="Left publisher collection and MCAP channel base", + ) + parser.add_argument( + "--soma-right-hand-collection-id", + default=DEFAULT_RIGHT_COLLECTION, + help="Right publisher collection and MCAP channel base", + ) + + +def collections(left, right): + if not left or not right or left == right: + raise ValueError("left and right collection IDs must be nonempty and distinct") + return {"hand_left": left, "hand_right": right} + + +def create_trackers(collection_ids, representation): + representation = SomaHandRepresentation(representation) + tracker_type = ( + SomaHandJointRotationsTracker + if representation is SomaHandRepresentation.JOINT_ROTATIONS + else SomaHandJointPosesTracker + ) + return { + key: tracker_type(collection_id) + for key, collection_id in collection_ids.items() + } + + +def create_layer(hand_side): + import soma + import torch + + hand_type = "left" if hand_side == SomaHandSide.LEFT else "right" + layer = soma.SOMAHandLayer( + data_root=str(soma.get_assets_dir()), + hand_type=hand_type, + device="cpu", + identity_model_type="soma", + lod="low", + output_unit=soma.Unit.METERS, + correctives_model_path=None, + ) + with torch.no_grad(): + layer.prepare_identity( + torch.zeros((1, layer.identity_model.num_identity_coeffs)) + ) + return layer + + +def build_pipeline(collection_ids, representation): + representation = SomaHandRepresentation(representation) + outputs = {} + for key, collection_id in collection_ids.items(): + hand_side = HAND_SIDES[key] + layer = ( + create_layer(hand_side) + if representation is SomaHandRepresentation.JOINT_ROTATIONS + else None + ) + source = SomaHandSource( + collection_id, + collection_id, + hand_side, + layer, + representation=representation, + ) + outputs[key] = source.output(SomaHandSource.HAND) + return OutputCombiner(outputs) + + +def create_visualizers(server, ground=None): + return { + key: JointInputViz( + server, + key, + len(JOINT_NAMES), + BONES, + SomaHandInputIndex.JOINT_POSITIONS, + SomaHandInputIndex.JOINT_VALID, + LEFT_COLOR if hand_side == SomaHandSide.LEFT else RIGHT_COLOR, + ground, + ) + for key, hand_side in HAND_SIDES.items() + } + + +def replay_info(path, collection_ids, representation): + suffix = "soma_hand_" + representation.replace("-", "_") + record_name = ( + "core.SomaHandJointRotationsRecord" + if representation == "joint-rotations" + else "core.SomaHandJointPosesRecord" + ) + info = { + key: channel_info(path, f"{collection_id}/{suffix}_tracked", record_name) + for key, collection_id in collection_ids.items() + } + busiest = max(info.values(), key=lambda item: item[0]) + if busiest[0] == 0: + raise ValueError( + f"{path}: no SOMA hand samples; check collection IDs and soma-hand-representation" + ) + return {key: count for key, (count, _) in info.items()}, busiest[1] diff --git a/tests/python/examples/CMakeLists.txt b/tests/python/examples/CMakeLists.txt index a0cc7b40cc..eefa4080b5 100644 --- a/tests/python/examples/CMakeLists.txt +++ b/tests/python/examples/CMakeLists.txt @@ -3,5 +3,6 @@ add_subdirectory(camera_viz) add_subdirectory(deviceio_live_view) +add_subdirectory(mcap_record_replay) add_subdirectory(soma_body_publisher) add_subdirectory(soma_hand_publisher) diff --git a/tests/python/examples/mcap_record_replay/CMakeLists.txt b/tests/python/examples/mcap_record_replay/CMakeLists.txt new file mode 100644 index 0000000000..1aa0596cd5 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/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 "mcap_record_replay_${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("mcap_record_replay_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" + LABELS "mcap_record_replay;unit" + ) +endforeach() diff --git a/tests/python/examples/mcap_record_replay/conftest.py b/tests/python/examples/mcap_record_replay/conftest.py new file mode 100644 index 0000000000..82ff4286ec --- /dev/null +++ b/tests/python/examples/mcap_record_replay/conftest.py @@ -0,0 +1,159 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import MagicMock + +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" / "mcap_record_replay" / "python")) + + +@pytest.fixture +def command_clock(monkeypatch): + def patch(module, timestamps): + ticks = iter(timestamps) + clock = SimpleNamespace(time=lambda: next(ticks), sleep=MagicMock()) + # Replace the command's clock, not global time used by native log receivers. + monkeypatch.setattr(module, "time", clock) + return clock + + return patch + + +def write_streams(path, streams): + from mcap.writer import CompressionType, Writer + + with path.open("wb") as stream: + # The native recorder and reader use uncompressed MCAP. + writer = Writer(stream, compression=CompressionType.NONE) + writer.start() + for suffix, collection, record_type, payloads, timestamps in streams: + bfbs = ( + repo_root() / "src/core/schema/golden" / f"{suffix}.bfbs" + ).read_bytes() + schema_id = writer.register_schema( + f"core.{record_type.__name__}", "flatbuffer", bfbs + ) + raw = writer.register_channel( + f"{collection}/{suffix}", "flatbuffer", schema_id + ) + tracked = writer.register_channel( + f"{collection}/{suffix}_tracked", "flatbuffer", schema_id + ) + for payload, timestamp in zip(payloads, timestamps, strict=True): + log_time = timestamp.available_time_local_common_clock + # Replay must select tracked records, not these different raw samples. + writer.add_message( + raw, log_time, record_type(None, timestamp).to_bytes(), log_time + ) + writer.add_message( + tracked, + log_time, + record_type(payload, timestamp).to_bytes(), + log_time, + ) + writer.finish() + return path + + +@pytest.fixture +def soma_mcap(tmp_path): + from isaaccapture import schema + + def write( + representation, payloads, timestamps, *, collection="soma_body_demo", path=None + ): + suffix = "soma_body_" + representation.replace("-", "_") + record_type = ( + schema.SomaBodyJointRotationsRecord + if representation == "joint-rotations" + else schema.SomaBodyJointPosesRecord + ) + path = path or tmp_path / f"{suffix}.mcap" + return write_streams( + path, [(suffix, collection, record_type, payloads, timestamps)] + ) + + return write + + +@pytest.fixture +def soma_hand_mcap(tmp_path): + from isaaccapture import schema + from isaaccapture_examples.mcap_record_replay.soma_hand_common import collections + + def write(representation, payloads, timestamps, *, collection_ids=None, path=None): + collection_ids = collection_ids or collections( + "soma_hand_left_demo", "soma_hand_right_demo" + ) + suffix = "soma_hand_" + representation.replace("-", "_") + record_type = ( + schema.SomaHandJointRotationsRecord + if representation == "joint-rotations" + else schema.SomaHandJointPosesRecord + ) + return write_streams( + path or tmp_path / f"{suffix}.mcap", + [ + (suffix, collection_ids[key], record_type, values, timestamps[key]) + for key, values in payloads.items() + ], + ) + + return write + + +@pytest.fixture +def legacy_mcap(tmp_path): + """Native OpenXR channels do not use SchemaTracker's raw/tracked pair.""" + import numpy as np + from mcap.writer import CompressionType, Writer + from isaaccapture import schema + + def write(kind): + body = kind == "body" + joints = schema.BodyJoints() if body else schema.HandJoints() + count = 24 if body else 26 + joints.positions[:] = np.arange(count * 3, dtype=np.float32).reshape(count, 3) + joints.orientations[:, 3] = 1 + joints.is_valid[:] = 1 + data = schema.FullBodyPose(joints) if body else schema.HandPose(joints) + record_type = schema.FullBodyPoseRecord if body else schema.HandPoseRecord + suffix = "full_body" if body else "hand" + topics = ( + ["full_body/full_body"] if body else ["hands/left_hand", "hands/right_hand"] + ) + path = tmp_path / f"{suffix}.mcap" + with path.open("wb") as stream: + writer = Writer(stream, compression=CompressionType.NONE) + writer.start() + schema_id = writer.register_schema( + f"core.{record_type.__name__}", + "flatbuffer", + ( + repo_root() / "src/core/schema/golden" / f"{suffix}.bfbs" + ).read_bytes(), + ) + for topic in topics: + channel = writer.register_channel(topic, "flatbuffer", schema_id) + for i, payload in enumerate([data, None, data]): + stamp = schema.DeviceDataTimestamp( + 1_000_000_000 + i * 100_000_000, 0, i + ) + writer.add_message( + channel, + stamp.available_time_local_common_clock, + record_type(payload, stamp).to_bytes(), + stamp.available_time_local_common_clock, + ) + writer.finish() + return path + + return write diff --git a/tests/python/examples/mcap_record_replay/pyproject.toml b/tests/python/examples/mcap_record_replay/pyproject.toml new file mode 100644 index 0000000000..e5b3828125 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/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 = "mcap-record-replay-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = ["pytest", "numpy", "mcap>=1.2.0", "viser>=0.2.0", "websockets>=14.0", "cryptography", "PyYAML"] + +[tool.pytest.ini_options] +pythonpath = ["."] diff --git a/tests/python/examples/mcap_record_replay/test_soma_hand.py b/tests/python/examples/mcap_record_replay/test_soma_hand.py new file mode 100644 index 0000000000..6fde2a704f --- /dev/null +++ b/tests/python/examples/mcap_record_replay/test_soma_hand.py @@ -0,0 +1,508 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import argparse +from types import SimpleNamespace +from unittest.mock import MagicMock + +import numpy as np +import pytest +from mcap.reader import make_reader + +from isaaccapture import schema +from isaaccapture.deviceio import McapReplayConfig, ReplaySession +from isaaccapture.teleop_session_manager import ( + SessionMode, + TeleopSession, + TeleopSessionConfig, +) +from isaaccapture.deviceio_trackers import ( + SomaHandJointPosesTracker, + SomaHandJointRotationsTracker, +) +from isaaccapture.retargeting_engine.deviceio_source_nodes import soma_hand_source +from isaaccapture.retargeting_engine.interface.tensor_group import OptionalTensorGroup +from isaaccapture.retargeting_engine.interface.tensor_group_type import OptionalType +from isaaccapture.retargeting_engine.tensor_types import ( + SomaHandInput, + SomaHandInputIndex, +) +from isaaccapture_examples.mcap_record_replay import ( + live_hand, + record_hand, + replay_hand, + common, + soma_hand_common, +) + + +def payload(representation, hand_side, indices=None, sign=1.0): + indices = range(25) if indices is None else indices + q = schema.Quaternion(0.0, sign * 0.6, 0.0, sign * 0.8) + if representation == "joint-rotations": + return schema.SomaHandJointRotations( + [schema.SomaHandJointRotation(schema.SomaHandJoint(i), q) for i in indices], + schema.Point(1.0, 2.0, 3.0), + True, + hand_side, + ) + return schema.SomaHandJointPoses( + [ + schema.SomaHandJointPose( + schema.SomaHandJoint(i), + schema.Pose(schema.Point(float(i), 2.0, 3.0), q), + ) + for i in indices + ], + hand_side, + ) + + +def timestamps(count, start=1_000_000_000): + return [ + schema.DeviceDataTimestamp(start + i * 100_000_000, 80 + i, 10 + i) + for i in range(count) + ] + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +def test_repeated_hand_roundtrip_preserves_sides_and_independent_ticks( + soma_hand_mcap, tmp_path, representation +): + sides = soma_hand_common.HAND_SIDES + collection_ids = soma_hand_common.collections("vendor.left", "vendor.right") + payloads = { + "hand_left": [ + payload(representation, sides["hand_left"]), + None, + payload(representation, sides["hand_left"], [0, 1, 24], -1), + ], + "hand_right": [payload(representation, sides["hand_right"], [0, 5, 24], -1)], + } + times = { + key: timestamps( + len(values), start=1_000_000_000 if key == "hand_left" else 9_000_000_000 + ) + for key, values in payloads.items() + } + expected = { + key: [None if data is None else data.to_bytes() for data in values] + for key, values in payloads.items() + } + record_type = ( + schema.SomaHandJointRotationsRecord + if representation == "joint-rotations" + else schema.SomaHandJointPosesRecord + ) + for generation in range(3): + path = soma_hand_mcap( + representation, + payloads, + times, + collection_ids=collection_ids, + path=tmp_path / f"hands_{generation}.mcap", + ) + for key, values in payloads.items(): + topic = f"{collection_ids[key]}/soma_hand_{representation.replace('-', '_')}_tracked" + with path.open("rb") as stream: + records = list(make_reader(stream).iter_messages(topics=[topic])) + for (_, _, message), data, stamp in zip( + records, values, times[key], strict=True + ): + assert message.data == record_type(data, stamp).to_bytes() + assert ( + message.log_time + == message.publish_time + == stamp.available_time_local_common_clock + ) + trackers = soma_hand_common.create_trackers(collection_ids, representation) + replayed = {key: [] for key in trackers} + config = McapReplayConfig( + str(path), + [(tracker, collection_ids[key]) for key, tracker in trackers.items()], + ) + with ReplaySession.run(config) as session: + for frame in range(3): + session.update() + for key, tracker in trackers.items(): + data = tracker.get_data(session) + encoded = ( + expected[key][frame] if frame < len(expected[key]) else None + ) + assert (None if data is None else data.to_bytes()) == encoded + if data is not None: + assert data.hand_side == sides[key] + if frame < len(expected[key]): + replayed[key].append(data) + session.update() + assert all( + tracker.get_data(session) is None for tracker in trackers.values() + ) + payloads = replayed + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +@pytest.mark.parametrize("right_present", [True, False]) +def test_hand_pipeline_repeats_with_missing_or_exhausted_hand( + soma_hand_mcap, monkeypatch, representation, right_present +): + collection_ids = soma_hand_common.collections( + "soma_hand_left_demo", "soma_hand_right_demo" + ) + values = { + "hand_left": [ + payload(representation, schema.SomaHandSide.LEFT), + None, + payload(representation, schema.SomaHandSide.LEFT, [0, 1, 24]), + ] + } + if right_present: + values["hand_right"] = [payload(representation, schema.SomaHandSide.RIGHT)] + times = {key: timestamps(len(data)) for key, data in values.items()} + path = soma_hand_mcap(representation, values, times) + evaluator = MagicMock() + evaluator.evaluate.return_value = ( + np.ones((25, 3), dtype=np.float32), + np.tile(np.array([0, 0, 0, 1], dtype=np.float32), (25, 1)), + np.ones(25, dtype=np.uint8), + ) + monkeypatch.setattr(soma_hand_source, "_SomaHandEvaluator", lambda *args: evaluator) + monkeypatch.setattr(soma_hand_common, "create_layer", lambda _: object()) + pipeline = soma_hand_common.build_pipeline(collection_ids, representation) + counts, rate = soma_hand_common.replay_info(path, collection_ids, representation) + assert counts == {"hand_left": 3, "hand_right": int(right_present)} + assert rate == pytest.approx(10) + frames = {key: [] for key in collection_ids} + + def capture(key, group): + frames[key].append( + None + if group.is_none + else np.asarray(group[SomaHandInputIndex.JOINT_VALID]).copy() + ) + + visualizers = { + key: MagicMock(update=lambda group, key=key: capture(key, group)) + for key in collection_ids + } + clock = [0.0] + monkeypatch.setattr( + common, + "time", + SimpleNamespace( + monotonic=lambda: clock[0], + sleep=lambda delay: clock.__setitem__(0, clock[0] + delay), + ), + ) + for _ in range(2): + + def update(result): + for key, viz in visualizers.items(): + viz.update(result[key]) + + assert ( + common.replay_once(path, pipeline, max(counts.values()), update, rate) == 3 + ) + assert clock[0] == pytest.approx(0.4) + assert frames["hand_left"][1] is None and frames["hand_left"][4] is None + assert np.count_nonzero(frames["hand_left"][0]) == 25 + if representation == "joint-poses": + assert np.flatnonzero(frames["hand_left"][2]).tolist() == [0, 1, 24] + for frame in (1, 2, 4, 5): + assert frames["hand_right"][frame] is None + assert (frames["hand_right"][0] is not None) == right_present + assert evaluator.evaluate.call_count == ( + 2 * (2 + right_present) if representation == "joint-rotations" else 0 + ) + + +def test_hand_pose_pipeline_and_native_25_joint_renderer_need_no_model(monkeypatch): + layer = MagicMock(side_effect=AssertionError("joint poses must not load models")) + monkeypatch.setattr(soma_hand_common, "create_layer", layer) + ids = soma_hand_common.collections("vendor.left", "vendor.right") + sources = { + source.name: source + for source in soma_hand_common.build_pipeline( + ids, "joint-poses" + ).get_leaf_nodes() + } + assert set(sources) == set(ids.values()) + assert all( + isinstance(source.get_tracker(), SomaHandJointPosesTracker) + for source in sources.values() + ) + assert sources["vendor.left"].hand_side == schema.SomaHandSide.LEFT + assert sources["vendor.right"].hand_side == schema.SomaHandSide.RIGHT + layer.assert_not_called() + server = MagicMock() + server.scene.add_point_cloud.side_effect = lambda **kwargs: MagicMock(**kwargs) + server.scene.add_line_segments.side_effect = lambda **kwargs: MagicMock(**kwargs) + visualizers = soma_hand_common.create_visualizers(server) + group = OptionalTensorGroup(OptionalType(SomaHandInput())) + group[SomaHandInputIndex.JOINT_POSITIONS] = np.arange(75, dtype=np.float32).reshape( + 25, 3 + ) + group[SomaHandInputIndex.JOINT_VALID] = np.ones(25, dtype=np.uint8) + assert len(soma_hand_common.JOINT_NAMES) == 25 + for viz in visualizers.values(): + assert viz.update(group) == 25 + assert viz.points.points.shape == (25, 3) + assert viz.bones.points.shape == (24, 2, 3) + group[SomaHandInputIndex.JOINT_VALID] = np.array( + [1, 1, *([0] * 23)], dtype=np.uint8 + ) + assert visualizers["hand_left"].update(group) == 2 + assert visualizers["hand_left"].bones.points.shape == (1, 2, 3) + group.set_none() + assert visualizers["hand_left"].update(group) == 0 + assert visualizers["hand_left"].points.points.shape == (0, 3) + + +@pytest.mark.parametrize("module", [live_hand, record_hand, replay_hand]) +def test_hand_cli_help_does_not_load_models(module, monkeypatch, capsys): + monkeypatch.setattr( + soma_hand_common, + "create_layer", + MagicMock(side_effect=AssertionError("model loaded")), + ) + with pytest.raises(SystemExit) as caught: + module.main(["hands", "--help"]) + assert caught.value.code == 0 + help_text = capsys.readouterr().out + assert "--hand-schema" in help_text + assert "--soma-hand-representation" in help_text + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +def test_hand_recorder_registers_two_trackers_without_fk( + representation, monkeypatch, tmp_path +): + monkeypatch.setattr( + soma_hand_common, + "create_layer", + MagicMock(side_effect=AssertionError("recording must not run FK")), + ) + monkeypatch.setattr(common.CloudXRLauncher, "launch_context", MagicMock()) + monkeypatch.setattr(common, "OpenXRSession", MagicMock()) + deviceio = MagicMock() + deviceio.run.return_value.__enter__.return_value.update.side_effect = ( + KeyboardInterrupt + ) + monkeypatch.setattr(common, "DeviceIOSession", deviceio) + path = tmp_path / "hands.mcap" + assert ( + record_hand.main( + [ + "record", + "5", + str(path), + "--hand-schema", + "soma", + "--soma-hand-representation", + representation, + ] + ) + == 0 + ) + trackers, _, config = deviceio.run.call_args.args + assert len(trackers) == 2 + expected_type = ( + SomaHandJointRotationsTracker + if representation == "joint-rotations" + else SomaHandJointPosesTracker + ) + assert all(isinstance(tracker, expected_type) for tracker in trackers) + assert config.get_tracker_names() == list( + zip(trackers, ["soma_hand_left_demo", "soma_hand_right_demo"], strict=True) + ) + assert config.filename == str(path) + deviceio.run.return_value.__exit__.assert_called_once() + + +def test_live_hand_preview_has_no_mcap_config(monkeypatch): + monkeypatch.setattr(live_hand.viser, "ViserServer", MagicMock()) + monkeypatch.setattr(live_hand.CloudXRLauncher, "launch_context", MagicMock()) + session = MagicMock() + session.__enter__.return_value.step.side_effect = KeyboardInterrupt + factory = MagicMock(return_value=session) + monkeypatch.setattr(live_hand, "TeleopSession", factory) + assert ( + live_hand.main( + [ + "live", + "--hand-schema", + "soma", + "--soma-hand-representation", + "joint-poses", + ] + ) + == 0 + ) + assert factory.call_args.args[0].mcap_config is None + + +def test_hand_replay_rejects_wrong_collections_or_profile(soma_hand_mcap): + path = soma_hand_mcap( + "joint-poses", + {"hand_left": [payload("joint-poses", schema.SomaHandSide.LEFT)]}, + {"hand_left": timestamps(1)}, + ) + for ids, profile in [ + (soma_hand_common.collections("wrong.left", "wrong.right"), "joint-poses"), + ( + soma_hand_common.collections("soma_hand_left_demo", "soma_hand_right_demo"), + "joint-rotations", + ), + ]: + with pytest.raises(ValueError, match="no SOMA hand samples"): + soma_hand_common.replay_info(path, ids, profile) + + +def test_hand_replay_rejects_wrong_anatomical_side(soma_hand_mcap): + path = soma_hand_mcap( + "joint-poses", + {"hand_left": [payload("joint-poses", schema.SomaHandSide.RIGHT)]}, + {"hand_left": timestamps(1)}, + ) + ids = soma_hand_common.collections("soma_hand_left_demo", "soma_hand_right_demo") + pipeline = soma_hand_common.build_pipeline(ids, "joint-poses") + with pytest.raises(ValueError, match="hand side"): + common.replay_once( + path, + pipeline, + 1, + MagicMock(), + 0, + ) + + +def test_hand_recording_resolution_does_not_select_body_files(tmp_path, monkeypatch): + monkeypatch.chdir(tmp_path) + recordings = tmp_path / "recordings" + recordings.mkdir() + (recordings / "soma_body_joint-poses_demo.mcap").touch() + with pytest.raises(FileNotFoundError, match="No soma_hand_joint-poses"): + common.resolve_recording(None, "soma_hand_joint-poses") + expected = recordings / "soma_hand_joint-poses_demo.mcap" + expected.touch() + assert common.resolve_recording(None, "soma_hand_joint-poses") == expected + + +@pytest.mark.parametrize("module", [live_hand, record_hand, replay_hand]) +def test_hand_commands_reject_duplicate_collections(module): + with pytest.raises(SystemExit) as caught: + module.main( + [ + "hands", + "--hand-schema", + "soma", + "--soma-left-hand-collection-id", + "same", + "--soma-right-hand-collection-id", + "same", + ] + ) + assert caught.value.code == 2 + + +def test_default_hand_native_replay_keeps_openxr_palm_and_26_joints(legacy_mcap): + path = legacy_mcap("hand") + config = TeleopSessionConfig( + app_name="OpenXRHandMcapRegression", + pipeline=common.build_hand_pipeline(), + mode=SessionMode.REPLAY, + mcap_config=McapReplayConfig(str(path)), + ) + parser = argparse.ArgumentParser() + common.add_hand_arguments(parser) + args = parser.parse_args([]) + assert args.hand_schema == "openxr-hand-pose" + visualizers = common.create_hand_visualizers(args, MagicMock()) + for _ in range(2): + with TeleopSession(config) as session: + for present in [True, False, True, False]: + assert common.update_hand_visualizers( + args, session.step(), visualizers + ) == [present, present] + for viz in visualizers.values(): + assert viz.points.points.shape == (26, 3) + if present: + np.testing.assert_array_equal( + viz.points.points[[0, 25]], [[0, 1, 2], [75, 76, 77]] + ) + + +def test_default_hand_recording_keeps_existing_source_name( + monkeypatch, tmp_path, command_clock +): + monkeypatch.setattr(record_hand.CloudXRLauncher, "launch_context", MagicMock()) + session = MagicMock() + session.__enter__.return_value.frame_count = 1 + factory = MagicMock(return_value=session) + monkeypatch.setattr(record_hand, "TeleopSession", factory) + command_clock(record_hand, [0, 0, 6]) + assert record_hand.main(["record", "5", str(tmp_path / "hand.mcap")]) == 0 + config = factory.call_args.args[0] + assert {source.name for source in config.pipeline.get_leaf_nodes()} == {"hands"} + assert config.mcap_config.filename == str(tmp_path / "hand.mcap") + + +def test_default_hand_replay_keeps_duration_and_60_hz( + monkeypatch, tmp_path, command_clock +): + session = MagicMock() + session.__enter__.return_value.frame_count = 1 + session.__enter__.return_value.step.return_value = { + "left_positions": [np.zeros((26, 3))], + "right_positions": [np.zeros((26, 3))], + "left_valid": [False], + "right_valid": [False], + } + factory = MagicMock(return_value=session) + monkeypatch.setattr(replay_hand, "TeleopSession", factory) + clock = command_clock(replay_hand, [0, 0, 1]) + assert ( + replay_hand.run_once(tmp_path / "hand.mcap", 0.5, MagicMock(), MagicMock()) == 1 + ) + clock.sleep.assert_called_once_with(1 / 60) + config = factory.call_args.args[0] + assert {source.name for source in config.pipeline.get_leaf_nodes()} == {"hands"} + + +def test_hand_replay_cli_uses_native_soma_recording(soma_hand_mcap, monkeypatch): + path = soma_hand_mcap( + "joint-poses", + {"hand_left": [payload("joint-poses", schema.SomaHandSide.LEFT), None]}, + {"hand_left": timestamps(2)}, + ) + server = MagicMock() + monkeypatch.setattr(replay_hand.viser, "ViserServer", lambda **kwargs: server) + frames = [] + real_replay = common.replay_once + + def once(path, pipeline, count, update, rate): + def capture(result): + frames.append([result[key].is_none for key in ("hand_left", "hand_right")]) + update(result) + + assert real_replay(path, pipeline, count, capture, rate) == 2 + raise KeyboardInterrupt + + monkeypatch.setattr(replay_hand, "replay_once", once) + assert ( + replay_hand.main( + [ + "replay", + str(path), + "--hand-schema", + "soma", + "--soma-hand-representation", + "joint-poses", + "--rate", + "0", + ] + ) + == 0 + ) + assert frames == [[False, True], [True, True]] + server.stop.assert_called_once() diff --git a/tests/python/examples/mcap_record_replay/test_soma_mcap.py b/tests/python/examples/mcap_record_replay/test_soma_mcap.py new file mode 100644 index 0000000000..f0089b7932 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/test_soma_mcap.py @@ -0,0 +1,208 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from types import SimpleNamespace +from unittest.mock import MagicMock + +import numpy as np +import pytest +from mcap.reader import make_reader + +from isaaccapture import schema +from isaaccapture.deviceio import McapReplayConfig, ReplaySession +from isaaccapture.retargeting_engine.deviceio_source_nodes import soma_body_source +from isaaccapture.retargeting_engine.tensor_types import SomaBodyInputIndex +from isaaccapture.teleop_session_manager import ( + SessionMode, + TeleopSession, + TeleopSessionConfig, +) +from isaaccapture_examples.mcap_record_replay import ( + common, + soma_body_common, +) + + +def payload(representation, indices, sign=1.0, translation_valid=True): + quaternion = schema.Quaternion(0.0, 0.0, sign * 0.6, sign * 0.8) + if representation == "joint-rotations": + return schema.SomaBodyJointRotations( + [ + schema.SomaBodyJointRotation(schema.SomaBodyJoint(i), quaternion) + for i in indices + ], + schema.Point(1.0, 2.0, 3.0), + translation_valid, + ) + return schema.SomaBodyJointPoses( + [ + schema.SomaBodyJointPose( + schema.SomaBodyJoint(i), + schema.Pose(schema.Point(float(i), 2.0, 3.0), quaternion), + ) + for i in indices + ] + ) + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +def test_repeated_native_roundtrip(soma_mcap, tmp_path, representation): + payloads = [ + payload(representation, range(77)), + None, + payload(representation, [0, 1, 76], -1), + payload(representation, [], translation_valid=False), + ] + timestamps = [ + schema.DeviceDataTimestamp(100 + i * 100_000_000, 80 + i, 10 + i) + for i in range(len(payloads)) + ] + expected = [None if data is None else data.to_bytes() for data in payloads] + record_type = ( + schema.SomaBodyJointRotationsRecord + if representation == "joint-rotations" + else schema.SomaBodyJointPosesRecord + ) + topic = f"vendor.body/soma_body_{representation.replace('-', '_')}_tracked" + for generation in range(3): + path = soma_mcap( + representation, + payloads, + timestamps, + collection="vendor.body", + path=tmp_path / f"generation_{generation}.mcap", + ) + with path.open("rb") as stream: + records = list(make_reader(stream).iter_messages(topics=[topic])) + assert len(records) == len(payloads) + for (_, _, message), data, timestamp in zip( + records, payloads, timestamps, strict=True + ): + assert message.data == record_type(data, timestamp).to_bytes() + assert ( + message.log_time + == message.publish_time + == timestamp.available_time_local_common_clock + ) + tracker = soma_body_common.create_tracker("vendor.body", representation) + replayed = [] + with ReplaySession.run( + McapReplayConfig(str(path), [(tracker, "vendor.body")]) + ) as session: + for encoded in expected: + session.update() + data = tracker.get_data(session) + assert (None if data is None else data.to_bytes()) == encoded + replayed.append(data) + for _ in range(2): + session.update() + assert tracker.get_data(session) is None + # Re-encode the actual received payloads, without changing their timestamps. + payloads = replayed + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +def test_native_replay_pipeline_and_bounded_loop( + soma_mcap, monkeypatch, representation +): + entries = [ + payload(representation, range(77)), + None, + payload(representation, [0, 1, 76]), + ] + timestamps = [ + schema.DeviceDataTimestamp(1_000_000_000 + i * 100_000_000, 0, i) + for i in range(3) + ] + path = soma_mcap(representation, entries, timestamps) + evaluator = MagicMock() + evaluator.evaluate.return_value = ( + np.ones((77, 3), dtype=np.float32), + np.tile(np.array([0, 0, 0, 1], dtype=np.float32), (77, 1)), + np.ones(77, dtype=np.uint8), + ) + monkeypatch.setattr(soma_body_common, "create_layer", lambda: object()) + monkeypatch.setattr(soma_body_source, "_SomaBodyEvaluator", lambda _: evaluator) + pipeline = soma_body_common.build_pipeline("soma_body_demo", representation) + config = TeleopSessionConfig( + app_name="SomaMcapRegression", + pipeline=pipeline, + mode=SessionMode.REPLAY, + mcap_config=McapReplayConfig(str(path)), + ) + for _ in range(2): + with TeleopSession(config) as session: + body = session.step()["soma_body"] + assert not body.is_none + assert np.count_nonzero(body[SomaBodyInputIndex.JOINT_VALID]) == 77 + assert session.step()["soma_body"].is_none + body = session.step()["soma_body"] + assert not body.is_none + if representation == "joint-poses": + valid = body[SomaBodyInputIndex.JOINT_VALID] + assert np.flatnonzero(valid).tolist() == [0, 1, 76] + assert body[SomaBodyInputIndex.JOINT_POSITIONS][76, 0] == 76.0 + assert session.step()["soma_body"].is_none + evaluator.reset_mock() + count, rate = soma_body_common.replay_info(path, "soma_body_demo", representation) + assert count == 3 + assert rate == pytest.approx(10.0) + viz = MagicMock() + clock = [0.0] + monkeypatch.setattr( + common, + "time", + SimpleNamespace( + monotonic=lambda: clock[0], + sleep=lambda delay: clock.__setitem__(0, clock[0] + delay), + ), + ) + for _ in range(2): + assert ( + common.replay_once( + path, + pipeline, + count, + lambda result: viz.update(result["soma_body"]), + rate, + ) + == 3 + ) + assert viz.update.call_count == 6 + assert clock[0] == pytest.approx(0.4) + assert evaluator.evaluate.call_count == ( + 4 if representation == "joint-rotations" else 0 + ) + if representation == "joint-rotations": + assert evaluator.evaluate.call_args.args[0].to_bytes() == entries[-1].to_bytes() + + +def test_replay_rejects_missing_collection_or_representation(soma_mcap): + path = soma_mcap( + "joint-poses", + [payload("joint-poses", [0])], + [schema.DeviceDataTimestamp(1, 2, 3)], + ) + for collection, representation in [ + ("wrong", "joint-poses"), + ("soma_body_demo", "joint-rotations"), + ]: + with pytest.raises(ValueError, match="no samples"): + soma_body_common.replay_info(path, collection, representation) + count, rate = soma_body_common.replay_info(path, "soma_body_demo", "joint-poses") + assert count == 1 + assert rate == 60.0 + + +def test_resolve_only_matching_recordings(tmp_path, monkeypatch): + monkeypatch.chdir(tmp_path) + recordings = tmp_path / "recordings" + recordings.mkdir() + (recordings / "full_body_20261009.mcap").touch() + (recordings / "soma_body_joint-rotations_20261009.mcap").touch() + with pytest.raises(FileNotFoundError, match="No soma_body_joint-poses"): + common.resolve_recording(None, "soma_body_joint-poses") + expected = recordings / "soma_body_joint-poses_20261009.mcap" + expected.touch() + assert common.resolve_recording(None, "soma_body_joint-poses") == expected + assert common.resolve_recording(str(expected), "soma_body_joint-poses") == expected diff --git a/tests/python/examples/mcap_record_replay/test_soma_viewer.py b/tests/python/examples/mcap_record_replay/test_soma_viewer.py new file mode 100644 index 0000000000..815d921c60 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/test_soma_viewer.py @@ -0,0 +1,345 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import argparse +from unittest.mock import MagicMock + +import numpy as np +import pytest + +from isaaccapture.deviceio_trackers import ( + SomaBodyJointPosesTracker, + SomaBodyJointRotationsTracker, +) +from isaaccapture.retargeting_engine.interface.tensor_group import OptionalTensorGroup +from isaaccapture.retargeting_engine.interface.tensor_group_type import OptionalType +from isaaccapture.retargeting_engine.tensor_types import ( + SomaBodyInput, + SomaBodyInputIndex, +) +from isaaccapture.deviceio import McapReplayConfig, ControllerTracker +from isaaccapture.teleop_session_manager import ( + SessionMode, + TeleopSession, + TeleopSessionConfig, +) +from isaaccapture_examples.mcap_record_replay import ( + common, + live_full_body, + record_full_body, + replay_full_body, + soma_body_common, +) +from isaaccapture_examples.mcap_record_replay.common import ( + BODY_BONES, + FullBodyViz, + build_full_body_pipeline, +) + + +def body_group(valid): + # OptionalType describes the shape; OptionalTensorGroup supplies absence state. + group = OptionalTensorGroup(OptionalType(SomaBodyInput())) + group[SomaBodyInputIndex.JOINT_POSITIONS] = np.arange( + 77 * 3, dtype=np.float32 + ).reshape(77, 3) + group[SomaBodyInputIndex.JOINT_VALID] = np.asarray(valid, dtype=np.uint8) + return group + + +def test_soma_renderer_keeps_native_topology_and_hides_unavailable_joints(): + server = MagicMock() + server.scene.add_point_cloud.side_effect = lambda **kwargs: MagicMock(**kwargs) + server.scene.add_line_segments.side_effect = lambda **kwargs: MagicMock(**kwargs) + ground = MagicMock() + viz = soma_body_common.SomaBodyViz(server, ground) + assert len(soma_body_common.JOINT_NAMES) == 77 + assert len(soma_body_common.BONES) == 76 + group = body_group(np.ones(77)) + assert viz.update(group) == 77 + assert viz.points.points.shape == (77, 3) + assert viz.bones.points.shape == (76, 2, 3) + np.testing.assert_array_equal( + viz.bones.points[-1], group[SomaBodyInputIndex.JOINT_POSITIONS][[75, 76]] + ) + valid = np.zeros(77) + valid[[0, 1, 76]] = 1 + assert viz.update(body_group(valid)) == 3 + assert viz.points.points.shape == (3, 3) + assert viz.bones.points.shape == (1, 2, 3) + group.set_none() + assert viz.update(group) == 0 + assert viz.points.points.shape == (0, 3) + assert viz.bones.points.shape == (0, 2, 3) + + +def test_joint_pose_pipeline_needs_no_model(monkeypatch): + layer = MagicMock( + side_effect=AssertionError("evaluated poses must not load a model") + ) + monkeypatch.setattr(soma_body_common, "create_layer", layer) + pipeline = soma_body_common.build_pipeline("vendor.body", "joint-poses") + sources = list(pipeline.get_leaf_nodes()) + assert len(sources) == 1 + assert sources[0].name == "vendor.body" + assert isinstance(sources[0].get_tracker(), SomaBodyJointPosesTracker) + assert isinstance( + soma_body_common.create_tracker("vendor.body", "joint-rotations"), + SomaBodyJointRotationsTracker, + ) + layer.assert_not_called() + + +def test_existing_full_body_pipeline_and_renderer_stay_24_joint(): + sources = {source.name for source in build_full_body_pipeline().get_leaf_nodes()} + assert sources == {"full_body", "controllers"} + server = MagicMock() + viz = FullBodyViz(server) + positions = np.zeros((24, 3), dtype=np.float32) + viz.update(positions, np.ones(24, dtype=np.uint8)) + assert viz.points.points.shape == (24, 3) + assert viz.bones.points.shape == (len(BODY_BONES), 2, 3) + + +@pytest.mark.parametrize("module", [live_full_body, record_full_body, replay_full_body]) +def test_cli_help_does_not_load_models_or_start_sessions(module, monkeypatch, capsys): + monkeypatch.setattr( + soma_body_common, + "create_layer", + MagicMock(side_effect=AssertionError("model loaded")), + ) + with pytest.raises(SystemExit) as caught: + module.main(["example", "--help"]) + assert caught.value.code == 0 + help_text = capsys.readouterr().out + assert "--body-schema" in help_text + assert "--soma-body-representation" in help_text + assert "--soma-body-collection-id" in help_text + + +def test_live_preview_has_no_mcap_config(monkeypatch): + monkeypatch.setattr(live_full_body.viser, "ViserServer", MagicMock()) + monkeypatch.setattr(live_full_body.CloudXRLauncher, "launch_context", MagicMock()) + session = MagicMock() + session.__enter__.return_value.step.side_effect = KeyboardInterrupt + session_factory = MagicMock(return_value=session) + monkeypatch.setattr(live_full_body, "TeleopSession", session_factory) + assert ( + live_full_body.main( + [ + "live", + "--body-schema", + "soma", + "--soma-body-representation", + "joint-poses", + ] + ) + == 0 + ) + assert session_factory.call_args.args[0].mcap_config is None + + +@pytest.mark.parametrize("representation", ["joint-rotations", "joint-poses"]) +def test_recorder_registers_only_raw_tracker_and_closes_on_interrupt( + representation, monkeypatch, tmp_path +): + monkeypatch.setattr( + soma_body_common, + "create_layer", + MagicMock(side_effect=AssertionError("recording must not run FK")), + ) + launcher = MagicMock() + monkeypatch.setattr(common.CloudXRLauncher, "launch_context", launcher) + oxr_factory = MagicMock() + monkeypatch.setattr(common, "OpenXRSession", oxr_factory) + deviceio = MagicMock() + deviceio.run.return_value.__enter__.return_value.update.side_effect = ( + KeyboardInterrupt + ) + monkeypatch.setattr(common, "DeviceIOSession", deviceio) + path = tmp_path / "body.mcap" + assert ( + record_full_body.main( + [ + "record", + "5", + str(path), + "--body-schema", + "soma", + "--soma-body-collection-id", + "vendor.body", + "--soma-body-representation", + representation, + ] + ) + == 0 + ) + trackers, handles, config = deviceio.run.call_args.args + assert len(trackers) == 1 + tracker_type = ( + SomaBodyJointRotationsTracker + if representation == "joint-rotations" + else SomaBodyJointPosesTracker + ) + assert isinstance(trackers[0], tracker_type) + assert config.filename == str(path) + assert config.get_tracker_names() == [(trackers[0], "vendor.body")] + assert ( + handles + == oxr_factory.return_value.__enter__.return_value.get_handles.return_value + ) + deviceio.run.return_value.__exit__.assert_called_once() + oxr_factory.return_value.__exit__.assert_called_once() + launcher.return_value.__exit__.assert_called_once() + + +@pytest.mark.parametrize("duration", ["0", "-1", "nan", "inf"]) +def test_recorder_rejects_invalid_duration(duration): + with pytest.raises(SystemExit) as caught: + record_full_body.main(["record", duration]) + assert caught.value.code == 2 + + +@pytest.mark.parametrize( + "flags,names", + [ + ([], {"full_body", "controllers"}), + (["--no-controllers"], {"full_body"}), + (["--body-schema", "soma"], {"soma_body_demo"}), + (["--body-schema", "soma", "--controllers"], {"soma_body_demo", "controllers"}), + ], +) +def test_body_schema_selection_preserves_controller_defaults(flags, names): + parser = argparse.ArgumentParser() + common.add_body_arguments(parser) + args = parser.parse_args([*flags, "--soma-body-representation", "joint-poses"]) + common.validate_body_arguments(parser, args) + pipeline = common.build_body_pipeline(args) + assert {source.name for source in pipeline.get_leaf_nodes()} == names + + +def test_soma_recorder_can_opt_in_to_controllers(monkeypatch, tmp_path): + record = MagicMock() + monkeypatch.setattr(record_full_body, "record_trackers", record) + assert ( + record_full_body.main( + [ + "record", + "5", + str(tmp_path / "body.mcap"), + "--body-schema", + "soma", + "--controllers", + ] + ) + == 0 + ) + trackers = record.call_args.args[2] + assert len(trackers) == 2 + assert isinstance(trackers[0][0], SomaBodyJointRotationsTracker) + assert isinstance(trackers[1][0], ControllerTracker) + assert trackers[1][1] == "controllers" + + +def test_default_body_recording_keeps_existing_source_names( + monkeypatch, tmp_path, command_clock +): + monkeypatch.setattr(record_full_body.CloudXRLauncher, "launch_context", MagicMock()) + session = MagicMock() + session.__enter__.return_value.frame_count = 1 + factory = MagicMock(return_value=session) + monkeypatch.setattr(record_full_body, "TeleopSession", factory) + command_clock(record_full_body, [0, 0, 6]) + assert record_full_body.main(["record", "5", str(tmp_path / "body.mcap")]) == 0 + config = factory.call_args.args[0] + assert {source.name for source in config.pipeline.get_leaf_nodes()} == { + "full_body", + "controllers", + } + assert config.mcap_config.filename == str(tmp_path / "body.mcap") + + +def test_default_body_native_replay_and_renderer(legacy_mcap): + path = legacy_mcap("body") + config = TeleopSessionConfig( + app_name="FullBodyMcapRegression", + pipeline=build_full_body_pipeline(), + mode=SessionMode.REPLAY, + mcap_config=McapReplayConfig(str(path)), + ) + server = MagicMock() + viz = FullBodyViz(server) + args = argparse.Namespace(body_schema="full-body-pose") + for _ in range(2): + with TeleopSession(config) as session: + for expected in [24, 0, 24, 0]: + assert ( + common.update_body_visualizer(args, session.step(), viz) == expected + ) + assert viz.points.points.shape == (24, 3) + if expected: + np.testing.assert_array_equal(viz.points.points[23], [69, 70, 71]) + else: + assert viz.bones.points.shape == (0, 2, 3) + + +def test_default_body_replay_keeps_duration_and_60_hz( + monkeypatch, tmp_path, command_clock +): + session = MagicMock() + session.__enter__.return_value.frame_count = 1 + session.__enter__.return_value.step.return_value = { + "full_body": MagicMock(is_none=True) + } + factory = MagicMock(return_value=session) + monkeypatch.setattr(replay_full_body, "TeleopSession", factory) + clock = command_clock(replay_full_body, [0, 0, 1]) + assert replay_full_body.run_once(tmp_path / "body.mcap", 0.5, MagicMock()) == 1 + clock.sleep.assert_called_once_with(1 / 60) + config = factory.call_args.args[0] + assert {source.name for source in config.pipeline.get_leaf_nodes()} == { + "full_body", + "controllers", + } + + +def test_body_replay_cli_uses_native_soma_recording(soma_mcap, monkeypatch): + from test_soma_mcap import payload + from isaaccapture import schema + + path = soma_mcap( + "joint-poses", + [payload("joint-poses", [0, 1, 76]), None], + [schema.DeviceDataTimestamp(1, 2, 3), schema.DeviceDataTimestamp(2, 3, 4)], + ) + server = MagicMock() + monkeypatch.setattr(replay_full_body.viser, "ViserServer", lambda **kwargs: server) + frames = [] + real_replay = common.replay_once + + def once(path, pipeline, count, update, rate): + def capture(result): + frames.append(result["soma_body"].is_none) + update(result) + + assert real_replay(path, pipeline, count, capture, rate) == 2 + raise KeyboardInterrupt + + monkeypatch.setattr(replay_full_body, "replay_once", once) + assert ( + replay_full_body.main( + [ + "replay", + str(path), + "--body-schema", + "soma", + "--soma-body-representation", + "joint-poses", + "--rate", + "0", + ] + ) + == 0 + ) + assert frames == [False, True] + server.stop.assert_called_once()