diff --git a/docs/source/references/mcap_record_replay.rst b/docs/source/references/mcap_record_replay.rst index 76cd1c68d6..efa55af5b3 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..9db10322d3 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,217 @@ 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.controllers is None: + args.controllers = args.body_schema == "full-body-pose" + if args.body_schema == "soma": + if not args.soma_body_collection_id: + parser.error("soma-body-collection-id must not be empty") + # Source names also identify MCAP channel bases and must remain distinct. + if args.controllers and args.soma_body_collection_id == "controllers": + parser.error( + "soma-body-collection-id 'controllers' is reserved when --controllers is enabled" + ) + + +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): + if args.hand_schema == "soma": + from .soma_hand_common import create_visualizers + + return create_visualizers(server) + 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) + 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 + # Only active recording handles Ctrl+C as a graceful completion. + try: + 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 +419,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 +519,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 +740,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..9c814d3a66 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) 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) 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..90e27194b6 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,77 @@ 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) 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..475bc53acf --- /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): + """Render standalone hands without treating hand height as a floor reference.""" + 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, + ) + 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..e8b0f44706 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/test_soma_hand.py @@ -0,0 +1,560 @@ +# 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, replay_hand]) +def test_hand_viewers_do_not_follow_hand_height(module, monkeypatch, tmp_path): + server = MagicMock() + ground = MagicMock() + monkeypatch.setattr(module.viser, "ViserServer", lambda **kwargs: server) + monkeypatch.setattr(module, "setup_scene", MagicMock(return_value=ground)) + result = {} + for side, height in (("left", 1.0), ("right", 2.0)): + group = OptionalTensorGroup(OptionalType(SomaHandInput())) + positions = np.zeros((25, 3), dtype=np.float32) + positions[:, 1] = height + group[SomaHandInputIndex.JOINT_POSITIONS] = positions + group[SomaHandInputIndex.JOINT_VALID] = np.ones(25, dtype=np.uint8) + result[f"hand_{side}"] = group + + if module is live_hand: + monkeypatch.setattr(module.CloudXRLauncher, "launch_context", MagicMock()) + session = MagicMock() + session.__enter__.return_value.step.side_effect = [result, KeyboardInterrupt] + monkeypatch.setattr(module, "TeleopSession", MagicMock(return_value=session)) + else: + monkeypatch.setattr( + module, "resolve_recording", lambda *args: tmp_path / "hands.mcap" + ) + monkeypatch.setattr( + soma_hand_common, + "replay_info", + lambda *args: ({"hand_left": 1, "hand_right": 1}, 30.0), + ) + + def replay(path, pipeline, count, update, rate): + update(result) + raise KeyboardInterrupt + + monkeypatch.setattr(module, "replay_once", replay) + + assert ( + module.main( + [ + "example", + "--hand-schema", + "soma", + "--soma-hand-representation", + "joint-poses", + ] + ) + == 0 + ) + ground.follow.assert_not_called() + server.stop.assert_called_once() + + +@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..41762dbbc4 --- /dev/null +++ b/tests/python/examples/mcap_record_replay/test_soma_viewer.py @@ -0,0 +1,415 @@ +# 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, + record_hand, + 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 + ground.follow.assert_called_once() + 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 + + +@pytest.mark.parametrize("module", [live_full_body, record_full_body, replay_full_body]) +def test_body_cli_rejects_controller_collection_collision( + module, monkeypatch, tmp_path, capsys +): + monkeypatch.chdir(tmp_path) + monkeypatch.setattr( + module, + "build_body_pipeline", + MagicMock(side_effect=AssertionError("invalid arguments reached the pipeline")), + ) + if module is record_full_body: + monkeypatch.setattr( + module, + "record_trackers", + MagicMock( + side_effect=AssertionError("invalid arguments reached recording") + ), + ) + with pytest.raises(SystemExit) as caught: + module.main( + [ + "example", + "--body-schema", + "soma", + "--soma-body-collection-id", + "controllers", + "--controllers", + "--soma-body-representation", + "joint-poses", + ] + ) + assert caught.value.code == 2 + assert "reserved" in capsys.readouterr().err + + +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( + "module,schema_flag", + [(record_full_body, "--body-schema"), (record_hand, "--hand-schema")], +) +@pytest.mark.parametrize("stage", ["launcher", "openxr", "deviceio"]) +def test_recorder_startup_interrupt_does_not_report_success( + module, schema_flag, stage, monkeypatch, tmp_path, capsys +): + launcher = MagicMock() + oxr_factory = MagicMock() + deviceio = MagicMock() + monkeypatch.setattr(common.CloudXRLauncher, "launch_context", launcher) + monkeypatch.setattr(common, "OpenXRSession", oxr_factory) + monkeypatch.setattr(common, "DeviceIOSession", deviceio) + context = { + "launcher": launcher.return_value, + "openxr": oxr_factory.return_value, + "deviceio": deviceio.run.return_value, + }[stage] + context.__enter__.side_effect = KeyboardInterrupt + path = tmp_path / "cancelled.mcap" + + with pytest.raises(KeyboardInterrupt): + module.main(["record", "5", str(path), schema_flag, "soma"]) + + assert "[record] done" not in capsys.readouterr().out + assert not path.exists() + + +@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", "--soma-body-collection-id", "controllers"], + {"controllers"}, + ), + (["--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()