diff --git a/docs/source/device/add_device.rst b/docs/source/device/add_device.rst index e85aba119f..39a57ea660 100644 --- a/docs/source/device/add_device.rst +++ b/docs/source/device/add_device.rst @@ -122,6 +122,9 @@ seven-layer tracker stack. Instead: :code-file:`src/core/codegen/generate_trackers.py` and wires generated sources into ``deviceio_trackers``, ``live_trackers``, and ``replay_trackers``. +A device with a table that is fixed per producer, such as a sensor layout, declares it with +``metadata``; see :ref:`Trackers with metadata `. + Use ``direction = "push"`` when Teleop **pushes** a typed table to a plugin (e.g. ``HapticCommandPushTracker``). Cross-process **consumers** that bucket multiple ``HapticCommand.endpoint`` samples on one collection (e.g. ``HapticCommandReaderTracker`` for Manus diff --git a/docs/source/references/generated_trackers.rst b/docs/source/references/generated_trackers.rst index 0ccd8e0249..ba1a4e7169 100644 --- a/docs/source/references/generated_trackers.rst +++ b/docs/source/references/generated_trackers.rst @@ -73,6 +73,28 @@ Python edit at all. ``.pyi`` stubs need no work either: :code-file:`src/core/python/generate_stubs.py` derives them from the built module. +.. _generated-trackers-metadata: + +Trackers with metadata +---------------------- + +Set ``metadata = ""`` on a ``pull`` entry when the device also has a table that is fixed per +producer and too large, or too static, to repeat in every frame (a sensor layout, a calibration). +The table lives in its own ``.fbs`` with that table as ``root_type`` and no ``Record`` wrapper; +``metadata_schema`` defaults to the table name in ``snake_case``. + +- **Transport.** The producer encodes the table once and pushes it with ``SchemaMetadataPusher`` + (:code-file:`src/core/pusherio/cpp/inc/pusherio/schema_metadata_pusher.hpp`) as a loop of + ``TrackerMetadataChunk`` samples on ``/metadata``, a few per frame, for as long as + the data collection exists. :code-file:`src/core/schema/cpp/inc/schema/tracker_metadata.hpp` holds + the chunking and reassembly rules. +- **Consumer.** The facade gains ``get_metadata()``. The tracker adopts the first complete metadata + and keeps it for the session, and ``get_data()`` stays empty unless the producer's metadata is the + adopted one, so a frame that reaches a consumer can always be read with ``get_metadata()``. +- **Recording.** The metadata is written once to ``_metadata``, before any data it lets + through; data is recorded only while it would be published. Replay reads the metadata when the + session opens. + Generated today --------------- diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 9b96fea7f9..55d8a433c0 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -19,6 +19,8 @@ if(BUILD_EXAMPLES) add_subdirectory(deviceio_live_view) add_subdirectory(soma_body_publisher) add_subdirectory(soma_hand_publisher) + add_subdirectory(tactile) + add_subdirectory(tactile_visualizer) add_subdirectory(haptic_feedback) add_subdirectory(sharpa_avatar) add_subdirectory(vehicle_teleop) diff --git a/examples/tactile/CMakeLists.txt b/examples/tactile/CMakeLists.txt new file mode 100644 index 0000000000..e3caba6501 --- /dev/null +++ b/examples/tactile/CMakeLists.txt @@ -0,0 +1,7 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) +install_python_example(DESTINATION examples/tactile) diff --git a/examples/tactile/README.md b/examples/tactile/README.md new file mode 100644 index 0000000000..83d5686aaf --- /dev/null +++ b/examples/tactile/README.md @@ -0,0 +1,49 @@ + + +# Tactile + +A mock tactile device and a consumer for exercising the tactile tracker without hardware. + +- `feeder` publishes a 26-joint hand with 200 taxels by default. Each frame carries one + `TactileData` and the next chunk of the `TactileShape` metadata. It publishes through + `TensorPushTracker` on a `DeviceIOSession`, as a tactile plugin would through `SchemaPusher` + and `SchemaMetadataPusher`. +- `printer` reads one `TactileTracker` directly. It discovers the joint and taxel counts from + `TactileShape`, prints the first and last taxel forces, and records the session to MCAP. + +```text +feeder -> "tactile" (TactileData, every frame) + -> "tactile/metadata" (TactileShape chunks, in a loop) + -> TactileTracker -> printer, and tactile/* MCAP channels +``` + +Install the example: + +```bash +uv pip install -e ./examples/tactile +``` + +Start the printer first; it starts the CloudXR runtime, and no headset needs to connect: + +```bash +python -m isaaccapture_examples.tactile.printer --accept-eula --duration 0 +``` + +Then, in another terminal, start the feeder. It attaches to the same runtime: + +```bash +python -m isaaccapture_examples.tactile.feeder +``` + +Stop the printer with `Ctrl+C`; it writes `./recordings/tactile_.mcap`. Print a +recording with no runtime at all: + +```bash +python -m isaaccapture_examples.tactile.printer --replay recordings/tactile_.mcap +``` + +Use feeder options `--joints` and `--taxels` to change the shape; the printer discovers both +counts from metadata. The two processes need only agree on `--collection-id`. diff --git a/examples/tactile/pyproject.toml b/examples/tactile/pyproject.toml new file mode 100644 index 0000000000..d9cdeedcab --- /dev/null +++ b/examples/tactile/pyproject.toml @@ -0,0 +1,23 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[project] +name = "isaaccapture-examples-tactile" +version = "0.0.0" +description = "Isaac Teleop tactile feeder and printer" +requires-python = ">=3.11,<3.14" +dependencies = [ + "isaaccapture[cloudxr]", + "numpy>=1.23.0", +] + +[tool.hatch.build.targets.wheel] +only-include = ["python/isaaccapture_examples/tactile"] +sources = ["python"] + +[tool.uv.sources] +isaaccapture = { path = "../..", editable = true } diff --git a/examples/tactile/python/isaaccapture_examples/tactile/__init__.py b/examples/tactile/python/isaaccapture_examples/tactile/__init__.py new file mode 100644 index 0000000000..52a7a9daf0 --- /dev/null +++ b/examples/tactile/python/isaaccapture_examples/tactile/__init__.py @@ -0,0 +1,2 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 diff --git a/examples/tactile/python/isaaccapture_examples/tactile/common.py b/examples/tactile/python/isaaccapture_examples/tactile/common.py new file mode 100644 index 0000000000..1535dd0152 --- /dev/null +++ b/examples/tactile/python/isaaccapture_examples/tactile/common.py @@ -0,0 +1,14 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Values the feeder and the printer must agree on.""" + +DEFAULT_COLLECTION_ID = "tactile" + +MIN_NUM_JOINTS = 2 +MAX_NUM_JOINTS = 26 +DEFAULT_NUM_JOINTS = MAX_NUM_JOINTS +DEFAULT_NUM_TAXELS = 200 + +# TactileTracker's default max_flatbuffer_size: the reader rejects a larger collection. +TACTILE_SAMPLE_SIZE = 16384 diff --git a/examples/tactile/python/isaaccapture_examples/tactile/feeder.py b/examples/tactile/python/isaaccapture_examples/tactile/feeder.py new file mode 100644 index 0000000000..c170ff46b9 --- /dev/null +++ b/examples/tactile/python/isaaccapture_examples/tactile/feeder.py @@ -0,0 +1,197 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Publish mock tactile frames and their TactileShape metadata, as a tactile device would. + +Each frame pushes one TactileData sample on the data collection and the next chunk(s) of the +TactileShape on "/metadata", looping over the chunks for as long as it runs. +Pair it with ``python -m isaaccapture_examples.tactile.printer``. +""" + +import argparse +import logging +import math +import sys +import time +from contextlib import ExitStack + +import numpy as np + +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.deviceio_session import DeviceIOSession +from isaaccapture.deviceio_trackers import TensorPushTracker +from isaaccapture.oxr import OpenXRSession +from isaaccapture.schema import ( + TRACKER_METADATA_CHUNK_SIZE, + TRACKER_METADATA_TENSOR_IDENTIFIER, + TactileData, + TactileShape, + split_tracker_metadata, + tracker_metadata_collection_id, +) + +from .common import ( + DEFAULT_COLLECTION_ID, + DEFAULT_NUM_JOINTS, + DEFAULT_NUM_TAXELS, + MAX_NUM_JOINTS, + MIN_NUM_JOINTS, + TACTILE_SAMPLE_SIZE, +) + +logger = logging.getLogger("isaaccapture.examples.tactile.feeder") + +WRIST = 1 +# First and distal joint of each finger, thumb to little, as XrHandJointEXT ordinals. +FINGER_FIRST_JOINTS = (2, 6, 11, 16, 21) +FINGER_DISTAL_JOINTS = (4, 9, 14, 19, 24) + + +def mock_shape(num_joints: int, num_taxels: int) -> TactileShape: + """Build a flat hand in hand-root space with taxels on its distal segments.""" + positions = np.zeros((num_joints, 3), np.float32) + parents = np.full(num_joints, WRIST, np.uint32) + parents[WRIST] = TactileShape.ROOT_PARENT + positions[0] = (0.0, 0.05, 0.0) + + taxel_joints = [] + for finger, first in enumerate(FINGER_FIRST_JOINTS): + if first >= num_joints: + break + finger_end = ( + FINGER_FIRST_JOINTS[finger + 1] - 1 if finger < 4 else MAX_NUM_JOINTS - 1 + ) + last = min(finger_end, num_joints - 1) + for joint in range(first, last + 1): + positions[joint] = ( + -0.04 + 0.02 * finger, + 0.02 + 0.025 * (joint - first), + 0.0, + ) + if joint > first: + parents[joint] = joint - 1 + taxel_joints.append(min(FINGER_DISTAL_JOINTS[finger], last)) + + if not taxel_joints: + taxel_joints.append(WRIST) + + taxel_positions = np.zeros((num_taxels, 3), np.float32) + joint_indices = np.zeros((num_taxels, 4), np.uint32) + joint_weights = np.zeros((num_taxels, 4), np.float32) + per_segment = math.ceil(num_taxels / len(taxel_joints)) + for taxel in range(num_taxels): + segment, slot = taxel % len(taxel_joints), taxel // len(taxel_joints) + joint = taxel_joints[segment] + taxel_positions[taxel] = positions[joint] + ( + 0.0, + 0.02 * slot / per_segment, + -0.005, + ) + joint_indices[taxel] = joint + joint_weights[taxel, 0] = 1.0 + + return TactileShape( + joint_bind_positions=positions, + joint_bind_orientations=np.tile([0.0, 0.0, 0.0, 1.0], (num_joints, 1)), + joint_parents=parents, + taxel_positions=taxel_positions, + taxel_normals=np.tile([0.0, 0.0, -1.0], (num_taxels, 1)), + taxel_tangents=np.tile([1.0, 0.0, 0.0], (num_taxels, 1)), + taxel_joint_indices=joint_indices, + taxel_joint_weights=joint_weights, + ) + + +def mock_forces(num_taxels: int, t: float) -> np.ndarray: + """Generate a positive 0.5 Hz Z-force wave, phase-shifted per finger.""" + finger = np.arange(num_taxels) % 5 + phase = 2.0 * math.pi * 0.5 * t - 1.2 * finger + forces = np.zeros((num_taxels, 3), np.float32) + forces[:, 2] = 1.25 + 0.75 * np.sin(phase) + return forces + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--collection-id", default=DEFAULT_COLLECTION_ID) + parser.add_argument("--joints", type=int, default=DEFAULT_NUM_JOINTS) + parser.add_argument("--taxels", type=int, default=DEFAULT_NUM_TAXELS) + parser.add_argument("--rate", type=float, default=60.0, help="Frames per second") + parser.add_argument( + "--chunks-per-frame", + type=int, + default=1, + help="TactileShape chunks pushed with each frame", + ) + parser.add_argument( + "--duration", + type=float, + default=0.0, + help="Seconds to run; 0 runs until Ctrl+C", + ) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args(argv[1:]) + if not MIN_NUM_JOINTS <= args.joints <= MAX_NUM_JOINTS: + parser.error(f"--joints must be between {MIN_NUM_JOINTS} and {MAX_NUM_JOINTS}") + if args.taxels <= 0 or args.chunks_per_frame <= 0: + parser.error("--taxels and --chunks-per-frame must be positive") + if not math.isfinite(args.rate) or args.rate <= 0: + parser.error("--rate must be finite and positive") + + chunks = split_tracker_metadata(mock_shape(args.joints, args.taxels).to_bytes()) + logger.info( + "Publishing %d joints and %d taxels on '%s'; TactileShape in %d chunk(s)", + args.joints, + args.taxels, + args.collection_id, + len(chunks), + ) + + frames = 0 + pushed_chunks = 0 + with ExitStack() as stack: + stack.enter_context(CloudXRLauncher.launch_context(args)) + data_tracker = TensorPushTracker( + args.collection_id, "tactile", TACTILE_SAMPLE_SIZE + ) + metadata_tracker = TensorPushTracker( + tracker_metadata_collection_id(args.collection_id), + TRACKER_METADATA_TENSOR_IDENTIFIER, + TRACKER_METADATA_CHUNK_SIZE, + ) + trackers = [data_tracker, metadata_tracker] + oxr_session = stack.enter_context( + OpenXRSession( + "TactileFeeder", DeviceIOSession.get_required_extensions(trackers) + ) + ) + session = stack.enter_context( + DeviceIOSession.run(trackers, oxr_session.get_handles()) + ) + + start = time.monotonic() + try: + while args.duration <= 0 or time.monotonic() - start < args.duration: + remaining = start + frames / args.rate - time.monotonic() + if remaining > 0: + time.sleep(remaining) + session.update() + t = time.monotonic() - start + data_tracker.push( + session, TactileData(mock_forces(args.taxels, t)).to_bytes() + ) + for _ in range(args.chunks_per_frame): + metadata_tracker.push(session, chunks[pushed_chunks % len(chunks)]) + pushed_chunks += 1 + frames += 1 + if frames % max(1, int(args.rate * 5)) == 0: + logger.info("Published %d frames", frames) + except KeyboardInterrupt: + pass + + logger.info("Published %d frames in total", frames) + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/examples/tactile/python/isaaccapture_examples/tactile/printer.py b/examples/tactile/python/isaaccapture_examples/tactile/printer.py new file mode 100644 index 0000000000..a4eeae7f68 --- /dev/null +++ b/examples/tactile/python/isaaccapture_examples/tactile/printer.py @@ -0,0 +1,177 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Print tactile frames from a TactileTracker and record them to MCAP. + +The tracker discovers the TactileShape published by +``python -m isaaccapture_examples.tactile.feeder`` and derives the joint and taxel counts from +it. The session records the "tactile" channels to ``--output``. ``--replay FILE`` prints a +recording instead, with no OpenXR runtime. +""" + +import argparse +import math +import sys +import time +from datetime import datetime +from pathlib import Path + +import numpy as np + +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.deviceio_session import ( + DeviceIOSession, + McapRecordingConfig, + McapReplayConfig, + ReplaySession, +) +from isaaccapture.deviceio_trackers import TactileTracker +from isaaccapture.oxr import OpenXRSession + +from .common import DEFAULT_COLLECTION_ID + +GREEN = "\033[32m" +RESET = "\033[0m" + +# Replay is unpaced, so use records rather than wall time to limit terminal output. +REPLAY_PRINT_EVERY_RECORDS = 30 +REPLAY_EOF_STEPS = 30 + + +def print_frame(elapsed: float | None, index: int, forces: np.ndarray) -> None: + first, last = forces[0], forces[-1] + prefix = ( + f"[replay] record {index:6d}" + if elapsed is None + else f"[{elapsed:6.2f}s] step {index:6d}" + ) + print( + f"{prefix} forces: " + f"({first[0]:.2f}, {first[1]:.2f}, {first[2]:.2f}) ... " + f"({last[0]:.2f}, {last[1]:.2f}, {last[2]:.2f})" + ) + + +def run(session, tracker: TactileTracker, args: argparse.Namespace) -> int: + start = time.monotonic() + shape_seen = False + expected_taxels = None + steps = empty = 0 + last_print = -1.0 + last_printed_step = 0 + last_replay_forces = None + while args.duration <= 0 or time.monotonic() - start < args.duration: + session.update() + elapsed = time.monotonic() - start + + shape = tracker.get_metadata(session) + if not shape_seen and shape is not None: + expected_taxels = len(shape.taxel_positions) + print( + f"{GREEN}[{elapsed:6.2f}s] (shape received) " + f"{len(shape.joint_parents)} joints, " + f"{expected_taxels} taxels{RESET}" + ) + shape_seen = True + + data = tracker.get_data(session) + if data is None: + empty += 1 + if args.replay and empty >= REPLAY_EOF_STEPS: + break + else: + empty = 0 + steps += 1 + force_array = np.asarray(data.taxel_force) + if expected_taxels is not None and len(force_array) != expected_taxels: + raise ValueError( + f"frame has {len(force_array)} taxels, " + f"TactileShape declares {expected_taxels}" + ) + if args.replay: + last_replay_forces = force_array.copy() + if steps == 1 or steps % REPLAY_PRINT_EVERY_RECORDS == 0: + print_frame(None, steps, force_array) + last_printed_step = steps + elif elapsed - last_print >= args.print_interval: + print_frame(elapsed, steps, force_array) + last_print = elapsed + if not args.replay: + time.sleep(0.005) + + if args.replay and last_replay_forces is not None and last_printed_step != steps: + print_frame(None, steps, last_replay_forces) + + if args.replay: + print(f"Replayed {steps} tactile records.") + else: + print(f"Processed {steps} session steps containing tactile data.") + return 0 + + +def main(argv: list[str]) -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--collection-id", default=DEFAULT_COLLECTION_ID) + parser.add_argument( + "--duration", + type=float, + default=30.0, + help="Seconds to run; 0 runs until Ctrl+C", + ) + parser.add_argument( + "--print-interval", + type=float, + default=0.5, + help="Seconds between live prints; replay prints every 30 records", + ) + parser.add_argument( + "--output", + type=Path, + help="MCAP to record to (default: ./recordings/tactile_.mcap)", + ) + parser.add_argument( + "--replay", type=Path, help="Print this recording instead of live data" + ) + CloudXRLauncher.add_launcher_arguments(parser) + args = parser.parse_args(argv[1:]) + if not math.isfinite(args.print_interval) or args.print_interval <= 0: + parser.error("--print-interval must be finite and positive") + + # Shape sizes belong to producer metadata, not receiver configuration. + tracker = TactileTracker(args.collection_id) + + if args.replay: + config = McapReplayConfig( + str(args.replay), tracker_names=[(tracker, "tactile")] + ) + with ReplaySession.run(config) as session: + return run(session, tracker, args) + + output = ( + args.output + or Path.cwd() / "recordings" / f"tactile_{datetime.now():%Y%m%d_%H%M%S}.mcap" + ) + output.parent.mkdir(parents=True, exist_ok=True) + trackers = [tracker] + recording = McapRecordingConfig(str(output), tracker_names=[(tracker, "tactile")]) + print(f"Recording to {output}. Start the feeder; Ctrl+C stops.") + try: + with ( + CloudXRLauncher.launch_context(args), + OpenXRSession( + "TactilePrinter", + DeviceIOSession.get_required_extensions(trackers), + ) as oxr_session, + DeviceIOSession.run( + trackers, oxr_session.get_handles(), recording + ) as session, + ): + run(session, tracker, args) + except KeyboardInterrupt: + pass + print(f"Recorded {output}") + return 0 + + +if __name__ == "__main__": + sys.exit(main(sys.argv)) diff --git a/examples/tactile_visualizer/CMakeLists.txt b/examples/tactile_visualizer/CMakeLists.txt new file mode 100644 index 0000000000..6f7f61af3d --- /dev/null +++ b/examples/tactile_visualizer/CMakeLists.txt @@ -0,0 +1,11 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) +install_python_example(DESTINATION examples/tactile_visualizer) +install( + FILES README.md IMPLEMENTATION_REPORT.md urdf_config.example.yaml + DESTINATION examples/tactile_visualizer +) diff --git a/examples/tactile_visualizer/IMPLEMENTATION_REPORT.md b/examples/tactile_visualizer/IMPLEMENTATION_REPORT.md new file mode 100644 index 0000000000..f03e215b16 --- /dev/null +++ b/examples/tactile_visualizer/IMPLEMENTATION_REPORT.md @@ -0,0 +1,94 @@ + + +# Implementation and verification report + +## Implemented + +- Standalone Viser panels: left selection inspector, right properties/files, + floating top toolbar, and central 3D scene. +- Exact FlatBuffer load/export, schema validation, canonical OpenXR-order + default shape, optional bounded atomic local writes, and browser downloads. +- Sharpa `TeleopSession` integration for OpenXR hands and 22 raw glove joints, + two configurable tactile trackers, optional plugin launch, and MCAP replay. +- URDF forward kinematics, hand skeleton, tactile skinning, force colors, + left/right display mirroring, selection, box selection, inspector edits, and + translate/rotate gizmos. +- Explicit session state machine, producer/renderer separation with a one-slot + latest mailbox, frozen pause view, local-shape override, and single-editor + lease. + +## Design decisions and existing-code changes + +- OpenXR defines the 26 ordinals and runtime poses, but no universal bind pose. + The new-file pose and hierarchy are documented tool-defined editor seeds. +- `TactileTaxel` has no unbound representation: all four indices must be valid, + including zero-weight slots. Added grid taxels therefore repeat wrist index 1 + and use weights `[1, 0, 0, 0]`. +- Producer metadata fixes one `TactileShape` for a tactile collection/session. + The tool caches it and validates each `TactileData.taxel_force` length; a + changed producer layout requires a new collection/session. +- The only functional existing-source change is a generic verifier-backed + `Serialized::from_bytes()` binding, strictly required to load exact schema + bytes. Parent example/test CMake lists changed only for installation/testing. + +## Files changed + +- `examples/tactile_visualizer/`: executable package, model/source adapters, + URDF config, README, and this report. +- `tests/python/examples/tactile_visualizer/`: deterministic model, mock-source, + config, and Viser construction tests. +- `src/core/schema/python/schema_serialized.h` and its tactile-shape test: + verified standalone FlatBuffer decoding. +- Parent example/test `CMakeLists.txt`: register the new package and tests. + +## Automated verification + +An editable native build and the focused suite were run locally: + +```bash +PYTHONPATH=examples/tactile_visualizer/python \ + .venv-tactile/bin/python -m pytest -q \ + tests/python/core/schema/test_tactile_shape.py \ + tests/python/examples/tactile_visualizer +# 33 passed +``` + +The native schema extension was built and installed into the test environment. +`SKIP=check-copyright-year pre-commit run --all-files` passed every enabled hook. +A three-second mock-mode server smoke run produced no application error; it was +terminated by the test timeout as intended. + +The tests are hardware-free. They do not claim that a particular Sharpa URDF, +plugin build, browser/GPU, or physical sensor has been verified. + +## Manual target-system checklist + +1. Open offline mode, load a known shape, expand representative joints/taxels, + export it, and byte-compare or decode the result. +2. In mock mode, confirm palm/fist motion, URDF FK, skeleton motion, taxel + skinning, force colors, hand mirroring, pause/resume, and timeout behavior. +3. Verify click, taxel/Shift-joint rectangle selection, local/world gizmo axes, + numeric edits, rejected invalid weights/indices, and one-editor enforcement + with two browsers. +4. With the target Sharpa installation, verify the SDK config, plugin path, + RAW-to-URDF mapping, tactile collection IDs, and physical hand direction. +5. Record and replay one session; confirm MCAP channel labels and final-frame + behavior. Disconnect the producer and confirm that the frozen view remains + editable. + +## Known integration limits + +- The default bind pose is an editor seed, not a standardized OpenXR bind pose + or a glove calibration. +- Viser 1.1.1 transform-control events do not expose mouse buttons. The toolbar + uses explicit Translate/Rotate modes rather than pretending to implement + left-button/right-button semantics. +- Pointer, rendering, and physical-device behavior require the manual checks + above; automated tests exercise only deterministic Python boundaries. +- Viser 1.1.1 exposes no folder-expansion callback. Shape leaf controls are + instantiated immediately but remain visually hidden in collapsed folders. +- Live hardware, vendor URDF alignment, browser pointer ergonomics, and + physical force calibration have not been verified. diff --git a/examples/tactile_visualizer/README.md b/examples/tactile_visualizer/README.md new file mode 100644 index 0000000000..ca0e8abe17 --- /dev/null +++ b/examples/tactile_visualizer/README.md @@ -0,0 +1,149 @@ + + +# TactileShape visualizer and editor + +A local developer tool for creating, inspecting, editing, and exporting +`TactileShape`, while optionally showing Sharpa glove URDF forward kinematics, +OpenXR hand joints, and `TactileData` forces. The browser is a view onto one +server-side model; the first connected browser owns the editing lease and later +clients are read-only. + +```text +avatar_raw_{left,right} -> JointStateSource -> URDF update_cfg() -> glove FK +/hand/{left,right} -> HandsSource -> OpenXR skeleton --+ +tactile_{left,right} -> TactileTracker -> shape + forces ----+-> viser + latest-only slot +``` + +The tool uses meters and XYZW quaternions in schema files. Position fields in +the UI are centimeters. `TactileShape` taxels are animated with the schema's +four-weight linear-blend-skinning equation. Force is in newtons and uses +`t = clamp(||force|| / 30, 0, 1)`, linearly interpolating blue to red. + +Coordinate and unit conventions: + +- Schema positions are hand-root-space meters; GUI positions are centimeters. +- Schema and hand-source quaternions are XYZW; Viser receives WXYZ. +- Joints use `XrHandJointEXT` ordinals 0–25. Live poses are transformed into + wrist-root space; the wrist is world origin. +- The stored shape is canonical right-hand data. Left display reflects X for + the skeleton, taxels, and URDF without changing exported bytes. +- A taxel frame uses tangent as X, normal as Z, and `normal × tangent` as Y; + edits re-normalize and orthogonalize the two stored vectors. +- Raw glove values are passed unchanged to matching URDF actuated joints + (normally radians). URDF config offset is centimeters, scale is unitless, + and config rotation is XYZW. + +## Install and run + +```bash +# Python 3.11+ and an IsaacCapture build exposing its Python modules. +uv pip install -e ./examples/tactile_visualizer + +# Safe offline editor (no OpenXR runtime or hardware). +python -m isaaccapture_examples.tactile_visualizer \ + --urdf /path/to/sharpa_hand.urdf \ + --urdf-config ./examples/tactile_visualizer/urdf_config.example.yaml \ + --shape /optional/existing_shape.bin + +# Deterministic palm/fist animation, raw joints, shape, and force samples. +python -m isaaccapture_examples.tactile_visualizer \ + --urdf /path/to/sharpa_hand.urdf --mock + +# Live DeviceIO. This starts CloudXR; launch the installed Sharpa plugin too. +python -m isaaccapture_examples.tactile_visualizer \ + --urdf /path/to/sharpa_hand.urdf --live --launch-plugin --accept-eula + +# Headless MCAP replay; channel names are configurable. +python -m isaaccapture_examples.tactile_visualizer \ + --urdf /path/to/sharpa_hand.urdf --replay capture.mcap +``` + +The live path deliberately reuses the Sharpa sample's `HandsSource`, +`JointStateSource`, RAW joint names, collection IDs, `PluginConfig`, and +`TeleopSession`. Tactile input is added through `TactileTracker`; each hand's +collection is configurable with `--left-tactile-collection` and +`--right-tactile-collection`. MCAP channel labels are independently configurable +with the corresponding `--*-tactile-channel` options. + +Use `urdf_config.example.yaml` to map Sharpa raw names to actuated URDF names and +to set the URDF scale/root transform. Unmapped URDF joints first try the same raw +name and otherwise receive zero. `ViserUrdf.update_cfg()` evaluates and renders +the URDF forward kinematics. + +For a live session: + +1. Build/install IsaacCapture, the Sharpa Avatar plugin, and its glove URDF. +2. Copy and adjust `urdf_config.example.yaml`, mapping actuated URDF joints to + the documented `avatar_raw_{left,right}` joint names. +3. Run with `--live --launch-plugin --accept-eula`; add `--sdk-config`, + `--plugin-search-path`, and tactile collection overrides when required. +4. Confirm that the toolbar reaches `LIVE_PLAYING`. This requires a skeleton + sample, raw-joint sample, fixed producer shape, and length-matched tactile + sample for the selected hand. + +A tactile collection/session has one fixed metadata `TactileShape`; a producer +with a changed layout must create a new collection/session. The tool caches that +metadata and validates every force count against it. `--liveness-timeout` +defaults to 1 second. Producer and renderer communicate through a one-slot +latest-value mailbox, so input cannot create an unbounded queue. + +## Interaction and states + +- Click a taxel to select it. Shift-click a hand joint to inspect it. +- In `Box select`, an unmodified rectangle selects taxels and Shift-drag + selects joints. Use `Navigate` for native orbit/pan/zoom. Clicking empty + space clears the selection. +- `Translate` exposes translation axes. `Rotate` exposes rotation rings and + changes taxel normal/tangent frames without moving taxel positions. Viser's + transform control does not report mouse buttons, so the explicit mode toggle + is the unambiguous supported interaction. Its native gizmo supplies axis hover + highlighting. +- A single taxel uses its local frame; a multi-taxel gizmo uses world axes at + the selection centroid. Joint axes are display-only. +- Live playback is read-only. Pause, disconnect, or use offline mode to edit. + The producer keeps running while paused, but the 3D view remains frozen and + only the newest unread sample is retained. +- Loading or editing creates a local shape override. Resume never silently + replaces it with producer metadata. Switching hands mirrors the canonical + shape and URDF for display without changing exported bytes. + +The explicit states are `OFFLINE`, `LIVE_PLAYING`, `LIVE_PAUSED`, and +`DISCONNECTED`. Live-message time, not one-time metadata receipt, drives the +configurable timeout. The last view is preserved and becomes editable. + +## Files and default shape + +`Load TactileShape` validates the uploaded FlatBuffer before replacing the +active model. `Export active shape` downloads exact `TactileShape` bytes. +`Create New` downloads a 26-joint, zero-taxel shape but intentionally does not +replace the active model. Pass `--allow-server-write-root DIR` to additionally +enable an atomic local write constrained below that explicit directory. + +`Add Taxels` appends 0–125 points in x-outer/y-middle/z-inner order from the +five-value grid on each axis. The schema has no unbound sentinel, so new taxels +repeat wrist index 1 in all slots and use weights `[1, 0, 0, 0]`. + +OpenXR defines the 26 joint ordinals and runtime joint locations, but does not +define one universal bind pose. The built-in right-hand neutral pose is therefore +a clearly labeled tool default, not a device calibration. It follows the +[Khronos joint order](https://registry.khronos.org/OpenXR/specs/1.0/html/xrspec.html#XrHandJointEXT) +and is suitable for bootstrapping files that should later be calibrated against +the intended glove. + +The right-side schema hierarchy is collapsed by default. Shape leaves are +editable only offline or while paused; live data is read-only. Viser 1.1.1 has +no folder-expansion callback, so leaf controls exist server-side immediately +but remain visually hidden until their folders are opened. + +## Validation boundary + +Automated tests cover serialization, malformed input, state transitions, +latest-only behavior, skinning/edit transforms, projection selection, force +colors, configuration, and the mock source. CI cannot establish physical glove +accuracy, browser pointer ergonomics, or vendor URDF alignment; follow the +manual checklist in [IMPLEMENTATION_REPORT.md](IMPLEMENTATION_REPORT.md) on the +target setup before treating those aspects as verified. diff --git a/examples/tactile_visualizer/pyproject.toml b/examples/tactile_visualizer/pyproject.toml new file mode 100644 index 0000000000..8ae60b799f --- /dev/null +++ b/examples/tactile_visualizer/pyproject.toml @@ -0,0 +1,27 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[build-system] +requires = ["hatchling"] +build-backend = "hatchling.build" + +[project] +name = "isaaccapture-examples-tactile-visualizer" +version = "0.0.0" +description = "Developer tactile shape editor and live DeviceIO visualizer" +requires-python = ">=3.11,<3.14" +dependencies = [ + "isaaccapture[cloudxr]", + "mcap>=1.2.0", + "numpy>=1.23.0", + "pyyaml>=6.0", + "viser>=1.1.1,<2", + "yourdfpy>=0.0.58", +] + +[tool.hatch.build.targets.wheel] +only-include = ["python/isaaccapture_examples/tactile_visualizer"] +sources = ["python"] + +[tool.uv.sources] +isaaccapture = { path = "../..", editable = true } diff --git a/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/__init__.py b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/__init__.py new file mode 100644 index 0000000000..7d695145ef --- /dev/null +++ b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/__init__.py @@ -0,0 +1,4 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Developer-oriented tactile shape editor and live visualizer.""" diff --git a/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/__main__.py b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/__main__.py new file mode 100644 index 0000000000..5a96edeaaa --- /dev/null +++ b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/__main__.py @@ -0,0 +1,8 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import sys + +from .cli import main + +sys.exit(main()) diff --git a/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/app.py b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/app.py new file mode 100644 index 0000000000..78874f1c7d --- /dev/null +++ b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/app.py @@ -0,0 +1,1348 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Viser application for editing and visualizing tactile hand data.""" + +from __future__ import annotations + +import logging +import time +from pathlib import Path +from typing import Any + +import numpy as np +import viser +from viser.extras import ViserUrdf + +from .model import ( + OPENXR_JOINT_NAMES, + ROOT_PARENT, + WRIST, + SessionController, + ShapeDocument, + append_grid_taxels, + apply_display_side, + atomic_write_within, + default_openxr_shape, + force_colors, + frame_wxyz, + matrix_to_quaternion_xyzw, + mirror_pose, + normalize, + project_points, + quaternion_xyzw_to_matrix, + resolve_current_joints, + set_taxel_display_frame, + set_taxel_display_position, + skin_taxels, +) +from .sources import SnapshotSource, SourceSnapshot +from .urdf_config import UrdfConfig, map_raw_joints + +logger = logging.getLogger("isaaccapture.examples.tactile_visualizer.app") + + +def _modifier(event: Any) -> str: + return str(getattr(event, "modifier", "")).lower() + + +def _parse_numbers(text: str, count: int, kind=float) -> np.ndarray: + values = [kind(item.strip()) for item in text.split(",") if item.strip()] + if len(values) != count: + raise ValueError(f"expected {count} comma-separated values") + value = np.asarray(values) + if value.dtype.kind == "f": + if not np.isfinite(value).all(): + raise ValueError("values must be finite") + if np.any(np.abs(value) > 999.99): + raise ValueError("values must be in [-999.99, 999.99]") + return value + + +def _validate_gui_vector(raw: Any) -> np.ndarray: + value = np.asarray(raw, dtype=np.float64) + if not np.isfinite(value).all(): + raise ValueError("values must be finite") + if np.any(np.abs(value) > 999.99): + raise ValueError("values must be in [-999.99, 999.99]") + return value + + +class TactileVisualizer: + """Own the active shape, live snapshot, Viser scene, and GUI callbacks.""" + + def __init__( + self, + *, + server: viser.ViserServer, + urdf_path: Path, + urdf_config_path: Path | None, + urdf_config: UrdfConfig, + document: ShapeDocument, + document_name: str, + source: SnapshotSource | None, + side: str, + liveness_timeout_s: float, + taxel_size_m: float, + write_root: Path | None, + ) -> None: + self.server = server + self.urdf_path = urdf_path + self.urdf_config_path = urdf_config_path + self.urdf_config = urdf_config + self.document = document + self.document_name = document_name + self.source = source + self.side = side + self.controller = SessionController(source is not None, liveness_timeout_s) + self.taxel_size_m = taxel_size_m + self.write_root = write_root.resolve() if write_root is not None else None + + self.editor_client_id: int | None = None + self.sequence = 0 + self.latest_snapshot: SourceSnapshot | None = None + self.display_snapshot: SourceSnapshot | None = None + self.latest_shapes: dict[str, ShapeDocument] = {} + self.latest_forces: dict[str, np.ndarray] = {} + self.display_forces: dict[str, np.ndarray] = {} + self._loaded_remote_sides: set[str] = set() + self.user_shape_override = False + self.selected_kind: str | None = None + self.selected_indices: set[int] = set() + self.transform_mode = "Translate" + self.pointer_mode = "Navigate" + self._programmatic = False + self._last_error = "" + self._gizmo = None + self._selection_axes = None + self._gizmo_context: dict[str, Any] | None = None + self._tree_root = None + self._tree_joint_inputs: list[dict[str, Any]] = [] + self._tree_taxel_inputs: list[dict[str, Any]] = [] + self._taxel_handles: list[Any] = [] + self._joint_handles: list[Any] = [] + self._bone_handles: dict[int, Any] = {} + self._current_positions = document.joint_bind_positions.astype(np.float64) + self._current_orientations = document.joint_bind_orientations.astype(np.float64) + self._display_taxel_positions = document.taxel_positions.copy() + self._display_taxel_normals = document.taxel_normals.copy() + self._display_taxel_tangents = document.taxel_tangents.copy() + + self._configure_scene() + self._build_panels() + self._build_schema_tree() + self._build_scene_handles() + self._install_callbacks() + self._set_pointer_mode() + self._refresh_editability() + + def _configure_scene(self) -> None: + self.server.scene.set_up_direction("+y") + self.server.scene.add_grid( + "/grid", + width=0.5, + height=0.5, + plane="xy", + cell_size=0.01, + section_size=0.05, + position=(0.0, 0.07, -0.012), + ) + self.server.initial_camera.position = (0.16, 0.11, 0.28) + self.server.initial_camera.look_at = (0.0, 0.07, 0.0) + self.server.initial_camera.up_direction = (0.0, 1.0, 0.0) + self.urdf = None + self._urdf_root = None + self.urdf_joint_names: tuple[str, ...] = () + self._load_urdf() + + def _load_urdf(self) -> None: + if self.urdf is not None: + self.urdf.remove() + if self._urdf_root is not None: + self._urdf_root.remove() + q = normalize( + np.asarray(self.urdf_config.rotation_xyzw, dtype=np.float64), + label="URDF rotation_xyzw", + ) + scale = (-1.0, 1.0, 1.0) if self.side == "left" else 1.0 + self._urdf_root = self.server.scene.add_frame( + "/glove", + show_axes=False, + position=np.asarray(self.urdf_config.offset_cm, dtype=np.float64) / 100.0, + wxyz=q[[3, 0, 1, 2]], + scale=scale, + ) + self.urdf = None + self.urdf_joint_names = () + try: + self.urdf = ViserUrdf( + self.server, + self.urdf_path, + scale=self.urdf_config.scale, + root_node_name="/glove/model", + mesh_color_override=(0.3, 0.55, 0.95, 0.35), + ) + self.urdf_joint_names = self.urdf.get_actuated_joint_names() + except Exception as error: + logger.exception("Failed to load URDF %s", self.urdf_path) + self._last_error = f"URDF unavailable: {error}" + + def _build_panels(self) -> None: + selection = self.server.gui.add_panel() + selection.dock_left() + selection.set_width(350) + with selection.add_tab("Selection Inspector"): + self.selection_summary = self.server.gui.add_markdown("No selection") + self.selection_placeholder = self.server.gui.add_markdown("") + with self.server.gui.add_folder("Selected object", expand_by_default=True): + bounds = (-999.99, -999.99, -999.99) + upper = (999.99, 999.99, 999.99) + self.position_input = self.server.gui.add_vector3( + "Position (cm)", (0.0, 0.0, 0.0), min=bounds, max=upper, step=0.01 + ) + self.normal_input = self.server.gui.add_vector3( + "Normal", (0.0, 0.0, 1.0), min=bounds, max=upper, step=0.01 + ) + self.tangent_input = self.server.gui.add_vector3( + "Tangent", (1.0, 0.0, 0.0), min=bounds, max=upper, step=0.01 + ) + self.orientation_text = self.server.gui.add_text( + "Bind quaternion XYZW", "" + ) + self.parent_text = self.server.gui.add_text("Parent index", "") + self.indices_text = self.server.gui.add_text("Joint indices", "") + self.weights_text = self.server.gui.add_text("Joint weights", "") + self.force_text = self.server.gui.add_text( + "Force magnitude (N)", "", disabled=True + ) + self.current_position_text = self.server.gui.add_text( + "Current position (cm)", "", disabled=True + ) + self.current_orientation_text = self.server.gui.add_text( + "Current quaternion XYZW", "", disabled=True + ) + + controls = self.server.gui.add_panel() + controls.dock_right() + controls.set_width(390) + with controls.add_tab("Controls / Schema"): + self.paths_markdown = self.server.gui.add_markdown("") + self._update_paths_text() + with self.server.gui.add_folder("File operations", expand_by_default=True): + self.upload = self.server.gui.add_upload_button( + "Load TactileShape", mime_type="application/octet-stream" + ) + self.filename = self.server.gui.add_text( + "Download / server filename", "tactile_shape.bin" + ) + self.create_new = self.server.gui.add_button("Create New (do not load)") + self.export = self.server.gui.add_button("Export active shape") + note = ( + f"Atomic server writes enabled below `{self.write_root}`." + if self.write_root is not None + else "Server writes disabled; files are browser downloads." + ) + self.file_status = self.server.gui.add_markdown(note) + with self.server.gui.add_folder("Add taxels", expand_by_default=True): + self.add_taxel_count = self.server.gui.add_number( + "Amount (0-125)", 0, min=0, max=125, step=1 + ) + self.add_taxels_button = self.server.gui.add_button("Add Taxels") + self.server.gui.add_markdown( + "Deterministic order: x outer, y middle, z inner; each axis " + "uses `[-4, -2, 0, 2, 4] cm`." + ) + self.inspector_status = self.server.gui.add_markdown("") + self.inspector_container = self.server.gui.add_folder( + "Full schema", expand_by_default=True + ) + + toolbar = self.server.gui.add_panel() + toolbar.float(x=0.27, y=0.01, width=0.46, height=0.16) + with toolbar.add_tab("Tactile toolbar"): + self.state_markdown = self.server.gui.add_markdown("") + self.reset_view_button = self.server.gui.add_button("Reset view") + self.pause_button = self.server.gui.add_button("Pause / Resume") + self.pointer_mode_control = self.server.gui.add_dropdown( + "Pointer mode", ("Navigate", "Box select"), initial_value="Navigate" + ) + self.mode_control = self.server.gui.add_dropdown( + "Taxel gizmo", + ("Translate", "Rotate"), + initial_value=self.transform_mode, + ) + self.side_control = self.server.gui.add_dropdown( + "Hand", ("left", "right"), initial_value=self.side + ) + + def _update_paths_text(self) -> None: + config = ( + str(self.urdf_config_path) + if self.urdf_config_path is not None + else "built-in defaults" + ) + self.paths_markdown.content = ( + f"**URDF:** `{self.urdf_path}` \n" + f"**URDF config:** `{config}` \n" + f"**Document:** `{self.document_name}`" + ) + + def _build_schema_tree(self) -> None: + if self._tree_root is not None: + self._tree_root.remove() + self._tree_joint_inputs.clear() + self._tree_taxel_inputs.clear() + with self.inspector_container: + self._tree_root = self.server.gui.add_folder( + "TactileShape", expand_by_default=False + ) + with self._tree_root: + with self.server.gui.add_folder( + f"joints [{self.document.num_joints}]", expand_by_default=False + ): + for index in range(self.document.num_joints): + name = ( + OPENXR_JOINT_NAMES[index] + if index < len(OPENXR_JOINT_NAMES) + else str(index) + ) + with self.server.gui.add_folder( + f"{index}: {name}", expand_by_default=False + ): + fields = self._add_joint_tree_fields() + self._tree_joint_inputs.append(fields) + self._bind_tree_fields("joint", index, fields) + with self.server.gui.add_folder( + f"taxels [{self.document.num_taxels}]", expand_by_default=False + ): + for index in range(self.document.num_taxels): + with self.server.gui.add_folder( + f"taxel[{index}]", expand_by_default=False + ): + fields = self._add_taxel_tree_fields() + self._tree_taxel_inputs.append(fields) + self._bind_tree_fields("taxel", index, fields) + self._data_tree = self.server.gui.add_folder( + "TactileData", expand_by_default=False + ) + with self._data_tree: + self._data_tree_text = self.server.gui.add_markdown( + "`taxel_force` (N): no sample" + ) + self._sync_tree_values() + + def _add_joint_tree_fields(self) -> dict[str, Any]: + bounds = (-999.99, -999.99, -999.99) + upper = (999.99, 999.99, 999.99) + return { + "position": self.server.gui.add_vector3( + "bind_pose.position (cm)", + (0.0, 0.0, 0.0), + min=bounds, + max=upper, + step=0.01, + ), + "orientation": self.server.gui.add_text("bind_pose.orientation XYZW", ""), + "parent": self.server.gui.add_text("parent", ""), + } + + def _add_taxel_tree_fields(self) -> dict[str, Any]: + bounds = (-999.99, -999.99, -999.99) + upper = (999.99, 999.99, 999.99) + return { + "position": self.server.gui.add_vector3( + "position (cm)", + (0.0, 0.0, 0.0), + min=bounds, + max=upper, + step=0.01, + ), + "normal": self.server.gui.add_vector3( + "normal", (0.0, 0.0, 1.0), min=bounds, max=upper, step=0.01 + ), + "tangent": self.server.gui.add_vector3( + "tangent", (1.0, 0.0, 0.0), min=bounds, max=upper, step=0.01 + ), + "indices": self.server.gui.add_text("joint_indices", ""), + "weights": self.server.gui.add_text("joint_weights", ""), + } + + def _bind_tree_fields(self, kind: str, index: int, fields: dict[str, Any]) -> None: + for field, handle in fields.items(): + + @handle.on_update + def _(event, kind=kind, index=index, field=field) -> None: + self._edit_field_event(event, kind, index, field) + + @staticmethod + def _format_vector(values: np.ndarray) -> str: + return ", ".join(f"{float(value):.2f}" for value in values) + + def _sync_tree_values(self) -> None: + for index, fields in enumerate(self._tree_joint_inputs): + self._set_value( + fields["position"], + tuple((self.document.joint_bind_positions[index] * 100.0).tolist()), + ) + self._set_value( + fields["orientation"], + self._format_vector(self.document.joint_bind_orientations[index]), + ) + parent = int(self.document.joint_parents[index]) + self._set_value( + fields["parent"], + "ROOT_PARENT" if parent == int(ROOT_PARENT) else str(parent), + ) + for index, fields in enumerate(self._tree_taxel_inputs): + self._set_value( + fields["position"], + tuple((self.document.taxel_positions[index] * 100.0).tolist()), + ) + self._set_value( + fields["normal"], tuple(self.document.taxel_normals[index].tolist()) + ) + self._set_value( + fields["tangent"], + tuple(self.document.taxel_tangents[index].tolist()), + ) + self._set_value( + fields["indices"], + ", ".join(map(str, self.document.taxel_joint_indices[index])), + ) + self._set_value( + fields["weights"], + self._format_vector(self.document.taxel_joint_weights[index]), + ) + + def _build_scene_handles(self) -> None: + for handle in ( + self._taxel_handles + + self._joint_handles + + list(self._bone_handles.values()) + ): + handle.remove() + self._taxel_handles.clear() + self._joint_handles.clear() + self._bone_handles.clear() + for joint in range(self.document.num_joints): + handle = self.server.scene.add_icosphere( + f"/hand/joints/{joint:02d}", + radius=0.0025, + color=(255, 180, 60), + subdivisions=2, + opacity=0.55, + ) + + @handle.on_click + def _(event, joint=joint) -> None: + if "shift" not in _modifier(event): + return + if self._claim_editor(event.client_id, event.client): + self._select("joint", {joint}) + + self._joint_handles.append(handle) + self._rebuild_bones() + for taxel in range(self.document.num_taxels): + handle = self.server.scene.add_box( + f"/tactile/taxels/{taxel:05d}", + dimensions=(self.taxel_size_m, self.taxel_size_m, 0.0005), + color=(0, 0, 255), + flat_shading=True, + cast_shadow=False, + ) + + @handle.on_click + def _(event, taxel=taxel) -> None: + if self._claim_editor(event.client_id, event.client): + self._select("taxel", {taxel}) + + self._taxel_handles.append(handle) + self._update_rendered_values() + + @staticmethod + def _segment_wxyz(delta: np.ndarray) -> np.ndarray: + z_axis = normalize(delta, label="bone segment") + seed = np.array([1.0, 0.0, 0.0]) + if abs(float(np.dot(seed, z_axis))) > 0.9: + seed = np.array([0.0, 1.0, 0.0]) + y_axis = normalize(np.cross(z_axis, seed)) + x_axis = np.cross(y_axis, z_axis) + xyzw = matrix_to_quaternion_xyzw(np.column_stack((x_axis, y_axis, z_axis))) + return xyzw[[3, 0, 1, 2]] + + def _rebuild_bones(self) -> None: + for handle in self._bone_handles.values(): + handle.remove() + self._bone_handles.clear() + positions, _ = self._display_joint_pose() + for child, parent in enumerate(self.document.joint_parents): + if int(parent) == int(ROOT_PARENT): + continue + delta = positions[child] - positions[int(parent)] + length = float(np.linalg.norm(delta)) + if length < 1e-8: + continue + self._bone_handles[child] = self.server.scene.add_cylinder( + f"/hand/bones/{child:02d}", + radius=0.0006, + height=length, + color=(255, 170, 50), + opacity=0.35, + radial_segments=8, + cast_shadow=False, + position=(positions[child] + positions[int(parent)]) / 2.0, + wxyz=self._segment_wxyz(delta), + ) + + def _install_callbacks(self) -> None: + @self.server.on_client_connect + def _(client) -> None: + if self.editor_client_id is None: + self.editor_client_id = client.client_id + client.add_notification( + title="Editing enabled", + body="This browser owns the single editing lease.", + ) + else: + client.add_notification( + title="Read-only client", + body="Another browser owns the editing lease.", + color="yellow", + ) + + @self.server.on_client_disconnect + def _(client) -> None: + if client.client_id == self.editor_client_id: + self.editor_client_id = None + + @self.reset_view_button.on_click + def _(event) -> None: + camera = event.client.camera + camera.position = (0.16, 0.11, 0.28) + camera.look_at = (0.0, 0.07, 0.0) + camera.up_direction = (0.0, 1.0, 0.0) + + @self.pause_button.on_click + def _(event) -> None: + if not self._claim_editor(event.client_id, event.client): + return + now = time.monotonic() + if self.controller.paused: + self.controller.resume(now) + if self.latest_snapshot is not None: + self.display_snapshot = self.latest_snapshot + self.display_forces = { + side: value.copy() for side, value in self.latest_forces.items() + } + self._update_rendered_values() + else: + self.controller.pause() + self._refresh_editability() + + @self.pointer_mode_control.on_update + def _(event) -> None: + if self._programmatic: + return + if not self._claim_editor(event.client_id, event.client): + self._set_value(self.pointer_mode_control, self.pointer_mode) + return + self.pointer_mode = str(event.target.value) + self._set_pointer_mode() + + @self.mode_control.on_update + def _(event) -> None: + if self._programmatic: + return + if not self._claim_editor(event.client_id, event.client): + self._set_value(self.mode_control, self.transform_mode) + return + self.transform_mode = str(event.target.value) + self._update_selection_gizmo() + + @self.side_control.on_update + def _(event) -> None: + if self._programmatic: + return + if not self._claim_editor(event.client_id, event.client): + self._set_value(self.side_control, self.side) + return + self.side = str(event.target.value) + self._load_urdf() + if self.source is not None: + ready = self.latest_snapshot is not None and self._source_ready( + self.latest_snapshot + ) + self.controller.requirements_changed( + time.monotonic(), ready=bool(ready) + ) + if not self.user_shape_override and self.side in self.latest_shapes: + self._replace_document(self.latest_shapes[self.side].copy()) + self._loaded_remote_sides.add(self.side) + self._select(None, set()) + self._update_rendered_values() + + @self.upload.on_upload + def _(event) -> None: + if not self._claim_editor(event.client_id, event.client): + return + if not self.controller.editing_enabled: + self._report_error("Pause live playback before loading.", event.client) + return + try: + document = ShapeDocument.from_bytes(event.target.value.content) + self.user_shape_override = True + self.document_name = event.target.value.name + self._replace_document(document) + self._update_paths_text() + self._set_status(f"Loaded `{event.target.value.name}`.") + except Exception as error: + self._report_error(f"Load rejected: {error}", event.client) + + @self.create_new.on_click + def _(event) -> None: + if not self._claim_editor(event.client_id, event.client): + return + if not self.controller.editing_enabled: + self._report_error("Pause live playback before creating.", event.client) + return + try: + self._deliver_file( + default_openxr_shape().to_bytes(), + self.filename.value, + event.client, + "Created a 26-joint, zero-taxel shape without loading it.", + ) + except Exception as error: + self._report_error(f"Create failed: {error}", event.client) + + @self.export.on_click + def _(event) -> None: + if not self._claim_editor(event.client_id, event.client): + return + try: + self._deliver_file( + self.document.to_bytes(), + self.filename.value, + event.client, + "Exported the active shape.", + ) + except Exception as error: + self._report_error(f"Export failed: {error}", event.client) + + @self.add_taxels_button.on_click + def _(event) -> None: + if not self._claim_editor(event.client_id, event.client): + return + if not self.controller.editing_enabled: + self._report_error( + "Pause live playback before adding taxels.", event.client + ) + return + try: + amount = int(self.add_taxel_count.value) + append_grid_taxels(self.document, amount) + self._mark_local_edit() + self._build_schema_tree() + self._build_scene_handles() + self._refresh_editability() + self._set_status(f"Added {amount} taxel(s).") + except Exception as error: + self._report_error(f"Add Taxels rejected: {error}", event.client) + + for handle, field in ( + (self.position_input, "position"), + (self.normal_input, "normal"), + (self.tangent_input, "tangent"), + (self.orientation_text, "orientation"), + (self.parent_text, "parent"), + (self.indices_text, "indices"), + (self.weights_text, "weights"), + ): + + @handle.on_update + def _(event, field=field) -> None: + if self.selected_kind is None or len(self.selected_indices) != 1: + return + self._edit_field_event( + event, + self.selected_kind, + next(iter(self.selected_indices)), + field, + ) + + def _set_pointer_mode(self) -> None: + self.server.scene.remove_click_callback() + self.server.scene.remove_rect_select_callback() + if self.pointer_mode == "Box select": + + @self.server.scene.on_rect_select() + def _(event) -> None: + if not self._claim_editor(event.client_id, event.client): + return + kind = "joint" if "shift" in _modifier(event) else "taxel" + points = ( + self._display_joint_pose()[0] + if kind == "joint" + else self._display_taxel_positions + ) + screen, in_front = project_points( + points, + event.client.camera.position, + event.client.camera.wxyz, + event.client.camera.fov, + event.client.camera.aspect, + ) + lower = np.minimum(event.screen_min, event.screen_max) + upper = np.maximum(event.screen_min, event.screen_max) + inside = ( + in_front + & np.all(screen >= lower, axis=1) + & np.all(screen <= upper, axis=1) + ) + self._select(kind, set(np.flatnonzero(inside).tolist())) + + else: + + @self.server.scene.on_click() + def _(event) -> None: + if not self._claim_editor(event.client_id, event.client): + return + points = np.concatenate( + (self._display_taxel_positions, self._display_joint_pose()[0]), + axis=0, + ) + if len(points): + screen, visible = project_points( + points, + event.client.camera.position, + event.client.camera.wxyz, + event.client.camera.fov, + event.client.camera.aspect, + ) + distance = np.linalg.norm(screen - event.screen_pos, axis=1) + if np.any(visible & (distance < 0.025)): + return + self._select(None, set()) + + def _claim_editor(self, client_id: int | None, client: Any | None) -> bool: + if client_id is None: + return True + if self.editor_client_id is None: + self.editor_client_id = client_id + if client_id == self.editor_client_id: + return True + if client is not None: + client.add_notification( + title="Read-only client", + body="Another browser owns the editing lease.", + color="yellow", + ) + return False + + def _set_value(self, handle: Any, value: Any) -> None: + self._programmatic = True + try: + handle.value = value + finally: + self._programmatic = False + + def _set_status(self, message: str) -> None: + self._last_error = "" + self.file_status.content = message + + def _report_error(self, message: str, client: Any | None = None) -> None: + logger.error("%s", message) + self._last_error = message + self.file_status.content = f"**Error:** {message}" + self._update_state_text() + if client is not None: + client.add_notification( + title="Tactile visualizer", body=message, color="red" + ) + + def _deliver_file( + self, payload: bytes, filename: str, client: Any | None, message: str + ) -> None: + clean_name = Path(filename).name + if not clean_name: + raise ValueError("filename is empty") + results = [] + if client is not None: + client.send_file_download(clean_name, payload) + results.append(f"browser download `{clean_name}`") + if self.write_root is not None: + target = atomic_write_within(self.write_root, filename, payload) + results.append(f"atomic server write `{target}`") + if not results: + raise RuntimeError("no browser client and no server write root") + self._set_status(f"{message} Delivered by " + " and ".join(results) + ".") + + def _replace_document(self, document: ShapeDocument) -> None: + document.validate() + self.document = document + self.selected_kind = None + self.selected_indices.clear() + self._remove_selection_visuals() + self._build_schema_tree() + self._build_scene_handles() + self._refresh_editability() + + def _edit_field_event(self, event: Any, kind: str, index: int, field: str) -> None: + if self._programmatic: + return + if not self._claim_editor(event.client_id, event.client): + self._sync_tree_values() + self._refresh_property_values() + return + if not self.controller.editing_enabled: + self._report_error("Pause live playback before editing.", event.client) + self._sync_tree_values() + self._refresh_property_values() + return + backup = self.document.copy() + try: + self._apply_field(kind, index, field, event.target.value) + self.document.validate() + if kind == "joint" and field == "parent": + self._rebuild_bones() + self._mark_local_edit() + self._sync_tree_values() + self._update_rendered_values() + except Exception as error: + self.document = backup + self._report_error(f"Edit rejected: {error}", event.client) + self._sync_tree_values() + self._update_rendered_values() + + def _apply_field(self, kind: str, index: int, field: str, raw: Any) -> None: + if kind == "joint": + if field == "position": + value = _validate_gui_vector(raw) / 100.0 + self.document.joint_bind_positions[index] = value + elif field == "orientation": + self.document.joint_bind_orientations[index] = normalize( + _parse_numbers(str(raw), 4), label="bind quaternion" + ) + elif field == "parent": + text = str(raw).strip() + parent = ( + int(ROOT_PARENT) if text.upper() == "ROOT_PARENT" else int(text) + ) + if parent == index: + raise ValueError("a joint cannot be its own parent") + if ( + parent != int(ROOT_PARENT) + and not 0 <= parent < self.document.num_joints + ): + raise ValueError("parent is outside the joint array") + self.document.joint_parents[index] = np.uint32(parent) + else: + raise ValueError(f"{field} is not a joint field") + return + if kind != "taxel": + raise ValueError("no editable object selected") + if field == "position": + value = _validate_gui_vector(raw) / 100.0 + self.document.taxel_positions[index] = value + elif field in ("normal", "tangent"): + normal = ( + _validate_gui_vector(raw) + if field == "normal" + else self.document.taxel_normals[index] + ) + tangent = ( + _validate_gui_vector(raw) + if field == "tangent" + else self.document.taxel_tangents[index] + ) + from .model import orthonormalize_frame + + normal, tangent = orthonormalize_frame(normal, tangent) + self.document.taxel_normals[index] = normal + self.document.taxel_tangents[index] = tangent + elif field == "indices": + value = _parse_numbers(str(raw), 4, int) + if np.any(value < 0) or np.any(value >= self.document.num_joints): + raise ValueError("joint index is outside the active shape") + self.document.taxel_joint_indices[index] = value.astype(np.uint32) + elif field == "weights": + value = _parse_numbers(str(raw), 4) + if np.any(value < 0) or not np.isclose(value.sum(), 1.0, atol=1e-4): + raise ValueError("weights must be nonnegative and sum to one") + self.document.taxel_joint_weights[index] = value.astype(np.float32) + else: + raise ValueError(f"{field} is not a taxel field") + + def _select(self, kind: str | None, indices: set[int]) -> None: + self.selected_kind = kind if indices else None + self.selected_indices = indices + self._refresh_property_values() + self._update_selection_gizmo() + self._update_rendered_values(update_gizmo=False) + + def _refresh_property_values(self) -> None: + kind = self.selected_kind + count = len(self.selected_indices) + taxel_single = kind == "taxel" and count == 1 + joint_single = kind == "joint" and count == 1 + all_fields = ( + self.position_input, + self.normal_input, + self.tangent_input, + self.orientation_text, + self.parent_text, + self.indices_text, + self.weights_text, + self.force_text, + self.current_position_text, + self.current_orientation_text, + ) + for handle in all_fields: + handle.visible = False + if not (taxel_single or joint_single): + self.selection_summary.content = ( + "No selection" if count == 0 else f"**{count} {kind}(s)**" + ) + self.selection_placeholder.content = "" if count == 0 else "`-`" + return + self.selection_placeholder.content = "" + index = next(iter(self.selected_indices)) + self.position_input.visible = True + if taxel_single: + self.selection_summary.content = "**1 taxel(s)**" + for handle in ( + self.normal_input, + self.tangent_input, + self.indices_text, + self.weights_text, + self.force_text, + ): + handle.visible = True + self._set_value( + self.position_input, + tuple((self.document.taxel_positions[index] * 100.0).tolist()), + ) + self._set_value( + self.normal_input, tuple(self.document.taxel_normals[index].tolist()) + ) + self._set_value( + self.tangent_input, + tuple(self.document.taxel_tangents[index].tolist()), + ) + self._set_value( + self.indices_text, + ", ".join(map(str, self.document.taxel_joint_indices[index])), + ) + self._set_value( + self.weights_text, + self._format_vector(self.document.taxel_joint_weights[index]), + ) + forces = self.display_forces.get(self.side) + force = ( + "unavailable" + if forces is None or index >= len(forces) + else f"{np.linalg.norm(forces[index]):.2f} N" + ) + self._set_value(self.force_text, force) + return + name = ( + OPENXR_JOINT_NAMES[index] if index < len(OPENXR_JOINT_NAMES) else str(index) + ) + self.selection_summary.content = f"**1 joint(s): {name}**" + for handle in ( + self.orientation_text, + self.parent_text, + self.current_position_text, + self.current_orientation_text, + ): + handle.visible = True + self._set_value( + self.position_input, + tuple((self.document.joint_bind_positions[index] * 100.0).tolist()), + ) + self._set_value( + self.orientation_text, + self._format_vector(self.document.joint_bind_orientations[index]), + ) + parent = int(self.document.joint_parents[index]) + self._set_value( + self.parent_text, + "ROOT_PARENT" if parent == int(ROOT_PARENT) else str(parent), + ) + positions, orientations = self._display_joint_pose() + self._set_value( + self.current_position_text, + self._format_vector(positions[index] * 100.0), + ) + self._set_value( + self.current_orientation_text, + self._format_vector(orientations[index]), + ) + + def _remove_selection_visuals(self) -> None: + if self._gizmo is not None: + self._gizmo.remove() + self._gizmo = None + if self._selection_axes is not None: + self._selection_axes.remove() + self._selection_axes = None + self._gizmo_context = None + + def _update_selection_gizmo(self) -> None: + self._remove_selection_visuals() + if not self.selected_indices: + return + indices = sorted(self.selected_indices) + if self.selected_kind == "joint": + if len(indices) == 1: + positions, orientations = self._display_joint_pose() + index = indices[0] + self._selection_axes = self.server.scene.add_frame( + "/selection/joint_axes", + show_axes=True, + axes_length=0.015, + axes_radius=0.0005, + position=positions[index], + wxyz=orientations[index][[3, 0, 1, 2]], + ) + return + if self.selected_kind != "taxel": + return + center = np.mean(self._display_taxel_positions[indices], axis=0) + single = len(indices) == 1 + orientation = ( + frame_wxyz( + self._display_taxel_normals[indices[0]], + self._display_taxel_tangents[indices[0]], + ) + if single + else np.array([1.0, 0.0, 0.0, 0.0]) + ) + if not self.controller.editing_enabled: + if single: + self._selection_axes = self.server.scene.add_frame( + "/selection/taxel_axes", + show_axes=True, + axes_length=0.015, + axes_radius=0.0005, + position=center, + wxyz=orientation, + ) + return + translating = self.transform_mode == "Translate" + self._gizmo = self.server.scene.add_transform_controls( + "/selection/taxel_gizmo", + scale=0.035, + position=center, + wxyz=orientation, + disable_axes=not translating, + disable_sliders=not translating, + disable_rotations=translating, + ) + self._gizmo_context = { + "indices": indices, + "origin_position": center.copy(), + "origin_rotation": quaternion_xyzw_to_matrix(orientation[[1, 2, 3, 0]]), + "positions": self._display_taxel_positions[indices].copy(), + "normals": self._display_taxel_normals[indices].copy(), + "tangents": self._display_taxel_tangents[indices].copy(), + "current_positions": self._current_positions.copy(), + "current_orientations": self._current_orientations.copy(), + } + + @self._gizmo.on_update + def _(event) -> None: + if not self._claim_editor(event.client_id, event.client): + return + if not self.controller.editing_enabled or self._gizmo_context is None: + return + context = self._gizmo_context + backup = self.document.copy() + try: + if self.transform_mode == "Translate": + delta = ( + np.asarray(event.target.position) - context["origin_position"] + ) + for slot, index in enumerate(context["indices"]): + set_taxel_display_position( + self.document, + context["current_positions"], + context["current_orientations"], + index, + context["positions"][slot] + delta, + self.side, + ) + else: + current_q = np.asarray(event.target.wxyz)[[1, 2, 3, 0]] + rotation = ( + quaternion_xyzw_to_matrix(current_q) + @ context["origin_rotation"].T + ) + for slot, index in enumerate(context["indices"]): + set_taxel_display_frame( + self.document, + context["current_positions"], + context["current_orientations"], + index, + rotation @ context["normals"][slot], + rotation @ context["tangents"][slot], + self.side, + ) + self.document.validate() + self._mark_local_edit() + except Exception as error: + self.document = backup + self._report_error(f"Gizmo edit rejected: {error}", event.client) + self._sync_tree_values() + self._update_rendered_values(update_gizmo=False) + + def _mark_local_edit(self) -> None: + self.user_shape_override = True + self.file_status.content = "Active shape has unexported local edits." + + def _refresh_editability(self) -> None: + enabled = self.controller.editing_enabled + for handle in ( + self.position_input, + self.normal_input, + self.tangent_input, + self.orientation_text, + self.parent_text, + self.indices_text, + self.weights_text, + self.upload, + self.create_new, + self.add_taxel_count, + self.add_taxels_button, + ): + handle.disabled = not enabled + self.force_text.disabled = True + self.current_position_text.disabled = True + self.current_orientation_text.disabled = True + for fields in self._tree_joint_inputs + self._tree_taxel_inputs: + for handle in fields.values(): + handle.disabled = not enabled + self._update_selection_gizmo() + self._update_state_text() + + def _update_state_text(self) -> None: + source = "offline editor" if self.source is None else "latest-only source" + override = "local override" if self.user_shape_override else "producer metadata" + error = f" \n**Error:** {self._last_error}" if self._last_error else "" + self.state_markdown.content = ( + f"**{self.controller.state.value}** · {source} · {self.side} hand · " + f"{override}{error}" + ) + self.inspector_status.content = ( + f"{self.document.num_joints} joints · {self.document.num_taxels} taxels" + ) + + def _active_hand(self): + if self.display_snapshot is None: + return None + return self.display_snapshot.hands.get(self.side) + + def _display_joint_pose(self) -> tuple[np.ndarray, np.ndarray]: + positions = self._current_positions + orientations = self._current_orientations + if self.side == "left": + positions, orientations = mirror_pose(positions, orientations) + return np.asarray(positions, dtype=np.float32), np.asarray( + orientations, dtype=np.float32 + ) + + def _update_rendered_values(self, *, update_gizmo: bool = True) -> None: + hand = self._active_hand() + self._current_positions, self._current_orientations = resolve_current_joints( + self.document, + None if hand is None else hand.positions, + None if hand is None else hand.orientations, + None if hand is None else hand.valid, + ) + positions, normals, tangents = skin_taxels( + self.document, self._current_positions, self._current_orientations + ) + positions, normals, tangents = apply_display_side( + positions, normals, tangents, self.side + ) + assert normals is not None and tangents is not None + self._display_taxel_positions = positions + self._display_taxel_normals = normals + self._display_taxel_tangents = tangents + joint_positions, _ = self._display_joint_pose() + for index, handle in enumerate(self._joint_handles): + handle.position = joint_positions[index] + handle.color = ( + (255, 40, 220) + if self.selected_kind == "joint" and index in self.selected_indices + else (255, 180, 60) + ) + for child, handle in self._bone_handles.items(): + parent = int(self.document.joint_parents[child]) + delta = joint_positions[child] - joint_positions[parent] + if float(np.linalg.norm(delta)) >= 1e-8: + handle.position = ( + joint_positions[child] + joint_positions[parent] + ) / 2.0 + handle.wxyz = self._segment_wxyz(delta) + colors = force_colors( + self.display_forces.get(self.side), self.document.num_taxels + ) + for index, handle in enumerate(self._taxel_handles): + handle.position = positions[index] + handle.wxyz = frame_wxyz(normals[index], tangents[index]) + handle.color = ( + (255, 30, 220) + if self.selected_kind == "taxel" and index in self.selected_indices + else tuple(int(value) for value in colors[index]) + ) + self._update_data_tree() + raw = ( + None + if self.display_snapshot is None + else self.display_snapshot.raw_joints.get(self.side) + ) + if self.urdf is not None: + try: + self.urdf.update_cfg( + map_raw_joints( + self.urdf_joint_names, raw, self.urdf_config.joint_map + ) + ) + except Exception as error: + logger.exception("URDF animation update failed") + self._last_error = f"URDF update failed: {error}" + if update_gizmo and self.selected_indices: + self._update_selection_gizmo() + self._refresh_property_values() + + def _update_data_tree(self) -> None: + forces = self.display_forces.get(self.side) + if forces is None: + self._data_tree_text.content = "`taxel_force` (N): no sample" + return + if len(forces) != self.document.num_taxels: + self._data_tree_text.content = ( + f"**Rejected layout:** `taxel_force` has {len(forces)} entries; " + f"TactileShape has {self.document.num_taxels} taxels." + ) + return + lines = [f"`taxel_force` (N) [{len(forces)}]"] + for index, force in enumerate(forces): + lines.append( + f"- `[{index}]`: [{self._format_vector(force)}] · " + f"|F| `{np.linalg.norm(force):.2f} N`" + ) + self._data_tree_text.content = "\n".join(lines) + + def _source_ready(self, snapshot: SourceSnapshot) -> bool: + shape = self.latest_shapes.get(self.side) + forces = self.latest_forces.get(self.side) + hand = snapshot.hands.get(self.side) + return ( + hand is not None + and len(hand.valid) > WRIST + and bool(hand.valid[WRIST]) + and self.side in snapshot.raw_joints + and shape is not None + and forces is not None + and len(forces) == shape.num_taxels + ) + + @staticmethod + def _validate_snapshot(snapshot: SourceSnapshot) -> None: + for side, hand in snapshot.hands.items(): + positions = np.asarray(hand.positions) + orientations = np.asarray(hand.orientations) + valid = np.asarray(hand.valid) + if ( + positions.ndim != 2 + or positions.shape[1:] != (3,) + or orientations.shape != (len(positions), 4) + or valid.shape != (len(positions),) + or not np.isfinite(positions).all() + or not np.isfinite(orientations).all() + ): + raise ValueError(f"{side} hand pose has an invalid layout or value") + norms = np.linalg.norm(orientations, axis=1) + if np.any(np.asarray(valid, dtype=bool) & (norms < 1e-8)): + raise ValueError(f"{side} hand pose has a zero valid quaternion") + for side, values in snapshot.raw_joints.items(): + if not all(np.isfinite(value) for value in values.values()): + raise ValueError(f"{side} raw-joint sample contains a non-finite value") + for shape in snapshot.shapes.values(): + shape.validate() + for side, forces in snapshot.forces.items(): + value = np.asarray(forces) + if ( + value.ndim != 2 + or value.shape[1:] != (3,) + or not np.isfinite(value).all() + ): + raise ValueError(f"{side} tactile force must be a finite (N, 3) array") + + def _consume_source(self, now: float) -> None: + if self.source is None: + return + sequence, snapshot, error = self.source.mailbox.latest_after(self.sequence) + self.sequence = sequence + if error is not None: + self._last_error = f"source message rejected: {error}" + if snapshot is None: + return + try: + self._validate_snapshot(snapshot) + except Exception as invalid: + self._last_error = f"malformed live message rejected: {invalid}" + return + if self._last_error.startswith(("source message rejected:", "malformed live")): + self._last_error = "" + self.latest_snapshot = snapshot + self.latest_shapes.update(snapshot.shapes) + self.latest_forces.update(snapshot.forces) + previous = self.controller.state + self.controller.message_received( + snapshot.received_at, ready=self._source_ready(snapshot) + ) + if not self.controller.paused: + self.display_snapshot = snapshot + self.display_forces = { + side: value.copy() for side, value in self.latest_forces.items() + } + if ( + not self.user_shape_override + and self.side in snapshot.shapes + and self.side not in self._loaded_remote_sides + ): + self._replace_document(snapshot.shapes[self.side].copy()) + self._loaded_remote_sides.add(self.side) + else: + self._update_rendered_values() + if previous != self.controller.state: + self._refresh_editability() + + def run(self, update_hz: float = 30.0) -> None: + if self.source is not None: + self.source.start() + period = 1.0 / update_hz + try: + while True: + started = time.monotonic() + try: + self._consume_source(started) + previous = self.controller.state + self.controller.tick(started) + if previous != self.controller.state: + self._refresh_editability() + else: + self._update_state_text() + except Exception as error: + logger.exception("Visualizer update rejected") + self._last_error = f"Update rejected: {error}" + self._update_state_text() + time.sleep(max(0.0, period - (time.monotonic() - started))) + except KeyboardInterrupt: + pass + finally: + if self.source is not None: + self.source.stop() diff --git a/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/cli.py b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/cli.py new file mode 100644 index 0000000000..88ea25b531 --- /dev/null +++ b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/cli.py @@ -0,0 +1,167 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Command-line entry point for the tactile shape editor and visualizer.""" + +from __future__ import annotations + +import argparse +import math +import sys +from pathlib import Path + +import viser + +from isaaccapture.cloudxr import CloudXRLauncher + +from .app import TactileVisualizer +from .model import ShapeDocument, default_openxr_shape +from .sources import MockSource, SessionSource, SessionSourceConfig +from .urdf_config import load_urdf_config + + +def _default_plugin_paths() -> tuple[Path, ...]: + tree = Path(__file__).resolve().parents[5] + return tuple( + path + for path in (tree / "plugins", tree / "install" / "plugins") + if (path / "sharpa_avatar" / "avatar_hand_plugin").is_file() + ) + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--urdf", type=Path, required=True, help="Glove URDF") + parser.add_argument( + "--urdf-config", + type=Path, + help="YAML/JSON with scale, offset_cm, rotation_xyzw, and joint_map", + ) + parser.add_argument("--shape", type=Path, help="TactileShape FlatBuffer to load") + parser.add_argument("--host", default="127.0.0.1") + parser.add_argument("--port", type=int, default=8080) + parser.add_argument("--side", choices=("left", "right"), default="right") + parser.add_argument( + "--taxels", type=int, default=200, help="Taxel count for --mock (default: 200)" + ) + parser.add_argument("--taxel-size-mm", type=float, default=4.0) + parser.add_argument("--liveness-timeout", type=float, default=1.0) + parser.add_argument( + "--allow-server-write-root", + type=Path, + help="Enable atomic local exports only below this directory", + ) + mode = parser.add_mutually_exclusive_group() + mode.add_argument("--live", action="store_true", help="Use live DeviceIO streams") + mode.add_argument("--mock", action="store_true", help="Use fist/palm mock data") + mode.add_argument("--replay", type=Path, help="Replay an MCAP recording") + parser.add_argument("--left-tactile-collection", default="tactile_left") + parser.add_argument("--right-tactile-collection", default="tactile_right") + parser.add_argument("--left-tactile-channel", default="tactile_left") + parser.add_argument("--right-tactile-channel", default="tactile_right") + parser.add_argument("--launch-plugin", action="store_true") + parser.add_argument("--plugin-search-path", type=Path, action="append") + parser.add_argument("--sdk-config", type=Path) + parser.add_argument("--datasets", default="human,raw,robot,haptic") + CloudXRLauncher.add_launcher_arguments(parser) + return parser + + +def _validate_args(parser: argparse.ArgumentParser, args: argparse.Namespace) -> None: + for path, label in ( + (args.urdf, "--urdf"), + (args.shape, "--shape"), + (args.replay, "--replay"), + ): + if path is not None and not path.is_file(): + parser.error(f"{label} does not exist: {path}") + if args.taxels < 0: + parser.error("--taxels must be nonnegative") + for value, label in ( + (args.taxel_size_mm, "--taxel-size-mm"), + (args.liveness_timeout, "--liveness-timeout"), + ): + if not math.isfinite(value) or value <= 0: + parser.error(f"{label} must be finite and positive") + if args.launch_plugin and not args.live: + parser.error("--launch-plugin requires --live") + paths = args.plugin_search_path or _default_plugin_paths() + if args.launch_plugin and not any( + (path / "sharpa_avatar" / "avatar_hand_plugin").is_file() for path in paths + ): + parser.error( + "avatar_hand_plugin not found; build/install the Sharpa plugin or pass " + "--plugin-search-path" + ) + + +def _make_source(args: argparse.Namespace): + if args.mock: + return MockSource(args.taxels) + if not args.live and args.replay is None: + return None + config = SessionSourceConfig( + tactile_collections={ + "left": args.left_tactile_collection, + "right": args.right_tactile_collection, + }, + tactile_channels={ + "left": args.left_tactile_channel, + "right": args.right_tactile_channel, + }, + launch_plugin=args.launch_plugin, + plugin_search_paths=tuple(args.plugin_search_path or _default_plugin_paths()), + sdk_config=args.sdk_config, + datasets=args.datasets, + replay_path=args.replay, + ) + return SessionSource(config, args) + + +def main(argv: list[str] | None = None) -> int: + parser = build_parser() + args = parser.parse_args(argv) + _validate_args(parser, args) + urdf_config = load_urdf_config(args.urdf_config) + shape_error = "" + if args.shape is None: + document = default_openxr_shape() + else: + try: + document = ShapeDocument.from_bytes(args.shape.read_bytes()) + except Exception as error: + document = default_openxr_shape() + shape_error = ( + f"Initial shape `{args.shape}` was rejected: {error}. " + "Started with an empty default shape." + ) + if args.allow_server_write_root is not None: + args.allow_server_write_root.mkdir(parents=True, exist_ok=True) + server = viser.ViserServer(host=args.host, port=args.port, label="TactileShape") + app = TactileVisualizer( + server=server, + urdf_path=args.urdf, + urdf_config_path=args.urdf_config, + urdf_config=urdf_config, + document=document, + document_name=( + str(args.shape) + if args.shape is not None and not shape_error + else "untitled" + ), + source=_make_source(args), + side=args.side, + liveness_timeout_s=args.liveness_timeout, + taxel_size_m=args.taxel_size_mm / 1000.0, + write_root=args.allow_server_write_root, + ) + if shape_error: + app._report_error(shape_error) + print(f"Tactile visualizer: http://{args.host}:{args.port}") + print("Ctrl+C stops the local server.") + app.run() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/model.py b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/model.py new file mode 100644 index 0000000000..2bbc70acf0 --- /dev/null +++ b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/model.py @@ -0,0 +1,687 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Pure data, geometry, and state-machine logic for the tactile visualizer.""" + +from __future__ import annotations + +import math +import os +import tempfile +import threading +from dataclasses import dataclass +from enum import Enum +from pathlib import Path +from typing import Generic, TypeVar + +import numpy as np + +OPENXR_JOINT_NAMES = ( + "PALM", + "WRIST", + "THUMB_METACARPAL", + "THUMB_PROXIMAL", + "THUMB_DISTAL", + "THUMB_TIP", + "INDEX_METACARPAL", + "INDEX_PROXIMAL", + "INDEX_INTERMEDIATE", + "INDEX_DISTAL", + "INDEX_TIP", + "MIDDLE_METACARPAL", + "MIDDLE_PROXIMAL", + "MIDDLE_INTERMEDIATE", + "MIDDLE_DISTAL", + "MIDDLE_TIP", + "RING_METACARPAL", + "RING_PROXIMAL", + "RING_INTERMEDIATE", + "RING_DISTAL", + "RING_TIP", + "LITTLE_METACARPAL", + "LITTLE_PROXIMAL", + "LITTLE_INTERMEDIATE", + "LITTLE_DISTAL", + "LITTLE_TIP", +) +WRIST = 1 +ROOT_PARENT = np.uint32(0x7FFFFFFF) +MIRROR_X = np.diag([-1.0, 1.0, 1.0]) + + +def normalize(vector: np.ndarray, *, label: str = "vector") -> np.ndarray: + value = np.asarray(vector, dtype=np.float64) + length = float(np.linalg.norm(value)) + if not math.isfinite(length) or length < 1e-8: + raise ValueError(f"{label} must have a finite, nonzero length") + return value / length + + +def orthonormalize_frame( + normal: np.ndarray, tangent: np.ndarray +) -> tuple[np.ndarray, np.ndarray]: + """Return a unit normal and a perpendicular unit tangent.""" + n = normalize(normal, label="normal") + t = np.asarray(tangent, dtype=np.float64) - n * float(np.dot(n, tangent)) + if np.linalg.norm(t) < 1e-8: + basis = np.zeros(3) + basis[int(np.argmin(np.abs(n)))] = 1.0 + t = basis - n * float(np.dot(n, basis)) + return n.astype(np.float32), normalize(t, label="tangent").astype(np.float32) + + +def quaternion_xyzw_to_matrix(quaternion: np.ndarray) -> np.ndarray: + x, y, z, w = normalize(quaternion, label="quaternion") + return np.array( + [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ], + dtype=np.float64, + ) + + +def matrix_to_quaternion_xyzw(matrix: np.ndarray) -> np.ndarray: + """Convert a proper rotation matrix to a normalized XYZW quaternion.""" + m = np.asarray(matrix, dtype=np.float64) + trace = float(np.trace(m)) + if trace > 0: + s = math.sqrt(trace + 1.0) * 2 + q = np.array( + [ + (m[2, 1] - m[1, 2]) / s, + (m[0, 2] - m[2, 0]) / s, + (m[1, 0] - m[0, 1]) / s, + 0.25 * s, + ] + ) + else: + axis = int(np.argmax(np.diag(m))) + if axis == 0: + s = math.sqrt(max(0.0, 1 + m[0, 0] - m[1, 1] - m[2, 2])) * 2 + q = np.array( + [ + 0.25 * s, + (m[0, 1] + m[1, 0]) / s, + (m[0, 2] + m[2, 0]) / s, + (m[2, 1] - m[1, 2]) / s, + ] + ) + elif axis == 1: + s = math.sqrt(max(0.0, 1 + m[1, 1] - m[0, 0] - m[2, 2])) * 2 + q = np.array( + [ + (m[0, 1] + m[1, 0]) / s, + 0.25 * s, + (m[1, 2] + m[2, 1]) / s, + (m[0, 2] - m[2, 0]) / s, + ] + ) + else: + s = math.sqrt(max(0.0, 1 + m[2, 2] - m[0, 0] - m[1, 1])) * 2 + q = np.array( + [ + (m[0, 2] + m[2, 0]) / s, + (m[1, 2] + m[2, 1]) / s, + 0.25 * s, + (m[1, 0] - m[0, 1]) / s, + ] + ) + return normalize(q, label="rotation quaternion").astype(np.float32) + + +def frame_matrix(normal: np.ndarray, tangent: np.ndarray) -> np.ndarray: + n, t = orthonormalize_frame(normal, tangent) + return np.column_stack((t, np.cross(n, t), n)).astype(np.float64) + + +def frame_wxyz(normal: np.ndarray, tangent: np.ndarray) -> np.ndarray: + q = matrix_to_quaternion_xyzw(frame_matrix(normal, tangent)) + return q[[3, 0, 1, 2]] + + +def mirror_pose( + positions: np.ndarray, orientations: np.ndarray +) -> tuple[np.ndarray, np.ndarray]: + pos = np.asarray(positions, dtype=np.float64) @ MIRROR_X.T + rotations = np.stack( + [ + MIRROR_X @ quaternion_xyzw_to_matrix(q) @ MIRROR_X + for q in np.asarray(orientations) + ] + ) + quat = np.stack([matrix_to_quaternion_xyzw(r) for r in rotations]) + return pos.astype(np.float32), quat.astype(np.float32) + + +@dataclass +class ShapeDocument: + """Owned, mutable arrays corresponding exactly to ``TactileShape``.""" + + joint_bind_positions: np.ndarray + joint_bind_orientations: np.ndarray + joint_parents: np.ndarray + taxel_positions: np.ndarray + taxel_normals: np.ndarray + taxel_tangents: np.ndarray + taxel_joint_indices: np.ndarray + taxel_joint_weights: np.ndarray + + @property + def num_joints(self) -> int: + return len(self.joint_parents) + + @property + def num_taxels(self) -> int: + return len(self.taxel_positions) + + def copy(self) -> ShapeDocument: + return ShapeDocument( + **{name: np.asarray(value).copy() for name, value in self.arrays().items()} + ) + + def arrays(self) -> dict[str, np.ndarray]: + return { + "joint_bind_positions": self.joint_bind_positions, + "joint_bind_orientations": self.joint_bind_orientations, + "joint_parents": self.joint_parents, + "taxel_positions": self.taxel_positions, + "taxel_normals": self.taxel_normals, + "taxel_tangents": self.taxel_tangents, + "taxel_joint_indices": self.taxel_joint_indices, + "taxel_joint_weights": self.taxel_joint_weights, + } + + @classmethod + def from_schema(cls, shape) -> ShapeDocument: + document = cls( + **{name: np.asarray(getattr(shape, name)).copy() for name in cls.fields()} + ) + document.validate() + return document + + @classmethod + def from_bytes(cls, payload: bytes) -> ShapeDocument: + from isaaccapture.schema import TactileShape + + return cls.from_schema(TactileShape.from_bytes(payload)) + + @staticmethod + def fields() -> tuple[str, ...]: + return ( + "joint_bind_positions", + "joint_bind_orientations", + "joint_parents", + "taxel_positions", + "taxel_normals", + "taxel_tangents", + "taxel_joint_indices", + "taxel_joint_weights", + ) + + def to_schema(self): + from isaaccapture.schema import TactileShape + + self.validate() + return TactileShape(**self.arrays()) + + def to_bytes(self) -> bytes: + return self.to_schema().to_bytes() + + def validate(self) -> None: + arrays = self.arrays() + expected = { + "joint_bind_positions": (self.num_joints, 3), + "joint_bind_orientations": (self.num_joints, 4), + "joint_parents": (self.num_joints,), + "taxel_positions": (self.num_taxels, 3), + "taxel_normals": (self.num_taxels, 3), + "taxel_tangents": (self.num_taxels, 3), + "taxel_joint_indices": (self.num_taxels, 4), + "taxel_joint_weights": (self.num_taxels, 4), + } + if not 1 <= self.num_joints <= len(OPENXR_JOINT_NAMES): + raise ValueError("joint count must be in OpenXR range [1, 26]") + for name, shape in expected.items(): + value = np.asarray(arrays[name]) + if value.shape != shape: + raise ValueError(f"{name} must have shape {shape}, got {value.shape}") + if value.dtype.kind == "f" and not np.isfinite(value).all(): + raise ValueError(f"{name} contains non-finite values") + + parents = np.asarray(self.joint_parents, dtype=np.uint32) + for index, parent in enumerate(parents): + if parent != ROOT_PARENT and int(parent) >= self.num_joints: + raise ValueError(f"joint {index} has invalid parent {parent}") + seen: set[int] = set() + cursor = index + while parents[cursor] != ROOT_PARENT: + if cursor in seen: + raise ValueError("joint hierarchy contains a cycle") + seen.add(cursor) + cursor = int(parents[cursor]) + if not np.any(parents == ROOT_PARENT): + raise ValueError("joint hierarchy has no root") + + q_norm = np.linalg.norm(self.joint_bind_orientations, axis=1) + if not np.allclose(q_norm, 1.0, atol=1e-4): + raise ValueError("joint bind orientations must be unit quaternions") + if self.num_taxels == 0: + return + indices = np.asarray(self.taxel_joint_indices) + if np.any(indices >= self.num_joints): + raise ValueError("taxel joint index is outside the joint array") + weights = np.asarray(self.taxel_joint_weights) + if np.any(weights < 0) or not np.allclose(weights.sum(axis=1), 1.0, atol=1e-4): + raise ValueError("taxel weights must be nonnegative and sum to one") + if np.any(np.count_nonzero(weights > 0, axis=1) == 0): + raise ValueError("every taxel must have at least one nonzero weight") + n_norm = np.linalg.norm(self.taxel_normals, axis=1) + t_norm = np.linalg.norm(self.taxel_tangents, axis=1) + dot = np.sum(self.taxel_normals * self.taxel_tangents, axis=1) + if not np.allclose(n_norm, 1.0, atol=1e-4): + raise ValueError("taxel normals must be unit length") + if not np.allclose(t_norm, 1.0, atol=1e-4): + raise ValueError("taxel tangents must be unit length") + if not np.allclose(dot, 0.0, atol=1e-4): + raise ValueError("taxel normals and tangents must be perpendicular") + + +def default_openxr_shape(num_taxels: int = 0) -> ShapeDocument: + """Build a tool-defined neutral right-hand pose in OpenXR ordinal order. + + OpenXR standardizes joint names and runtime poses, but not a universal bind + pose. These approximate metric locations are therefore an explicit editor + default, not a hardware calibration. + """ + if num_taxels < 0: + raise ValueError("num_taxels must be nonnegative") + positions = np.zeros((26, 3), dtype=np.float32) + positions[0] = (0.0, 0.042, 0.0) + positions[1] = (0.0, 0.0, 0.0) + positions[2:6] = ( + (-0.026, 0.026, 0.0), + (-0.041, 0.041, 0.0), + (-0.052, 0.054, 0.0), + (-0.060, 0.066, 0.0), + ) + finger_specs = ( + (6, -0.027, (0.042, 0.070, 0.096, 0.116, 0.133)), + (11, -0.009, (0.044, 0.076, 0.105, 0.128, 0.147)), + (16, 0.010, (0.043, 0.073, 0.100, 0.121, 0.139)), + (21, 0.029, (0.039, 0.064, 0.087, 0.105, 0.120)), + ) + for start, x, ys in finger_specs: + positions[start : start + 5] = [(x, y, 0.0) for y in ys] + + parents = np.full(26, ROOT_PARENT, dtype=np.uint32) + parents[0] = WRIST + for start, length in ((2, 4), (6, 5), (11, 5), (16, 5), (21, 5)): + parents[start] = WRIST + parents[start + 1 : start + length] = np.arange( + start, start + length - 1, dtype=np.uint32 + ) + + attachments = np.array( + [0, 4, 5, 8, 9, 10, 13, 14, 15, 18, 19, 20, 23, 24, 25], dtype=np.uint32 + ) + taxel_positions = np.empty((num_taxels, 3), dtype=np.float32) + indices = np.empty((num_taxels, 4), dtype=np.uint32) + weights = np.zeros((num_taxels, 4), dtype=np.float32) + for taxel in range(num_taxels): + joint = int(attachments[taxel % len(attachments)]) + ring = taxel // len(attachments) + dx = ((ring % 5) - 2) * 0.0025 + dy = ((ring // 5) % 5 - 2) * 0.0025 + taxel_positions[taxel] = positions[joint] + (dx, dy, 0.002) + indices[taxel] = joint + weights[taxel, 0] = 1.0 + + document = ShapeDocument( + joint_bind_positions=positions, + joint_bind_orientations=np.tile([0.0, 0.0, 0.0, 1.0], (26, 1)).astype( + np.float32 + ), + joint_parents=parents, + taxel_positions=taxel_positions, + taxel_normals=np.tile([0.0, 0.0, 1.0], (num_taxels, 1)).astype(np.float32), + taxel_tangents=np.tile([1.0, 0.0, 0.0], (num_taxels, 1)).astype(np.float32), + taxel_joint_indices=indices, + taxel_joint_weights=weights, + ) + document.validate() + return document + + +def append_grid_taxels(document: ShapeDocument, amount: int) -> None: + """Append the first N points from the documented x/y/z centimeter grid.""" + if not 0 <= amount <= 125: + raise ValueError("taxel amount must be between 0 and 125") + if amount == 0: + return + grid_cm = (-4.0, -2.0, 0.0, 2.0, 4.0) + candidates = np.asarray( + [(x, y, z) for x in grid_cm for y in grid_cm for z in grid_cm], + dtype=np.float32, + ) + positions = candidates[:amount] / 100.0 + normals = np.tile([0.0, 0.0, 1.0], (amount, 1)).astype(np.float32) + tangents = np.tile([1.0, 0.0, 0.0], (amount, 1)).astype(np.float32) + # TactileTaxel has no unbound sentinel: all four indices must be valid and + # 1-4 weights must be nonzero. Bind to wrist with one unit weight. + attachment = WRIST if document.num_joints > WRIST else 0 + indices = np.full((amount, 4), attachment, dtype=np.uint32) + weights = np.zeros((amount, 4), dtype=np.float32) + weights[:, 0] = 1.0 + document.taxel_positions = np.concatenate( + (document.taxel_positions, positions), axis=0 + ) + document.taxel_normals = np.concatenate((document.taxel_normals, normals), axis=0) + document.taxel_tangents = np.concatenate( + (document.taxel_tangents, tangents), axis=0 + ) + document.taxel_joint_indices = np.concatenate( + (document.taxel_joint_indices, indices), axis=0 + ) + document.taxel_joint_weights = np.concatenate( + (document.taxel_joint_weights, weights), axis=0 + ) + document.validate() + + +def normalize_hand_to_wrist( + positions: np.ndarray, + orientations: np.ndarray, + valid: np.ndarray, +) -> tuple[np.ndarray, np.ndarray]: + pos = np.asarray(positions, dtype=np.float64).copy() + quat = np.asarray(orientations, dtype=np.float64).copy() + mask = np.asarray(valid, dtype=bool) + if len(pos) <= WRIST or not mask[WRIST]: + return pos.astype(np.float32), quat.astype(np.float32) + wrist_rotation = quaternion_xyzw_to_matrix(quat[WRIST]) + inverse = wrist_rotation.T + pos = (pos - pos[WRIST]) @ inverse.T + for i, q in enumerate(quat): + if mask[i]: + quat[i] = matrix_to_quaternion_xyzw(inverse @ quaternion_xyzw_to_matrix(q)) + return pos.astype(np.float32), quat.astype(np.float32) + + +def resolve_current_joints( + document: ShapeDocument, + positions: np.ndarray | None, + orientations: np.ndarray | None, + valid: np.ndarray | None, +) -> tuple[np.ndarray, np.ndarray]: + bind_pos = document.joint_bind_positions.astype(np.float64) + bind_quat = document.joint_bind_orientations.astype(np.float64) + if positions is None or orientations is None or valid is None: + return bind_pos, bind_quat + if len(valid) <= WRIST or not bool(valid[WRIST]): + return bind_pos, bind_quat + count = min(document.num_joints, len(positions), len(orientations), len(valid)) + current_pos = bind_pos.copy() + current_quat = bind_quat.copy() + normalized_pos, normalized_quat = normalize_hand_to_wrist( + positions[:count], orientations[:count], valid[:count] + ) + mask = np.asarray(valid[:count], dtype=bool) + valid_indices = np.flatnonzero(mask) + current_pos[valid_indices] = normalized_pos[valid_indices] + current_quat[valid_indices] = normalized_quat[valid_indices] + return current_pos, current_quat + + +def taxel_skinning_affine( + document: ShapeDocument, + current_positions: np.ndarray, + current_orientations: np.ndarray, + taxel: int, +) -> tuple[np.ndarray, np.ndarray]: + linear = np.zeros((3, 3), dtype=np.float64) + offset = np.zeros(3, dtype=np.float64) + for joint, weight in zip( + document.taxel_joint_indices[taxel], + document.taxel_joint_weights[taxel], + ): + if weight <= 0: + continue + j = int(joint) + bind_rotation = quaternion_xyzw_to_matrix(document.joint_bind_orientations[j]) + current_rotation = quaternion_xyzw_to_matrix(current_orientations[j]) + transform = current_rotation @ bind_rotation.T + linear += float(weight) * transform + offset += float(weight) * ( + current_positions[j] - transform @ document.joint_bind_positions[j] + ) + return linear, offset + + +def skin_taxels( + document: ShapeDocument, + current_positions: np.ndarray, + current_orientations: np.ndarray, +) -> tuple[np.ndarray, np.ndarray, np.ndarray]: + positions = np.empty_like(document.taxel_positions) + normals = np.empty_like(document.taxel_normals) + tangents = np.empty_like(document.taxel_tangents) + for taxel in range(document.num_taxels): + linear, offset = taxel_skinning_affine( + document, current_positions, current_orientations, taxel + ) + positions[taxel] = linear @ document.taxel_positions[taxel] + offset + normal, tangent = orthonormalize_frame( + linear @ document.taxel_normals[taxel], + linear @ document.taxel_tangents[taxel], + ) + normals[taxel], tangents[taxel] = normal, tangent + return positions, normals, tangents + + +def apply_display_side( + positions: np.ndarray, + normals: np.ndarray | None, + tangents: np.ndarray | None, + side: str, +) -> tuple[np.ndarray, np.ndarray | None, np.ndarray | None]: + if side == "right": + return positions, normals, tangents + pos = np.asarray(positions) @ MIRROR_X.T + normal = None if normals is None else np.asarray(normals) @ MIRROR_X.T + tangent = None if tangents is None else np.asarray(tangents) @ MIRROR_X.T + return pos, normal, tangent + + +def set_taxel_display_position( + document: ShapeDocument, + current_positions: np.ndarray, + current_orientations: np.ndarray, + taxel: int, + display_position: np.ndarray, + side: str, +) -> None: + desired = np.asarray(display_position, dtype=np.float64) + if side == "left": + desired = MIRROR_X @ desired + linear, offset = taxel_skinning_affine( + document, current_positions, current_orientations, taxel + ) + document.taxel_positions[taxel] = np.linalg.pinv(linear) @ (desired - offset) + + +def set_taxel_display_frame( + document: ShapeDocument, + current_positions: np.ndarray, + current_orientations: np.ndarray, + taxel: int, + display_normal: np.ndarray, + display_tangent: np.ndarray, + side: str, +) -> None: + normal = np.asarray(display_normal, dtype=np.float64) + tangent = np.asarray(display_tangent, dtype=np.float64) + if side == "left": + normal, tangent = MIRROR_X @ normal, MIRROR_X @ tangent + linear, _ = taxel_skinning_affine( + document, current_positions, current_orientations, taxel + ) + inverse = np.linalg.pinv(linear) + normal, tangent = orthonormalize_frame(inverse @ normal, inverse @ tangent) + document.taxel_normals[taxel] = normal + document.taxel_tangents[taxel] = tangent + + +def force_colors(forces: np.ndarray | None, count: int) -> np.ndarray: + """Map 0..30 N linearly from blue to red, clamping larger forces.""" + if forces is None or len(forces) != count: + return np.tile(np.array([0, 0, 255], dtype=np.uint8), (count, 1)) + value = np.clip(np.linalg.norm(forces, axis=1) / 30.0, 0.0, 1.0) + colors = np.zeros((count, 3), dtype=np.float64) + colors[:, 0] = 255.0 * value + colors[:, 2] = 255.0 * (1.0 - value) + return np.rint(colors).astype(np.uint8) + + +def project_points( + points: np.ndarray, + camera_position: np.ndarray, + camera_wxyz: np.ndarray, + vertical_fov: float, + aspect: float, +) -> tuple[np.ndarray, np.ndarray]: + """Project world points to Viser normalized screen coordinates. + + Viser follows the OpenCV camera convention: +Z forward, +X right, +Y down. + """ + w, x, y, z = normalize(camera_wxyz, label="camera quaternion") + rotation = quaternion_xyzw_to_matrix(np.array([x, y, z, w])) + camera = (np.asarray(points) - camera_position) @ rotation + depth = camera[:, 2] + tan_half = math.tan(float(vertical_fov) / 2) + screen = np.empty((len(camera), 2), dtype=np.float64) + screen[:, 0] = 0.5 + camera[:, 0] / (2 * depth * tan_half * aspect) + screen[:, 1] = 0.5 + camera[:, 1] / (2 * depth * tan_half) + return screen, depth > 0 + + +class SessionState(str, Enum): + OFFLINE = "OFFLINE" + LIVE_PLAYING = "LIVE_PLAYING" + LIVE_PAUSED = "LIVE_PAUSED" + DISCONNECTED = "DISCONNECTED" + + +class SessionController: + def __init__(self, live_source: bool, timeout_s: float) -> None: + if not math.isfinite(timeout_s) or timeout_s <= 0: + raise ValueError("liveness timeout must be finite and positive") + self.live_source = live_source + self.timeout_s = timeout_s + self.paused = False + self.ready = False + self.last_message_time: float | None = None + self.last_sample_time: float | None = None + self.state = SessionState.OFFLINE + + def sample_received(self, now: float) -> SessionState: + self.last_message_time = now + self.last_sample_time = now + self.ready = True + self.state = ( + SessionState.LIVE_PAUSED if self.paused else SessionState.LIVE_PLAYING + ) + return self.state + + def message_received(self, now: float, *, ready: bool) -> SessionState: + self.last_message_time = now + if ready: + return self.sample_received(now) + if not self.ready: + self.state = SessionState.OFFLINE + return self.state + + def requirements_changed(self, now: float, *, ready: bool) -> SessionState: + self.ready = ready + return self.tick(now) + + def pause(self) -> SessionState: + self.paused = True + if self.state is SessionState.LIVE_PLAYING: + self.state = SessionState.LIVE_PAUSED + return self.state + + def resume(self, now: float) -> SessionState: + self.paused = False + return self.tick(now) + + def tick(self, now: float) -> SessionState: + if not self.live_source: + self.state = SessionState.OFFLINE + elif self.last_message_time is None or not self.ready: + self.state = SessionState.OFFLINE + elif now - self.last_message_time > self.timeout_s: + self.state = SessionState.DISCONNECTED + else: + self.state = ( + SessionState.LIVE_PAUSED if self.paused else SessionState.LIVE_PLAYING + ) + return self.state + + @property + def editing_enabled(self) -> bool: + return self.state is not SessionState.LIVE_PLAYING + + +T = TypeVar("T") + + +class LatestMailbox(Generic[T]): + """Thread-safe, one-slot mailbox; publishing replaces any unread sample.""" + + def __init__(self) -> None: + self._lock = threading.Lock() + self._sequence = 0 + self._value: T | None = None + self._error: BaseException | None = None + + def put(self, value: T) -> None: + with self._lock: + self._value = value + self._error = None + self._sequence += 1 + + def fail(self, error: BaseException) -> None: + with self._lock: + self._value = None + self._error = error + self._sequence += 1 + + def latest_after(self, sequence: int) -> tuple[int, T | None, BaseException | None]: + with self._lock: + if sequence == self._sequence: + return sequence, None, None + return self._sequence, self._value, self._error + + +def atomic_write_within(root: Path, destination: str, payload: bytes) -> Path: + """Atomically write below an explicitly allowed local root.""" + root = root.expanduser().resolve() + target = (root / destination).resolve() + if not target.is_relative_to(root): + raise ValueError("server destination must remain inside the allowed write root") + target.parent.mkdir(parents=True, exist_ok=True) + descriptor, temporary = tempfile.mkstemp( + prefix=f".{target.name}.", dir=target.parent + ) + try: + with os.fdopen(descriptor, "wb") as stream: + stream.write(payload) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, target) + except BaseException: + Path(temporary).unlink(missing_ok=True) + raise + return target diff --git a/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/sources.py b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/sources.py new file mode 100644 index 0000000000..125bf6d8b6 --- /dev/null +++ b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/sources.py @@ -0,0 +1,358 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Latest-only mock, live, and MCAP inputs for the tactile visualizer.""" + +from __future__ import annotations + +import logging +import math +import threading +import time +from contextlib import nullcontext +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +import numpy as np + +from .model import ( + LatestMailbox, + ShapeDocument, + default_openxr_shape, + matrix_to_quaternion_xyzw, +) + +logger = logging.getLogger("isaaccapture.examples.tactile_visualizer.sources") + +RAW_JOINT_NAMES = ( + "thumb_CMC_FE", + "thumb_CMC_AA", + "thumb_MCP_FE", + "thumb_MCP_AA", + "thumb_IP", + "index_MCP_AA", + "index_MCP_FE", + "index_PIP", + "index_DIP", + "middle_MCP_AA", + "middle_MCP_FE", + "middle_PIP", + "middle_DIP", + "ring_MCP_AA", + "ring_MCP_FE", + "ring_PIP", + "ring_DIP", + "pinky_MCP_AA", + "pinky_MCP_FE", + "pinky_DIP1", + "pinky_DIP2", + "pinky_DIP3", +) +RAW_COLLECTIONS = { + "left": "avatar_raw_left", + "right": "avatar_raw_right", +} + + +@dataclass(frozen=True) +class HandSample: + positions: np.ndarray + orientations: np.ndarray + valid: np.ndarray + + +@dataclass(frozen=True) +class SourceSnapshot: + received_at: float + hands: dict[str, HandSample] = field(default_factory=dict) + raw_joints: dict[str, dict[str, float]] = field(default_factory=dict) + shapes: dict[str, ShapeDocument] = field(default_factory=dict) + forces: dict[str, np.ndarray] = field(default_factory=dict) + + +class SnapshotSource: + def __init__(self) -> None: + self.mailbox: LatestMailbox[SourceSnapshot] = LatestMailbox() + self._stop = threading.Event() + self._thread: threading.Thread | None = None + + def start(self) -> None: + if self._thread is not None: + raise RuntimeError("source is already started") + self._thread = threading.Thread( + target=self._thread_main, + name=f"{type(self).__name__}-producer", + daemon=True, + ) + self._thread.start() + + def stop(self) -> None: + self._stop.set() + if self._thread is not None: + self._thread.join(timeout=5.0) + if self._thread.is_alive(): + logger.error("%s did not stop within five seconds", type(self).__name__) + + def _thread_main(self) -> None: + try: + self.run() + except BaseException as error: + logger.exception("%s stopped", type(self).__name__) + self.mailbox.fail(error) + + def run(self) -> None: + raise NotImplementedError + + +def _mock_hand(shape: ShapeDocument, curl: float) -> HandSample: + positions = shape.joint_bind_positions.copy() + orientations = shape.joint_bind_orientations.copy() + for start, length in ((2, 4), (6, 5), (11, 5), (16, 5), (21, 5)): + base = positions[start].copy() + previous = base + cumulative = 0.0 + for joint in range(start + 1, start + length): + segment = float( + np.linalg.norm( + shape.joint_bind_positions[joint] + - shape.joint_bind_positions[joint - 1] + ) + ) + cumulative += curl * (0.35 if start == 2 else 0.48) + direction = np.array([0.0, math.cos(cumulative), -math.sin(cumulative)]) + positions[joint] = previous + segment * direction + c, s = math.cos(cumulative), math.sin(cumulative) + orientations[joint] = matrix_to_quaternion_xyzw( + np.array([[1.0, 0.0, 0.0], [0.0, c, s], [0.0, -s, c]]) + ) + previous = positions[joint] + return HandSample( + positions=positions, + orientations=orientations, + valid=np.ones(shape.num_joints, dtype=bool), + ) + + +class MockSource(SnapshotSource): + """Alternates between open palm and fist while publishing tactile forces.""" + + def __init__(self, num_taxels: int = 200, rate_hz: float = 30.0) -> None: + super().__init__() + if rate_hz <= 0: + raise ValueError("mock rate must be positive") + self.shape = default_openxr_shape(num_taxels) + self.rate_hz = rate_hz + + def run(self) -> None: + start = time.monotonic() + frame = 0 + while not self._stop.is_set(): + now = time.monotonic() + phase = 0.5 - 0.5 * math.cos(2 * math.pi * (now - start) / 4.0) + hand = _mock_hand(self.shape, phase) + raw = { + name: phase * (1.1 if name.endswith(("FE", "IP", "PIP")) else 0.35) + for name in RAW_JOINT_NAMES + } + force = np.zeros((self.shape.num_taxels, 3), dtype=np.float32) + taxel_phase = np.arange(self.shape.num_taxels, dtype=np.float32) * 0.07 + force[:, 2] = np.maximum( + 0.0, + phase * (1.4 + 0.6 * np.sin(2 * math.pi * (now - start) + taxel_phase)), + ) + shapes = {"left": self.shape, "right": self.shape} + self.mailbox.put( + SourceSnapshot( + received_at=now, + hands={"left": hand, "right": hand}, + raw_joints={"left": raw, "right": raw}, + shapes=shapes, + forces={"left": force, "right": force}, + ) + ) + frame += 1 + self._stop.wait(max(0.0, start + frame / self.rate_hz - time.monotonic())) + + +@dataclass(frozen=True) +class SessionSourceConfig: + tactile_collections: dict[str, str] + tactile_channels: dict[str, str] + launch_plugin: bool = False + plugin_search_paths: tuple[Path, ...] = () + sdk_config: Path | None = None + datasets: str = "human,raw,robot,haptic" + replay_path: Path | None = None + replay_rate_hz: float = 60.0 + + +class SessionSource(SnapshotSource): + """Uses the same TeleopSession sources and collection names as Sharpa Avatar.""" + + def __init__(self, config: SessionSourceConfig, launcher_args: Any) -> None: + super().__init__() + self.config = config + self.launcher_args = launcher_args + self._known_shapes: dict[str, ShapeDocument] = {} + + def _plugins(self): + if not self.config.launch_plugin: + return [] + from isaaccapture.teleop_session_manager import PluginConfig + + plugin_args = [f"--datasets={self.config.datasets}"] + if self.config.sdk_config is not None: + plugin_args.insert(0, str(self.config.sdk_config)) + return [ + PluginConfig( + plugin_name="avatar_hand_plugin", + plugin_root_id="sharpa_avatar", + search_paths=list(self.config.plugin_search_paths), + plugin_args=plugin_args, + ) + ] + + def _duration_s(self) -> float | None: + if self.config.replay_path is None: + return None + from mcap.reader import make_reader + + with self.config.replay_path.open("rb") as stream: + summary = make_reader(stream).get_summary() + if summary is None or summary.statistics is None: + return None + stats = summary.statistics + return max(0.0, (stats.message_end_time - stats.message_start_time) / 1e9) + + @staticmethod + def _hand(group, index) -> HandSample | None: + if group is None or group.is_none: + return None + return HandSample( + positions=np.asarray(group[index.JOINT_POSITIONS], dtype=np.float32).copy(), + orientations=np.asarray( + group[index.JOINT_ORIENTATIONS], dtype=np.float32 + ).copy(), + valid=np.asarray(group[index.JOINT_VALID], dtype=bool).copy(), + ) + + @staticmethod + def _raw(group) -> dict[str, float] | None: + if group is None or group.is_none: + return None + return {name: float(group[i]) for i, name in enumerate(RAW_JOINT_NAMES)} + + def run(self) -> None: + from isaaccapture.cloudxr import CloudXRLauncher + from isaaccapture.deviceio_session import McapReplayConfig + from isaaccapture.deviceio_trackers import TactileTracker + from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + HandsSource, + JointStateSource, + ) + from isaaccapture.retargeting_engine.interface import OutputCombiner + from isaaccapture.retargeting_engine.tensor_types import HandInputIndex + from isaaccapture.teleop_session_manager import ( + SessionMode, + TeleopSession, + TeleopSessionConfig, + ) + + hands = HandsSource(name="hands") + raw_sources = { + side: JointStateSource( + name=RAW_COLLECTIONS[side], + collection_id=RAW_COLLECTIONS[side], + joint_names=list(RAW_JOINT_NAMES), + ) + for side in ("left", "right") + } + pipeline = OutputCombiner( + { + "hand_left": hands.output(HandsSource.LEFT), + "hand_right": hands.output(HandsSource.RIGHT), + "raw_left": raw_sources["left"].output(JointStateSource.JOINTS), + "raw_right": raw_sources["right"].output(JointStateSource.JOINTS), + } + ) + tactile = { + side: TactileTracker(self.config.tactile_collections[side]) + for side in ("left", "right") + } + replay = self.config.replay_path is not None + replay_config = None + if replay: + replay_config = McapReplayConfig( + str(self.config.replay_path), + [ + (tactile[side], self.config.tactile_channels[side]) + for side in ("left", "right") + ], + ) + session_config = TeleopSessionConfig( + app_name="TactileShapeVisualizer", + pipeline=pipeline, + mode=SessionMode.REPLAY if replay else SessionMode.LIVE, + trackers=[] if replay else list(tactile.values()), + plugins=[] if replay else self._plugins(), + mcap_config=replay_config, + verbose=False, + ) + duration = self._duration_s() + started = time.monotonic() + launcher = ( + nullcontext() + if replay + else CloudXRLauncher.launch_context(self.launcher_args) + ) + with launcher, TeleopSession(session_config) as session: + frame = 0 + while not self._stop.is_set(): + if duration is not None and time.monotonic() - started > duration: + return + try: + result = session.step() + now = time.monotonic() + hand_values: dict[str, HandSample] = {} + raw_values: dict[str, dict[str, float]] = {} + shapes: dict[str, ShapeDocument] = {} + forces: dict[str, np.ndarray] = {} + for side in ("left", "right"): + hand = self._hand(result.get(f"hand_{side}"), HandInputIndex) + raw = self._raw(result.get(f"raw_{side}")) + if hand is not None: + hand_values[side] = hand + if raw is not None: + raw_values[side] = raw + if side not in self._known_shapes: + shape = tactile[side].get_metadata(session.deviceio_session) + if shape is not None: + self._known_shapes[side] = ShapeDocument.from_schema( + shape + ) + if side in self._known_shapes: + shapes[side] = self._known_shapes[side] + data = tactile[side].get_data(session.deviceio_session) + if data is not None: + forces[side] = np.asarray( + data.taxel_force, dtype=np.float32 + ).copy() + # Cached metadata alone is not a live sample and must not + # keep the connection state alive after producers stop. + if hand_values or raw_values or forces: + self.mailbox.put( + SourceSnapshot( + received_at=now, + hands=hand_values, + raw_joints=raw_values, + shapes=shapes, + forces=forces, + ) + ) + except Exception as error: + logger.exception("Rejected malformed live message") + self.mailbox.fail(error) + frame += 1 + target = started + frame / self.config.replay_rate_hz + self._stop.wait(max(0.0, target - time.monotonic())) diff --git a/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/urdf_config.py b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/urdf_config.py new file mode 100644 index 0000000000..e6cf6c498c --- /dev/null +++ b/examples/tactile_visualizer/python/isaaccapture_examples/tactile_visualizer/urdf_config.py @@ -0,0 +1,80 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""URDF placement and Sharpa raw-joint mapping configuration.""" + +from __future__ import annotations + +import json +import math +from dataclasses import dataclass, field +from pathlib import Path + +import numpy as np + +from .model import normalize + + +@dataclass(frozen=True) +class UrdfConfig: + scale: float = 1.0 + offset_cm: tuple[float, float, float] = (0.0, 0.0, 0.0) + rotation_xyzw: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) + joint_map: dict[str, str] = field(default_factory=dict) + + def validate(self) -> None: + if not math.isfinite(self.scale) or self.scale <= 0: + raise ValueError("URDF scale must be finite and positive") + if len(self.offset_cm) != 3 or not np.isfinite(self.offset_cm).all(): + raise ValueError("URDF offset_cm must contain three finite numbers") + if len(self.rotation_xyzw) != 4: + raise ValueError("URDF rotation_xyzw must contain four numbers") + normalize(np.asarray(self.rotation_xyzw), label="URDF rotation_xyzw") + if not all( + isinstance(key, str) and isinstance(value, str) + for key, value in self.joint_map.items() + ): + raise ValueError("URDF joint_map must map URDF names to raw joint names") + + +def load_urdf_config(path: Path | None) -> UrdfConfig: + if path is None: + config = UrdfConfig() + config.validate() + return config + if not path.is_file(): + raise ValueError(f"URDF config does not exist: {path}") + text = path.read_text(encoding="utf-8") + if path.suffix.lower() == ".json": + raw = json.loads(text) + else: + import yaml + + raw = yaml.safe_load(text) + if raw is None: + raw = {} + if not isinstance(raw, dict): + raise ValueError("URDF config root must be an object") + unknown = set(raw) - {"scale", "offset_cm", "rotation_xyzw", "joint_map"} + if unknown: + raise ValueError(f"unknown URDF config keys: {sorted(unknown)}") + config = UrdfConfig( + scale=float(raw.get("scale", 1.0)), + offset_cm=tuple(float(v) for v in raw.get("offset_cm", (0, 0, 0))), + rotation_xyzw=tuple(float(v) for v in raw.get("rotation_xyzw", (0, 0, 0, 1))), + joint_map=dict(raw.get("joint_map", {})), + ) + config.validate() + return config + + +def map_raw_joints( + urdf_joint_names: list[str], + raw_values: dict[str, float] | None, + joint_map: dict[str, str], +) -> np.ndarray: + raw = raw_values or {} + return np.asarray( + [raw.get(joint_map.get(name, name), 0.0) for name in urdf_joint_names], + dtype=np.float64, + ) diff --git a/examples/tactile_visualizer/urdf_config.example.yaml b/examples/tactile_visualizer/urdf_config.example.yaml new file mode 100644 index 0000000000..fb05940e5a --- /dev/null +++ b/examples/tactile_visualizer/urdf_config.example.yaml @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# URDF meshes and translations are scaled uniformly before the root placement. +scale: 1.0 +offset_cm: [0.0, 0.0, -1.0] +rotation_xyzw: [0.0, 0.0, 0.0, 1.0] + +# Keys are actuated URDF joint names; values are names in avatar_raw_left/right. +# Unlisted URDF joints use the same name, then fall back to zero if absent. +joint_map: + thumb_CMC_FE: thumb_CMC_FE + index_MCP_FE: index_MCP_FE diff --git a/src/core/AGENTS.md b/src/core/AGENTS.md index 706cd668d3..6b85531fe0 100644 --- a/src/core/AGENTS.md +++ b/src/core/AGENTS.md @@ -18,6 +18,7 @@ If work under **`src/core/`** went wrong—**user** correction, **pre-commit/CI* - When changing `TeleopSession` retargeting execution defaults, update config, docs, and default-behavior tests together so opt-in vs. default semantics stay aligned. - Preserve existing `TeleopSession` lifecycle flag semantics unless changing the public/context-manager contract intentionally; use tests to lock down cleanup details before altering them. - After Python test or session-manager edits, let `ruff format`/pre-commit own wrapping and rerun the hook when it modifies files. +- Schema NumPy properties are stubbed as `numpy.ndarray[numpy.float32]`, so mypy (`retargeting_mypy_type_check`) rejects `.shape[i]` on them; use `len()` or `np.asarray()` first. - **`IDeviceIOSource` leaves are only discovered when reachable from a declared graph root** — the `pipeline` combiner's outputs, the `teleop_control_pipeline`, or a subgraph registered in `TeleopSessionConfig(sinks=[...])`. `TeleopSession._discover_sources` unions the leaves of all three. An input *source* whose only purpose is a side effect (e.g. message-channel send) must therefore expose at least one output (a heartbeat boolean is the established pattern) **and** be reachable from one of those roots; silent no-discovery is the recurring footgun. **Output `IDeviceIOSink` nodes are different:** they are registered via `TeleopSessionConfig(sinks=[...])` and the session runs and flushes them explicitly each frame, so a sink needs no heartbeat output and no `OutputCombiner` reachability. - **A plugin a wheel install must be able to start is bundled into `isaaccapture/plugins//`** (binary + `plugin.yaml`, via a POST_BUILD copy and `add_dependencies(python_package ...)`), and its binary must not need shared libraries the wheel does not ship. Apps start it explicitly with `PluginConfig(search_paths=[BUNDLED_PLUGINS_DIR])`; sources never start plugins. See `src/plugins/spacemouse`. - **All C++ diagnostics go through `isaaccapture::Logger`** (`log_bridge/cpp/inc/log_bridge/logger.hpp`), never `std::cout`/`std::cerr`/`printf`, and the logger name is always `isaaccapture..`. The env-var contract and wire format are shared with the Python half, so a change to either must land in both — see [`log_bridge/AGENTS.md`](log_bridge/AGENTS.md) before touching the logging machinery, and the repo root [`AGENTS.md`](../../AGENTS.md) "Logging" section for the rules that apply to every call site. diff --git a/src/core/codegen/AGENTS.md b/src/core/codegen/AGENTS.md index 2ffb5942a0..8fb0f22d9d 100644 --- a/src/core/codegen/AGENTS.md +++ b/src/core/codegen/AGENTS.md @@ -61,7 +61,9 @@ and the `.inc` fragments. mangle `@KEY@` on save. Conditionals stay in Python (pick which template to load) or as `#if @FLAG@` for optional fragments within one shape (e.g. `@HAS_TENSOR_IDENTIFIER@`). Do **not** merge `push/` with `pull/` via shared `#if`/`#else` — those control flows are different - enough that separate templates stay clearer. `*.hpp.template` / `*.cpp.template` carry SPDX + enough that separate templates stay clearer. The same holds for `metadata`: the live and replay + impls of such a tracker are `pull/*_metadata.*.template`, while the interface, facade, pybind and + traits only gain `#if @HAS_METADATA@` blocks. `*.hpp.template` / `*.cpp.template` carry SPDX headers for REUSE; `template_renderer` strips them before prepending the emit-time COPYRIGHT / AUTO-GENERATED banner. - **One-line aggregators** (includes, dispatch rows, factory decls, cmake source lists, Python diff --git a/src/core/codegen/generate_trackers.py b/src/core/codegen/generate_trackers.py index 2c1b46e5f2..d01ab57225 100644 --- a/src/core/codegen/generate_trackers.py +++ b/src/core/codegen/generate_trackers.py @@ -46,6 +46,8 @@ def _join_lines(lines: Iterator[str]) -> str: def _render_tracker_set(ctx: TrackerGenContext, prefix: str) -> dict[str, str]: """Seven-file bundle: deviceio_base, facade hpp/cpp, live hpp/cpp, replay hpp/cpp.""" + # A tracker with metadata reads, gates and replays two streams, so its impls have their own bodies. + impl = "_metadata" if ctx.metadata else "" return { f"deviceio_base/{ctx.base_header}.hpp": render_with_copyright( f"{prefix}/deviceio_base.hpp.template", ctx, COPYRIGHT @@ -57,16 +59,16 @@ def _render_tracker_set(ctx: TrackerGenContext, prefix: str) -> dict[str, str]: f"{prefix}/facade.cpp.template", ctx, COPYRIGHT ), f"live_trackers/{ctx.live_impl_file}.hpp": render_with_copyright( - f"{prefix}/live.hpp.template", ctx, COPYRIGHT + f"{prefix}/live{impl}.hpp.template", ctx, COPYRIGHT ), f"live_trackers/{ctx.live_impl_file}.cpp": render_with_copyright( - f"{prefix}/live.cpp.template", ctx, COPYRIGHT + f"{prefix}/live{impl}.cpp.template", ctx, COPYRIGHT ), f"replay_trackers/{ctx.replay_impl_file}.hpp": render_with_copyright( - f"{prefix}/replay.hpp.template", ctx, COPYRIGHT + f"{prefix}/replay{impl}.hpp.template", ctx, COPYRIGHT ), f"replay_trackers/{ctx.replay_impl_file}.cpp": render_with_copyright( - f"{prefix}/replay.cpp.template", ctx, COPYRIGHT + f"{prefix}/replay{impl}.cpp.template", ctx, COPYRIGHT ), } @@ -156,6 +158,8 @@ def _live_factory_template(ctx: TrackerGenContext) -> str: def _replay_factory_template(ctx: TrackerGenContext) -> str: if ctx.direction == "push": return "replay_factory_push.template" + if ctx.metadata: + return "replay_factory_pull_metadata.template" if ctx.record: return "replay_factory_pull.template" return "replay_factory_pull_plain.template" diff --git a/src/core/codegen/manifest.py b/src/core/codegen/manifest.py index bb5f15ad35..015a938b89 100644 --- a/src/core/codegen/manifest.py +++ b/src/core/codegen/manifest.py @@ -32,11 +32,14 @@ "max_flatbuffer_size", "record", "facade_tensor_constant", + "metadata", + "metadata_schema", + "metadata_channel", } ) _PLACEHOLDER = re.compile( - r"%(?P[a-zA-Z0-9_]+?)(?P_(?:CamelCase|UPPER))?%|(?P%%)" + r"%(?P[a-zA-Z0-9_]+?)(?P_(?:CamelCase|snake_case|UPPER))?%|(?P%%)" ) @@ -61,9 +64,18 @@ def snake_to_camel(value: str) -> str: return "".join(out) +def camel_to_snake(value: str) -> str: + """Convert CamelCase to snake_case (TactileShape -> tactile_shape).""" + return re.sub( + r"(?<=[a-z0-9])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])", "_", value + ).lower() + + def apply_transform(value: str, transform: str | None) -> str: if transform == "_CamelCase": return snake_to_camel(value) + if transform == "_snake_case": + return camel_to_snake(value) if transform == "_UPPER": return value.upper() return value @@ -122,6 +134,11 @@ def resolve_tracker_entry( raise ValueError("tracker entry missing required key 'name'") if "table" not in raw: raise ValueError(f"tracker '{raw['name']}': missing required key 'table'") + if "record_type" in raw: + raise ValueError( + f"tracker '{raw['name']}': record_type is no longer supported; " + "generated trackers always record
Record" + ) direction = str( raw.get("direction", defaults_doc.get("defaults", {}).get("direction", "pull")) @@ -172,6 +189,11 @@ def resolve_tracker_entry( f"tracker '{raw['name']}': record must be a boolean, got {type(record).__name__}" ) + if resolved["metadata"] and resolved["direction"] != "pull": + raise ValueError( + f"tracker '{raw['name']}': metadata is supported only for direction=pull" + ) + if resolved["direction"] == "pull" and record: for key in ("mcap_channels", "replay_channels"): if key not in resolved: diff --git a/src/core/codegen/template_renderer.py b/src/core/codegen/template_renderer.py index 05cde6f9fd..117185ff10 100644 --- a/src/core/codegen/template_renderer.py +++ b/src/core/codegen/template_renderer.py @@ -63,6 +63,10 @@ def template_values(ctx: TrackerGenContext) -> dict[str, str]: "PYTHON_ACCESSOR": ctx.python_accessor, "TENSOR_IDENTIFIER_EXPR": _tensor_identifier_expr(ctx), "HAS_TENSOR_IDENTIFIER": "1" if ctx.facade_tensor_constant else "0", + "HAS_METADATA": "1" if ctx.metadata else "0", + "METADATA": ctx.metadata or "", + "METADATA_SCHEMA": ctx.metadata_schema, + "METADATA_CHANNEL": ctx.metadata_channel, } if ctx.mcap_channels is not None: values["RECORDING_CHANNELS"] = ", ".join(f'"{c}"' for c in ctx.mcap_channels) diff --git a/src/core/codegen/templates.py b/src/core/codegen/templates.py index 241df46576..ae9c013e9d 100644 --- a/src/core/codegen/templates.py +++ b/src/core/codegen/templates.py @@ -39,6 +39,10 @@ class TrackerGenContext: facade_tensor_constant: bool mcap_channels: tuple[str, ...] | None replay_channels: tuple[str, ...] | None + # Metadata table, its .fbs stem and its MCAP sub-channel; metadata is None when unset. + metadata: str | None + metadata_schema: str + metadata_channel: str header: str base_header: str live_impl_file: str @@ -99,6 +103,9 @@ def enrich_context(entry: dict[str, Any]) -> TrackerGenContext: facade_tensor_constant=bool(entry["facade_tensor_constant"]), mcap_channels=mcap_channels, replay_channels=replay_channels, + metadata=str(entry["metadata"]) or None, + metadata_schema=str(entry["metadata_schema"]), + metadata_channel=str(entry["metadata_channel"]), header=header, base_header=base, live_impl_file=live_impl_file, diff --git a/src/core/codegen/templates/fragments/pybind_pull.template b/src/core/codegen/templates/fragments/pybind_pull.template index d4ead323cf..4c6d0b8c84 100644 --- a/src/core/codegen/templates/fragments/pybind_pull.template +++ b/src/core/codegen/templates/fragments/pybind_pull.template @@ -6,4 +6,13 @@ [](const core::@CLASS@& self, const core::ITrackerSession& session) { return to_python(self.get_data(session)); }, py::arg("session"), - "Get the current state (None when no data available)"); + "Get the current state (None when no data available)") +#if @HAS_METADATA@ + .def( + "get_metadata", + [](const core::@CLASS@& self, const core::ITrackerSession& session) + { return to_python(self.get_metadata(session)); }, + py::arg("session"), + "Get the producer's metadata (None until it arrives; fixed for the session after that)") +#endif + ; diff --git a/src/core/codegen/templates/fragments/recording_traits_pull.template b/src/core/codegen/templates/fragments/recording_traits_pull.template index 0a5fa6021e..cfb8d97dc2 100644 --- a/src/core/codegen/templates/fragments/recording_traits_pull.template +++ b/src/core/codegen/templates/fragments/recording_traits_pull.template @@ -2,4 +2,7 @@ struct @TRAITS@ { static constexpr std::array recording_channels = { @RECORDING_CHANNELS@ }; static constexpr std::array replay_channels = { @REPLAY_CHANNELS@ }; +#if @HAS_METADATA@ + static constexpr std::array metadata_channels = { "@METADATA_CHANNEL@" }; +#endif }; diff --git a/src/core/codegen/templates/fragments/replay_factory_pull_metadata.template b/src/core/codegen/templates/fragments/replay_factory_pull_metadata.template new file mode 100644 index 0000000000..d8d57ce971 --- /dev/null +++ b/src/core/codegen/templates/fragments/replay_factory_pull_metadata.template @@ -0,0 +1,5 @@ +std::unique_ptr<@IFACE@> ReplayDeviceIOFactory::create_@NAME@_tracker_impl(const @CLASS@* tracker) +{ + return std::make_unique<@REPLAY_IMPL@>( + open_reader(filename_), open_reader(filename_), get_name(tracker), recorded_schemas_); +} diff --git a/src/core/codegen/templates/pull/deviceio_base.hpp.template b/src/core/codegen/templates/pull/deviceio_base.hpp.template index 261d844ba8..1937984b16 100644 --- a/src/core/codegen/templates/pull/deviceio_base.hpp.template +++ b/src/core/codegen/templates/pull/deviceio_base.hpp.template @@ -7,9 +7,15 @@ namespace core { struct @FB_TABLE@; +#if @HAS_METADATA@ +struct @METADATA@; +#endif class @IFACE@ : public ITrackerImpl { public: virtual const Serialized<@FB_TABLE@>& get_data() const = 0; +#if @HAS_METADATA@ + virtual const Serialized<@METADATA@>& get_metadata() const = 0; +#endif }; } // namespace core diff --git a/src/core/codegen/templates/pull/facade.cpp.template b/src/core/codegen/templates/pull/facade.cpp.template index 657a00c431..98fcde580a 100644 --- a/src/core/codegen/templates/pull/facade.cpp.template +++ b/src/core/codegen/templates/pull/facade.cpp.template @@ -14,4 +14,10 @@ const Serialized<@FB_TABLE@>& @CLASS@::get_data(const ITrackerSession& session) { return static_cast(session.get_tracker_impl(*this)).get_data(); } +#if @HAS_METADATA@ +const Serialized<@METADATA@>& @CLASS@::get_metadata(const ITrackerSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_metadata(); +} +#endif } // namespace core diff --git a/src/core/codegen/templates/pull/facade.hpp.template b/src/core/codegen/templates/pull/facade.hpp.template index 150e1b847c..2130178e5d 100644 --- a/src/core/codegen/templates/pull/facade.hpp.template +++ b/src/core/codegen/templates/pull/facade.hpp.template @@ -4,6 +4,9 @@ #pragma once #include #include +#if @HAS_METADATA@ +#include +#endif #include #include @@ -25,6 +28,11 @@ public: return TRACKER_NAME; } const Serialized<@FB_TABLE@>& get_data(const ITrackerSession& session) const; +#if @HAS_METADATA@ + //! The producer's metadata: empty until it arrives, then fixed for the session. get_data() + //! stays empty while the producer's metadata is not the one held here. + const Serialized<@METADATA@>& get_metadata(const ITrackerSession& session) const; +#endif const std::string& collection_id() const { return collection_id_; diff --git a/src/core/codegen/templates/pull/live_metadata.cpp.template b/src/core/codegen/templates/pull/live_metadata.cpp.template new file mode 100644 index 0000000000..3a1bfca35d --- /dev/null +++ b/src/core/codegen/templates/pull/live_metadata.cpp.template @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "@LIVE_IMPL_FILE@.hpp" + +#include +#include +#include + +namespace core +{ +namespace +{ +SchemaTrackerConfig make_@NAME@_tensor_config(const @CLASS@* tracker) +{ + SchemaTrackerConfig cfg; + cfg.collection_id = tracker->collection_id(); + cfg.max_flatbuffer_size = tracker->max_flatbuffer_size(); + cfg.tensor_identifier = @TENSOR_IDENTIFIER_EXPR@; + cfg.localized_name = "@LOCALIZED_NAME@"; + return cfg; +} +} // namespace +@MCAP_CHANNELS_TYPE@::@MCAP_CHANNELS_TYPE@(mcap::McapWriter& writer, std::string_view base_name) + : data(writer, + base_name, + std::vector(@TRAITS@::recording_channels.begin(), @TRAITS@::recording_channels.end())), + metadata(writer, + base_name, + std::vector(@TRAITS@::metadata_channels.begin(), @TRAITS@::metadata_channels.end())) +{ + // No-op: members set in the initializer list. +} +std::unique_ptr<@MCAP_CHANNELS_TYPE@> @LIVE_IMPL@::create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name) +{ + return std::make_unique<@MCAP_CHANNELS_TYPE@>(writer, base_name); +} +@LIVE_IMPL@::@LIVE_IMPL@(const OpenXRSessionHandles& handles, + const @CLASS@* tracker, + std::unique_ptr<@MCAP_CHANNELS_TYPE@> mcap_channels) + : mcap_channels_(std::move(mcap_channels)), + m_schema_reader(handles, make_@NAME@_tensor_config(tracker), mcap_channels_ ? &mcap_channels_->data : nullptr, 0, 1), + m_metadata_reader(handles, tracker->collection_id(), mcap_channels_ ? &mcap_channels_->metadata : nullptr) +{ + // No-op: members set in the initializer list. +} +void @LIVE_IMPL@::update(int64_t monotonic_time_ns) +{ + // Metadata first, so data drained on the tick it is verified is already recorded. + m_metadata_reader.update(monotonic_time_ns); + const bool present = m_schema_reader.update(m_received, m_metadata_reader.verified()); + m_metadata_reader.set_data_present(present); + if (!m_metadata_reader.verified()) + { + // Do not retain a sample received before this producer's metadata was verified. + m_received.reset(); + } + m_tracked = m_metadata_reader.verified() ? m_received : Serialized<@FB_TABLE@>(); +} +const Serialized<@FB_TABLE@>& @LIVE_IMPL@::get_data() const +{ + return m_tracked; +} +const Serialized<@METADATA@>& @LIVE_IMPL@::get_metadata() const +{ + return m_metadata_reader.get(); +} + +} // namespace core diff --git a/src/core/codegen/templates/pull/live_metadata.hpp.template b/src/core/codegen/templates/pull/live_metadata.hpp.template new file mode 100644 index 0000000000..53f3cef86a --- /dev/null +++ b/src/core/codegen/templates/pull/live_metadata.hpp.template @@ -0,0 +1,53 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +namespace core +{ +// Data and metadata channels together, so the factory creates and hands over one object. +struct @MCAP_CHANNELS_TYPE@ +{ + @MCAP_CHANNELS_TYPE@(mcap::McapWriter& writer, std::string_view base_name); + + McapTrackerChannels<@RECORD_TYPE@> data; + McapTrackerChannels<@METADATA@> metadata; +}; +using @SCHEMA_TRACKER_TYPE@ = SchemaTracker<@RECORD_TYPE@, @FB_TABLE@>; +class @LIVE_IMPL@ : public @IFACE@ +{ +public: + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + static std::unique_ptr<@MCAP_CHANNELS_TYPE@> create_mcap_channels(mcap::McapWriter& writer, + std::string_view base_name); + @LIVE_IMPL@(const OpenXRSessionHandles& handles, + const @CLASS@* tracker, + std::unique_ptr<@MCAP_CHANNELS_TYPE@> mcap_channels); + void update(int64_t monotonic_time_ns) override; + const Serialized<@FB_TABLE@>& get_data() const override; + const Serialized<@METADATA@>& get_metadata() const override; + +private: + std::unique_ptr<@MCAP_CHANNELS_TYPE@> mcap_channels_; + @SCHEMA_TRACKER_TYPE@ m_schema_reader; + SchemaMetadataReader<@METADATA@> m_metadata_reader; + // What the collection last delivered, and what is published: the same, or empty while the + // metadata is unverified. + Serialized<@FB_TABLE@> m_received; + Serialized<@FB_TABLE@> m_tracked; +}; +} // namespace core diff --git a/src/core/codegen/templates/pull/replay_metadata.cpp.template b/src/core/codegen/templates/pull/replay_metadata.cpp.template new file mode 100644 index 0000000000..3d3df31e9a --- /dev/null +++ b/src/core/codegen/templates/pull/replay_metadata.cpp.template @@ -0,0 +1,75 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "@REPLAY_IMPL_FILE@.hpp" + +#include +#include +#include + +#include +#include + +namespace core +{ +namespace +{ +// Live records the metadata once, before any data it lets through, so the first message is the +// one every recorded frame was published with. +Serialized<@METADATA@> read_@NAME@_metadata(std::unique_ptr reader, + std::string_view base_name, + const RecordedSchemas& recorded) +{ + McapTrackerViewers<@METADATA@> viewers( + std::move(reader), base_name, + std::vector(@TRAITS@::metadata_channels.begin(), @TRAITS@::metadata_channels.end()), recorded); + return viewers.read(0); +} +} // namespace +@REPLAY_IMPL@::@REPLAY_IMPL@(std::unique_ptr reader, + std::unique_ptr metadata_reader, + std::string_view base_name, + const RecordedSchemas& recorded) + : metadata_(read_@NAME@_metadata(std::move(metadata_reader), base_name, recorded)), + mcap_viewers_(std::make_unique<@MCAP_VIEWERS_TYPE@>( + std::move(reader), + base_name, + std::vector(@TRAITS@::replay_channels.begin(), @TRAITS@::replay_channels.end()), + recorded)), + logger_(isaaccapture::Logger::get("isaaccapture.core.@REPLAY_IMPL@")), + no_data_message_("no data (EOF or gap), base_name=" + std::string(base_name)) +{ + if (!metadata_) + { + logger_->warn("recording has no {} for base_name={}; its data will not be published", + @METADATA@::GetFullyQualifiedName(), base_name); + } +} +const Serialized<@FB_TABLE@>& @REPLAY_IMPL@::get_data() const +{ + return tracked_; +} +const Serialized<@METADATA@>& @REPLAY_IMPL@::get_metadata() const +{ + return metadata_; +} +void @REPLAY_IMPL@::update(int64_t /*monotonic_time_ns*/) +{ + auto record = mcap_viewers_->read(0); + if (record) + { + // Mirrors live, which publishes data only with its metadata. + tracked_ = metadata_ ? record.narrow(record->data()) : Serialized<@FB_TABLE@>(); + warned_no_data_ = false; + } + else + { + if (!warned_no_data_) + { + logger_->warn("{}", no_data_message_); + warned_no_data_ = true; + } + tracked_.reset(); + } +} +} // namespace core diff --git a/src/core/codegen/templates/pull/replay_metadata.hpp.template b/src/core/codegen/templates/pull/replay_metadata.hpp.template new file mode 100644 index 0000000000..366c0a04db --- /dev/null +++ b/src/core/codegen/templates/pull/replay_metadata.hpp.template @@ -0,0 +1,38 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once +#include +#include +#include +#include +#include + +#include +#include +#include +#include +namespace core +{ +using @MCAP_VIEWERS_TYPE@ = McapTrackerViewers<@RECORD_TYPE@>; +class @REPLAY_IMPL@ : public @IFACE@ +{ +public: + // `metadata_reader` is read to the metadata and dropped during construction. + @REPLAY_IMPL@(std::unique_ptr reader, + std::unique_ptr metadata_reader, + std::string_view base_name, + const RecordedSchemas& recorded); + void update(int64_t monotonic_time_ns) override; + const Serialized<@FB_TABLE@>& get_data() const override; + const Serialized<@METADATA@>& get_metadata() const override; + +private: + Serialized<@FB_TABLE@> tracked_; + Serialized<@METADATA@> metadata_; + std::unique_ptr<@MCAP_VIEWERS_TYPE@> mcap_viewers_; + std::shared_ptr logger_; + std::string no_data_message_; + bool warned_no_data_ = false; +}; +} // namespace core diff --git a/src/core/codegen/test_manifest.py b/src/core/codegen/test_manifest.py index 651deb4292..c0f6aee291 100644 --- a/src/core/codegen/test_manifest.py +++ b/src/core/codegen/test_manifest.py @@ -8,7 +8,13 @@ import unittest from pathlib import Path -from manifest import load_defaults, load_manifest, resolve_tracker_entry, snake_to_camel +from manifest import ( + camel_to_snake, + load_defaults, + load_manifest, + resolve_tracker_entry, + snake_to_camel, +) CORE_ROOT = Path(__file__).resolve().parents[1] @@ -23,6 +29,13 @@ def test_se3(self) -> None: self.assertEqual(snake_to_camel("se3_tracker"), "Se3Tracker") +class CamelToSnakeTest(unittest.TestCase): + def test_metadata_tables(self) -> None: + self.assertEqual(camel_to_snake("TactileShape"), "tactile_shape") + self.assertEqual(camel_to_snake("Se3TrackerPose"), "se3_tracker_pose") + self.assertEqual(camel_to_snake("HTTPHeader"), "http_header") + + class ManifestResolverTest(unittest.TestCase): def test_cycle_is_rejected(self) -> None: defaults = {"defaults": {"a": "%b%", "b": "%a%"}} @@ -176,6 +189,14 @@ def test_record_must_be_boolean(self) -> None: self.assertIn("record", message) self.assertIn("boolean", message) + def test_removed_record_type_is_rejected(self) -> None: + with self.assertRaises(ValueError) as ctx: + resolve_tracker_entry( + {"name": "x", "table": "T", "record_type": "T"}, + load_defaults(DEFAULTS_PATH), + ) + self.assertIn("record_type", str(ctx.exception)) + def test_load_manifest_file(self) -> None: manifest = CORE_ROOT / "deviceio_trackers" / "trackers.toml" self.assertTrue(manifest.is_file(), f"missing required manifest: {manifest}") @@ -207,5 +228,59 @@ def test_header_override_preserves_file_stems(self) -> None: self.assertEqual(ctx.traits, "OakRecordingTraits") +class MetadataTest(unittest.TestCase): + def _resolve(self, **overrides) -> dict: + raw = {"name": "tactile", "table": "TactileData", **overrides} + return resolve_tracker_entry(raw, load_defaults(DEFAULTS_PATH)) + + def test_unset_by_default(self) -> None: + self.assertEqual(self._resolve()["metadata"], "") + + def test_schema_and_channel_derive_from_the_table(self) -> None: + entry = self._resolve(metadata="TactileShape") + self.assertEqual(entry["metadata_schema"], "tactile_shape") + self.assertEqual(entry["metadata_channel"], "tactile_metadata") + + def test_push_direction_is_rejected(self) -> None: + with self.assertRaises(ValueError) as ctx: + self._resolve(direction="push", metadata="TactileShape") + self.assertIn("metadata", str(ctx.exception)) + + def test_renders_metadata_impls_with_no_placeholder_left(self) -> None: + from generate_trackers import _render_fragment, render_for_entry + from templates import enrich_context + + ctx = enrich_context(self._resolve(metadata="TactileShape")) + files = render_for_entry(ctx) + files["factory"] = _render_fragment( + "replay_factory_pull_metadata.template", ctx + ) + for name, text in files.items(): + self.assertNotRegex(text, r"@[A-Z_]+@", name) + self.assertIn( + "SchemaMetadataReader", + files["live_trackers/live_tactile_tracker_impl.hpp"], + ) + self.assertIn( + "if (!m_metadata_reader.verified())\n {\n" + " // Do not retain a sample received before this producer's metadata was verified.\n" + " m_received.reset();", + files["live_trackers/live_tactile_tracker_impl.cpp"], + ) + self.assertIn( + "get_metadata", + files["deviceio_trackers/inc/deviceio_trackers/tactile_tracker.hpp"], + ) + + def test_trackers_without_metadata_keep_their_impls(self) -> None: + from generate_trackers import render_for_entry + from templates import enrich_context + + files = render_for_entry(enrich_context(self._resolve())) + self.assertNotIn( + "SchemaMetadataReader", files["live_trackers/live_tactile_tracker_impl.hpp"] + ) + + if __name__ == "__main__": unittest.main() diff --git a/src/core/deviceio_trackers/AGENTS.md b/src/core/deviceio_trackers/AGENTS.md index 8f0ca5f0cd..a0c0c0cf6b 100644 --- a/src/core/deviceio_trackers/AGENTS.md +++ b/src/core/deviceio_trackers/AGENTS.md @@ -25,6 +25,9 @@ under `${CMAKE_BINARY_DIR}/generated/trackers/`, **not** in `cpp/`. public `#include` stem stays `frame_metadata_tracker_oak`). - **`direction = "push"`** generates a typed `
PushTracker` (Teleop → plugin) instead of a reader facade. `TensorPushTracker` stays as the untyped `bytes` escape hatch — keep it. +- **Static per-device data is `metadata`, not a second tracker.** `metadata = "
"` adds + `get_metadata()` and the chunked companion collection; do not declare the table as its own + `[[tracker]]`, which makes every consumer pair and cache two streams itself. - **Do not hand-register a manifest tracker.** Factory dispatch rows, factory methods, pybind blocks, MCAP recording traits, and the Python `__all__` are emitted as `.inc` fragments that the hand-written files `#include`; adding a row yourself produces a duplicate definition. diff --git a/src/core/deviceio_trackers/defaults.toml b/src/core/deviceio_trackers/defaults.toml index 9c45c042ed..66782b53fd 100644 --- a/src/core/deviceio_trackers/defaults.toml +++ b/src/core/deviceio_trackers/defaults.toml @@ -1,7 +1,8 @@ # SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -# Default tracker manifest values. Placeholders use %key% or %key_CamelCase% / %key_UPPER%. +# Default tracker manifest values. Placeholders use %key% or %key_CamelCase% / %key_snake_case% / +# %key_UPPER%. # See src/core/codegen/manifest.py for resolution rules. [defaults] @@ -17,6 +18,10 @@ python_accessor = "get_data" max_flatbuffer_size = 512 record = true facade_tensor_constant = false +# Table the tracker also publishes once per producer; empty for none (see trackers.toml). +metadata = "" +metadata_schema = "%metadata_snake_case%" +metadata_channel = "%channel%_metadata" [defaults.pull] mcap_channels = ["%channel%", "%channel%_tracked"] diff --git a/src/core/deviceio_trackers/trackers.toml b/src/core/deviceio_trackers/trackers.toml index 549561a285..a0df5a99f8 100644 --- a/src/core/deviceio_trackers/trackers.toml +++ b/src/core/deviceio_trackers/trackers.toml @@ -106,3 +106,11 @@ class = "SpaceMouseTracker" traits = "SpaceMouseRecordingTraits" max_flatbuffer_size = 256 python_accessor = "get_spacemouse_data" + +# Defaults fit about 1300 taxels (12 B each); larger devices pass max_flatbuffer_size. +# The TactileShape metadata is chunked, so its size is not bounded by a sample. +[[tracker]] +name = "tactile" +table = "TactileData" +metadata = "TactileShape" +max_flatbuffer_size = 16384 diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index fad1087cae..a216f263d4 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -5,6 +5,7 @@ cmake_minimum_required(VERSION 3.20) add_library(live_trackers STATIC schema_tracker_base.cpp + schema_metadata_reader.cpp live_deviceio_factory.cpp live_head_tracker_impl.cpp live_hand_tracker_impl.cpp @@ -19,6 +20,7 @@ add_library(live_trackers STATIC ${GENERATED_TRACKER_LIVE_SOURCES} inc/live_trackers/schema_tracker_base.hpp inc/live_trackers/schema_tracker.hpp + inc/live_trackers/schema_metadata_reader.hpp inc/live_trackers/live_deviceio_factory.hpp live_head_tracker_impl.hpp live_hand_tracker_impl.hpp diff --git a/src/core/live_trackers/cpp/inc/live_trackers/schema_metadata_reader.hpp b/src/core/live_trackers/cpp/inc/live_trackers/schema_metadata_reader.hpp new file mode 100644 index 0000000000..03416121f6 --- /dev/null +++ b/src/core/live_trackers/cpp/inc/live_trackers/schema_metadata_reader.hpp @@ -0,0 +1,172 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_tracker_base.hpp" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace core +{ + +/*! + * @brief Untyped half of SchemaMetadataReader: reads the metadata collection and reassembles + * its chunks. + */ +class SchemaMetadataReaderBase +{ +public: + /*! + * @param collection_id The data collection; the chunks are read from + * tracker_metadata_collection_id(collection_id). + * @throws std::invalid_argument when that name does not fit XR_MAX_TENSOR_IDENTIFIER_SIZE. + */ + SchemaMetadataReaderBase(const OpenXRSessionHandles& handles, const std::string& collection_id); + +protected: + struct DrainResult + { + std::vector completed; + std::optional last_observed_hash; + }; + + //! Drains pending chunks and reports the hash of the last well-formed chunk it saw. + DrainResult drain(); + + //! Drops the assembly in progress, so the next buffer comes only from chunks read after this. + void restart(); + + std::shared_ptr m_logger = isaaccapture::Logger::get("isaaccapture.core.SchemaMetadataReader"); + +private: + SchemaTrackerBase m_reader; + TrackerMetadataAssembler m_assembler; + std::vector m_samples; + bool m_warned_rejection = false; +}; + +/*! + * @brief Live metadata of a tracker whose manifest entry sets `metadata`. + * + * The first complete metadata is adopted, recorded once, and kept for the life of this reader. + * After that, each complete pass of the producer's chunk loop is checked against it: the data + * may be published and recorded only while verified(). Losing the data collection clears + * verified() until the producer that comes back publishes the same metadata again. + */ +template +class SchemaMetadataReader : public SchemaMetadataReaderBase +{ +public: + using Channels = McapTrackerChannels; + + //! @param mcap_channels Non-owning; channel 0 receives the adopted metadata. Null when not recording. + SchemaMetadataReader(const OpenXRSessionHandles& handles, const std::string& collection_id, Channels* mcap_channels) + : SchemaMetadataReaderBase(handles, collection_id), m_mcap_channels(mcap_channels) + { + } + + //! Call once per tick, before reading the data collection. + void update(int64_t monotonic_time_ns) + { + DrainResult result = drain(); + const bool ended_with_unexpected_hash = + m_metadata && result.last_observed_hash && *result.last_observed_hash != m_metadata_hash; + if (ended_with_unexpected_hash) + { + withhold_for_mismatch(); + } + std::vector completed = std::move(result.completed); + if (completed.empty()) + { + return; + } + + // A complete candidate supersedes the previous verification result. Restore it only + // after the candidate is both a valid table and the metadata already adopted here. + m_verified = false; + flatbuffers::Verifier verifier(completed.data(), completed.size()); + if (!verifier.VerifyBuffer(nullptr)) + { + m_logger->warn("reassembled metadata is not a valid {}", MetadataT::GetFullyQualifiedName()); + return; + } + + if (!m_metadata) + { + m_metadata = Serialized::adopt(std::move(completed)); + m_metadata_hash = tracker_metadata_hash(m_metadata.buffer()); + m_verified = true; + if (m_mcap_channels != nullptr) + { + m_mcap_channels->write(0, m_metadata, monotonic_time_ns); + } + m_logger->info("adopted {} B of {}", m_metadata.buffer().size(), MetadataT::GetFullyQualifiedName()); + return; + } + + const std::span adopted = m_metadata.buffer(); + if (ended_with_unexpected_hash || !std::equal(adopted.begin(), adopted.end(), completed.begin(), completed.end())) + { + withhold_for_mismatch(); + } + else + { + m_verified = true; + } + } + + //! Reports whether the data collection was present this tick. + void set_data_present(bool present) + { + if (!present) + { + m_verified = false; + restart(); + } + } + + bool verified() const + { + return m_verified; + } + + //! Empty until the first complete metadata, then fixed. + const Serialized& get() const + { + return m_metadata; + } + +private: + void withhold_for_mismatch() + { + m_verified = false; + if (!m_warned_mismatch) + { + m_logger->error("producer now publishes a different {} than the one adopted; its data is withheld", + MetadataT::GetFullyQualifiedName()); + m_warned_mismatch = true; + } + } + + Channels* m_mcap_channels; + Serialized m_metadata; + uint64_t m_metadata_hash = 0; + bool m_verified = false; + bool m_warned_mismatch = false; +}; + +} // namespace core diff --git a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker.hpp b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker.hpp index 9f57084bda..2301b3e6ac 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker.hpp @@ -11,6 +11,7 @@ #include #include +#include namespace core { @@ -65,12 +66,15 @@ class SchemaTracker : public SchemaTrackerBase * @param out Receives the final sample of this tick when any were read; left * untouched when the collection is present but produced nothing (the * last-known sample is retained); emptied when the collection is absent. + * @param record False skips MCAP for this tick's samples; a tracker with metadata + * records only data it can publish. + * @return Whether the collection is present. * @throws std::runtime_error On critical OpenXR/tensor API failures propagated * from SchemaTrackerBase. * @note Missing collection, temporary collection loss, and "no new sample" * are treated as common non-fatal conditions and do not throw. */ - void update(Serialized& out) + bool update(Serialized& out, bool record = true) { samples_.clear(); bool present = read_all_samples(samples_); @@ -81,10 +85,10 @@ class SchemaTracker : public SchemaTrackerBase { out.reset(); } - return; + return present; } - if (mcap_channels_) + if (mcap_channels_ && record) { Serialized last_record; for (const auto& sample : samples_) @@ -116,6 +120,7 @@ class SchemaTracker : public SchemaTrackerBase // published type. Each tick owns its own buffer, so a consumer still holding last // tick's handle keeps last tick's values. out = Serialized::adopt(std::move(samples_.back().buffer)); + return present; } private: diff --git a/src/core/live_trackers/cpp/schema_metadata_reader.cpp b/src/core/live_trackers/cpp/schema_metadata_reader.cpp new file mode 100644 index 0000000000..8924784a37 --- /dev/null +++ b/src/core/live_trackers/cpp/schema_metadata_reader.cpp @@ -0,0 +1,89 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/live_trackers/schema_metadata_reader.hpp" + +#include +#include +#include +#include + +namespace core +{ + +namespace +{ + +SchemaTrackerConfig metadata_reader_config(const std::string& collection_id) +{ + std::string metadata_collection_id = tracker_metadata_collection_id(collection_id); + // The pusher truncates longer identifiers, so this reader would look for another name. + if (metadata_collection_id.size() >= XR_MAX_TENSOR_IDENTIFIER_SIZE) + { + throw std::invalid_argument("SchemaMetadataReader: collection id '" + metadata_collection_id + + "' does not fit XR_MAX_TENSOR_IDENTIFIER_SIZE"); + } + return SchemaTrackerConfig{ .collection_id = std::move(metadata_collection_id), + .max_flatbuffer_size = TRACKER_METADATA_CHUNK_SIZE, + .tensor_identifier = std::string(TRACKER_METADATA_TENSOR_IDENTIFIER), + .localized_name = "Tracker metadata" }; +} + +} // namespace + +SchemaMetadataReaderBase::SchemaMetadataReaderBase(const OpenXRSessionHandles& handles, const std::string& collection_id) + : m_reader(handles, metadata_reader_config(collection_id)) +{ +} + +SchemaMetadataReaderBase::DrainResult SchemaMetadataReaderBase::drain() +{ + m_samples.clear(); + m_reader.read_all_samples(m_samples); + + DrainResult result; + for (const auto& sample : m_samples) + { + flatbuffers::Verifier verifier(sample.buffer.data(), sample.buffer.size()); + if (!verifier.VerifyBuffer(nullptr)) + { + if (!m_warned_rejection) + { + m_logger->warn("dropping a sample that is not a TrackerMetadataChunk"); + m_warned_rejection = true; + } + continue; + } + + const auto& chunk = *flatbuffers::GetRoot(sample.buffer.data()); + const auto status = m_assembler.add(chunk); + if (status != TrackerMetadataAssembler::Status::Rejected) + { + result.last_observed_hash = chunk.metadata_hash(); + } + switch (status) + { + case TrackerMetadataAssembler::Status::Pending: + break; + case TrackerMetadataAssembler::Status::Complete: + result.completed = m_assembler.take(); + m_warned_rejection = false; + break; + case TrackerMetadataAssembler::Status::Rejected: + if (!m_warned_rejection) + { + m_logger->warn("dropping a metadata chunk: {}", m_assembler.rejection()); + m_warned_rejection = true; + } + break; + } + } + return result; +} + +void SchemaMetadataReaderBase::restart() +{ + m_assembler.reset(); +} + +} // namespace core diff --git a/src/core/mcap/cpp/inc/mcap/tracker_channels.hpp b/src/core/mcap/cpp/inc/mcap/tracker_channels.hpp index 152766ac4a..847a9f0c64 100644 --- a/src/core/mcap/cpp/inc/mcap/tracker_channels.hpp +++ b/src/core/mcap/cpp/inc/mcap/tracker_channels.hpp @@ -38,8 +38,10 @@ inline std::string mcap_topic(std::string_view base_name, const std::string& sub /** * @brief Type-safe MCAP channel writer for FlatBuffer record types. * - * @tparam RecordT The FlatBuffer record wrapper type (e.g. HeadPoseRecord). - * Must expose BinarySchema; records are encoded by pack_record(). + * @tparam RecordT The FlatBuffer root written to the channels: a record wrapper (e.g. + * HeadPoseRecord, encoded by pack_record()), or a table recorded as itself, + * which has no timestamp and is written with an explicit log time. + * Must expose BinarySchema. * * The factory creates a unique_ptr> only when recording * is active and passes it to the impl. Impls null-check before calling write(). @@ -73,6 +75,13 @@ class McapTrackerChannels * */ void write(size_t channel_index, const Serialized& record) + { + const auto* timestamp = record ? record->timestamp() : nullptr; + write(channel_index, record, timestamp != nullptr ? timestamp->available_time_local_common_clock() : 0); + } + + //! Writes a root with no timestamp of its own, such as a table recorded as itself, at `log_time_ns`. + void write(size_t channel_index, const Serialized& record, int64_t log_time_ns) { if (channel_index >= channel_ids_.size()) { @@ -87,11 +96,9 @@ class McapTrackerChannels throw std::invalid_argument("McapTrackerChannels: write called with a record that owns no buffer"); } - const auto* timestamp = record->timestamp(); mcap::Message msg; msg.channelId = channel_ids_[channel_index]; - msg.logTime = - timestamp != nullptr ? static_cast(timestamp->available_time_local_common_clock()) : 0; + msg.logTime = static_cast(log_time_ns); msg.publishTime = msg.logTime; msg.sequence = sequence_++; msg.data = reinterpret_cast(bytes.data()); diff --git a/src/core/pusherio/cpp/CMakeLists.txt b/src/core/pusherio/cpp/CMakeLists.txt index 268ff07294..4d74552e65 100644 --- a/src/core/pusherio/cpp/CMakeLists.txt +++ b/src/core/pusherio/cpp/CMakeLists.txt @@ -6,7 +6,9 @@ cmake_minimum_required(VERSION 3.20) # PusherIO library (static; consumed in-tree via pusherio::pusherio) add_library(pusherio STATIC schema_pusher.cpp + schema_metadata_pusher.cpp inc/pusherio/schema_pusher.hpp + inc/pusherio/schema_metadata_pusher.hpp ) target_include_directories(pusherio @@ -24,6 +26,9 @@ target_link_libraries(pusherio Teleop::openxr_extensions log_bridge::log_bridge_core + + # TrackerMetadataChunk and its split, for SchemaMetadataPusher + isaacteleop_schema ) # pusherio does NOT link to OpenXR loader - it's completely standalone diff --git a/src/core/pusherio/cpp/inc/pusherio/schema_metadata_pusher.hpp b/src/core/pusherio/cpp/inc/pusherio/schema_metadata_pusher.hpp new file mode 100644 index 0000000000..a36e8a897a --- /dev/null +++ b/src/core/pusherio/cpp/inc/pusherio/schema_metadata_pusher.hpp @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "schema_pusher.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace core +{ + +/*! + * @brief Publishes a tracker's metadata as a loop of chunks, a few per frame. + * + * Pairs with a generated tracker whose manifest entry sets `metadata`. The chunks go to + * tracker_metadata_collection_id(collection_id), next to the data collection. Call push_next() + * every frame for as long as the data collection exists: a consumer that connects late, or + * drops samples, completes the metadata on a later pass of the loop. + * + * The metadata is fixed for the life of this object. Different metadata needs a new data + * collection too, or consumers that already hold the old one withhold the data. + */ +class SchemaMetadataPusher +{ +public: + static std::vector get_required_extensions() + { + return SchemaPusher::get_required_extensions(); + } + + /*! + * @param collection_id The data collection the metadata describes. + * @param metadata The encoded metadata table, e.g. `pack(native).buffer()`. + * @param max_chunk_size Encoded chunk size; at most TRACKER_METADATA_CHUNK_SIZE, which is + * what consumers accept. + * @throws std::invalid_argument on an empty or oversized buffer, or a bad chunk size. + */ + SchemaMetadataPusher(const OpenXRSessionHandles& handles, + const std::string& collection_id, + std::span metadata, + size_t max_chunk_size = TRACKER_METADATA_CHUNK_SIZE); + + /*! + * @brief Pushes the next `count` chunks of the loop. + * @param sample_time_local_common_clock_ns Push time in system monotonic nanoseconds. + */ + void push_next(int64_t sample_time_local_common_clock_ns, size_t count = 1); + + //! Frames of push_next(_, 1) one pass over the metadata takes. + size_t chunk_count() const + { + return m_chunks.size(); + } + +private: + std::vector> m_chunks; + SchemaPusher m_pusher; + size_t m_next = 0; +}; + +} // namespace core diff --git a/src/core/pusherio/cpp/schema_metadata_pusher.cpp b/src/core/pusherio/cpp/schema_metadata_pusher.cpp new file mode 100644 index 0000000000..d269d0879a --- /dev/null +++ b/src/core/pusherio/cpp/schema_metadata_pusher.cpp @@ -0,0 +1,54 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/pusherio/schema_metadata_pusher.hpp" + +#include +#include +#include +#include + +namespace core +{ + +namespace +{ + +SchemaPusherConfig metadata_pusher_config(const std::string& collection_id, size_t max_chunk_size) +{ + std::string metadata_collection_id = tracker_metadata_collection_id(collection_id); + // SchemaPusher truncates longer identifiers, and the reader would then look for another name. + if (metadata_collection_id.size() >= XR_MAX_TENSOR_IDENTIFIER_SIZE) + { + throw std::invalid_argument("SchemaMetadataPusher: collection id '" + metadata_collection_id + + "' does not fit XR_MAX_TENSOR_IDENTIFIER_SIZE"); + } + return SchemaPusherConfig{ .collection_id = std::move(metadata_collection_id), + .max_flatbuffer_size = max_chunk_size, + .tensor_identifier = std::string(TRACKER_METADATA_TENSOR_IDENTIFIER), + .localized_name = "Tracker metadata" }; +} + +} // namespace + +SchemaMetadataPusher::SchemaMetadataPusher(const OpenXRSessionHandles& handles, + const std::string& collection_id, + std::span metadata, + size_t max_chunk_size) + : m_chunks(split_tracker_metadata(metadata, max_chunk_size)), + m_pusher(handles, metadata_pusher_config(collection_id, max_chunk_size)) +{ +} + +void SchemaMetadataPusher::push_next(int64_t sample_time_local_common_clock_ns, size_t count) +{ + for (size_t i = 0; i < count; ++i) + { + const std::span bytes = m_chunks[m_next].buffer(); + m_pusher.push_buffer( + bytes.data(), bytes.size(), sample_time_local_common_clock_ns, sample_time_local_common_clock_ns); + m_next = (m_next + 1) % m_chunks.size(); + } +} + +} // namespace core diff --git a/src/core/schema/cpp/inc/schema/tracker_metadata.hpp b/src/core/schema/cpp/inc/schema/tracker_metadata.hpp new file mode 100644 index 0000000000..a952e7ebac --- /dev/null +++ b/src/core/schema/cpp/inc/schema/tracker_metadata.hpp @@ -0,0 +1,195 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Wire contract for tracker metadata: a table fixed per producer, possibly larger than one tensor +// sample, so it travels as TrackerMetadataChunk samples on a companion collection. +// +// The producer pushes every chunk in a loop for as long as it runs, so a consumer that joins late +// or drops samples completes the buffer on a later pass. + +#pragma once + +#include "serialized.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace core +{ + +//! Chunk sample size producers use by default, and the largest one consumers accept. 128 KiB +//! samples failed in the CloudXR runtime's IPC; 16 KiB is the largest size in use (noitom_mocap). +inline constexpr size_t TRACKER_METADATA_CHUNK_SIZE = 16384; + +//! Largest metadata buffer a consumer reassembles; bounds what a chunk header can make it allocate. +inline constexpr size_t TRACKER_METADATA_MAX_SIZE = 16 * 1024 * 1024; + +//! Bytes of an encoded chunk that are not payload: root offset, vtable, scalars, vector length +//! and alignment. About 80 in practice. +inline constexpr size_t TRACKER_METADATA_CHUNK_OVERHEAD = 128; + +//! Collection the metadata of the tracker reading `collection_id` is pushed on. +inline std::string tracker_metadata_collection_id(std::string_view collection_id) +{ + return std::string(collection_id) + "/metadata"; +} + +//! Name of the one tensor in a metadata collection. +inline constexpr std::string_view TRACKER_METADATA_TENSOR_IDENTIFIER = "metadata"; + +//! FNV-1a 64 over `bytes`. +inline uint64_t tracker_metadata_hash(std::span bytes) +{ + uint64_t hash = 0xcbf29ce484222325ULL; + for (const uint8_t byte : bytes) + { + hash ^= byte; + hash *= 0x100000001b3ULL; + } + return hash; +} + +/*! + * @brief Splits an encoded metadata buffer into chunks that each encode to at most `max_chunk_size` bytes. + * @throws std::invalid_argument when `metadata` is empty or larger than TRACKER_METADATA_MAX_SIZE, + * or when `max_chunk_size` is outside the protocol's supported range. + */ +inline std::vector> split_tracker_metadata( + std::span metadata, size_t max_chunk_size = TRACKER_METADATA_CHUNK_SIZE) +{ + if (metadata.empty() || metadata.size() > TRACKER_METADATA_MAX_SIZE) + { + throw std::invalid_argument("split_tracker_metadata: metadata size " + std::to_string(metadata.size()) + + " is outside 1.." + std::to_string(TRACKER_METADATA_MAX_SIZE)); + } + if (max_chunk_size <= TRACKER_METADATA_CHUNK_OVERHEAD) + { + throw std::invalid_argument("split_tracker_metadata: max_chunk_size must exceed " + + std::to_string(TRACKER_METADATA_CHUNK_OVERHEAD)); + } + if (max_chunk_size > TRACKER_METADATA_CHUNK_SIZE) + { + throw std::invalid_argument("split_tracker_metadata: max_chunk_size must not exceed " + + std::to_string(TRACKER_METADATA_CHUNK_SIZE)); + } + + const size_t capacity = max_chunk_size - TRACKER_METADATA_CHUNK_OVERHEAD; + const auto count = static_cast((metadata.size() - 1) / capacity + 1); + const uint64_t hash = tracker_metadata_hash(metadata); + + std::vector> chunks; + chunks.reserve(count); + flatbuffers::FlatBufferBuilder builder(max_chunk_size); + for (uint32_t index = 0; index < count; ++index) + { + const size_t offset = index * capacity; + const size_t size = std::min(capacity, metadata.size() - offset); + const auto bytes = builder.CreateVector(metadata.data() + offset, size); + builder.Finish(CreateTrackerMetadataChunk( + builder, hash, static_cast(metadata.size()), index, count, static_cast(offset), bytes)); + assert(builder.GetSize() <= max_chunk_size && "TRACKER_METADATA_CHUNK_OVERHEAD is too small"); + chunks.push_back(Serialized::adopt(builder)); + } + return chunks; +} + +/*! + * @brief Reassembles a metadata buffer from chunks that arrive in any order and repeat. + * + * A chunk whose hash, total size or chunk count differs from the assembly in progress belongs to + * other metadata and restarts the assembly. The finished buffer is checked against its hash. + */ +class TrackerMetadataAssembler +{ +public: + enum class Status + { + Pending, + Complete, + Rejected + }; + + //! Adds one chunk. After Complete, take() returns the buffer; after Rejected, rejection() says why. + Status add(const TrackerMetadataChunk& chunk) + { + const auto* bytes = chunk.bytes(); + const uint32_t total = chunk.total_size(); + const uint32_t count = chunk.chunk_count(); + if (bytes == nullptr || bytes->size() == 0 || total > TRACKER_METADATA_MAX_SIZE || count == 0 || + count > total || chunk.chunk_index() >= count || uint64_t{ chunk.offset() } + bytes->size() > total) + { + return reject("chunk header does not fit its payload"); + } + + if (chunk.metadata_hash() != hash_ || total != bytes_.size() || count != received_.size()) + { + hash_ = chunk.metadata_hash(); + bytes_.assign(total, 0); + received_.assign(count, false); + missing_ = count; + } + + if (!received_[chunk.chunk_index()]) + { + std::copy(bytes->begin(), bytes->end(), bytes_.begin() + chunk.offset()); + received_[chunk.chunk_index()] = true; + --missing_; + } + if (missing_ > 0) + { + return Status::Pending; + } + if (tracker_metadata_hash(bytes_) != hash_) + { + reset(); + return reject("reassembled metadata does not match its hash"); + } + return Status::Complete; + } + + //! The buffer the last add() completed. Leaves the assembler empty. + std::vector take() + { + std::vector out = std::move(bytes_); + reset(); + return out; + } + + //! Drops the assembly in progress. + void reset() + { + hash_ = 0; + bytes_.clear(); + received_.clear(); + missing_ = 0; + } + + const std::string& rejection() const + { + return rejection_; + } + +private: + Status reject(std::string reason) + { + rejection_ = std::move(reason); + return Status::Rejected; + } + + uint64_t hash_ = 0; + std::vector bytes_; + std::vector received_; + uint32_t missing_ = 0; + std::string rejection_; +}; + +} // namespace core diff --git a/src/core/schema/fbs/tactile.fbs b/src/core/schema/fbs/tactile.fbs new file mode 100644 index 0000000000..c73745c585 --- /dev/null +++ b/src/core/schema/fbs/tactile.fbs @@ -0,0 +1,23 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "point.fbs"; +include "timestamp.fbs"; + +namespace core; + +// Per-frame tactile reading. The taxel layout it indexes is TactileShape (tactile_shape.fbs), +// the tactile tracker's metadata. +table TactileData { + // Force vector per taxel, in TactileShape taxel order. + taxel_force: [Point] (id: 0); +} + +// MCAP recording wrapper. Record types are the root types written to MCAP +// channels; trackers serialize into Record types in serialize_all(). +table TactileDataRecord { + data: TactileData (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type TactileDataRecord; diff --git a/src/core/schema/fbs/tactile_shape.fbs b/src/core/schema/fbs/tactile_shape.fbs new file mode 100644 index 0000000000..d4f5fb79d6 --- /dev/null +++ b/src/core/schema/fbs/tactile_shape.fbs @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "pose.fbs"; + +namespace core; + +// One joint of the hand skeleton at the reference bind pose. +struct TactileJoint { + // Pose in hand-root space. Position in meters, unit quaternion. + bind_pose: Pose; + + // Parent joint ordinal; the root holds 0x7FFFFFFF (XR_HAND_JOINT_MAX_ENUM_EXT). + // The hierarchy is acyclic. + parent: uint32; +} + +// One taxel at the reference bind pose, in hand-root space. +struct TactileTaxel { + // Position in meters; unit normal; unit tangent perpendicular to the normal. + position: Point; + normal: Point; + tangent: Point; + + // Linear blend skinning over four slots: joint_indices[k], an XrHandJointEXT ordinal (HandJoint + // in hand.fbs), weighted by joint_weights[k]. Every slot names a valid joint, zero-weight slots + // included. Weights are finite and nonnegative, 1-4 of them nonzero, summing to 1. A taxel moves as + // p_current = sum_k w_k * T_current[j_k] * inverse(T_bind[j_k]) * p_bind + // with T_bind from the joint's bind_pose and T_current the current joint-to-hand-root + // transform, both rigid. It blends transformed points, not poses. + joint_indices: [uint32:4]; + joint_weights: [float:4]; +} + +// Hand skeleton and taxel layout at the reference bind pose, for the device that publishes +// TactileData. `joints` is indexed by XrHandJointEXT ordinal; `taxels` is in +// TactileData.taxel_force order. +// +// Published as the metadata of the tactile tracker (`metadata` in trackers.toml), not as +// samples: it carries no timestamp, and the tracker holds one shape for the whole session. +table TactileShape { + joints: [TactileJoint] (id: 0); + taxels: [TactileTaxel] (id: 1); +} + +root_type TactileShape; diff --git a/src/core/schema/fbs/tracker_metadata.fbs b/src/core/schema/fbs/tracker_metadata.fbs new file mode 100644 index 0000000000..a618114bb8 --- /dev/null +++ b/src/core/schema/fbs/tracker_metadata.fbs @@ -0,0 +1,26 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +namespace core; + +// One slice of a tracker's metadata buffer. A producer splits the encoded metadata table +// into chunks and pushes them, cyclically, on the tracker's metadata collection; see +// schema/tracker_metadata.hpp for the collection name and the reassembly rules. +table TrackerMetadataChunk { + // FNV-1a 64 of the whole metadata buffer. Chunks with different hashes belong to + // different metadata, and the reassembled buffer must hash to this value. + metadata_hash: uint64 (id: 0); + + // Byte size of the whole metadata buffer. + total_size: uint32 (id: 1); + + chunk_index: uint32 (id: 2); + chunk_count: uint32 (id: 3); + + // Byte offset of `bytes` within the whole metadata buffer. + offset: uint32 (id: 4); + + bytes: [ubyte] (id: 5); +} + +root_type TrackerMetadataChunk; diff --git a/src/core/schema/golden/tactile.bfbs b/src/core/schema/golden/tactile.bfbs new file mode 100644 index 0000000000..e476e46c4b Binary files /dev/null and b/src/core/schema/golden/tactile.bfbs differ diff --git a/src/core/schema/golden/tactile_shape.bfbs b/src/core/schema/golden/tactile_shape.bfbs new file mode 100644 index 0000000000..7d26712353 Binary files /dev/null and b/src/core/schema/golden/tactile_shape.bfbs differ diff --git a/src/core/schema/golden/tracker_metadata.bfbs b/src/core/schema/golden/tracker_metadata.bfbs new file mode 100644 index 0000000000..904f9c7abf Binary files /dev/null and b/src/core/schema/golden/tracker_metadata.bfbs differ diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index d8fc43bfd8..1af41e00a4 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -27,6 +27,9 @@ pybind11_add_module(schema_py schema_module.cpp spacemouse_bindings.h steering_wheel_bindings.h + tactile_bindings.h + tactile_shape_bindings.h + tracker_metadata_bindings.h ) target_link_libraries(schema_py diff --git a/src/core/schema/python/schema_array_views.h b/src/core/schema/python/schema_array_views.h index 07e376ba1c..2d5ec5c130 100644 --- a/src/core/schema/python/schema_array_views.h +++ b/src/core/schema/python/schema_array_views.h @@ -1,7 +1,7 @@ // SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 -// Helpers for exposing fixed-size FlatBuffers struct arrays to Python as NumPy views. +// Helpers for exposing FlatBuffers struct arrays and vectors to Python as NumPy views. // // A FlatBuffers struct array is interleaved (array of structs), so a field is exposed as a // strided view rather than a packed buffer; NumPy and DLPack both carry strides natively, @@ -27,6 +27,7 @@ #include #include #include +#include namespace py = pybind11; @@ -115,4 +116,37 @@ inline py::array_t strided_field_view( return py::array_t({ count }, { stride }, first, owner); } +// (count, Width) view over a FlatBuffers vector of structs that are exactly Width packed T +// (Point as 3 floats, say). FlatBuffers omits an empty vector, so absent reads as (0, Width). +template +inline py::array_t struct_vector_view(py::object owner, const ::flatbuffers::Vector* values) +{ + static_assert(sizeof(StructT) == sizeof(T) * Width, "struct must be exactly Width packed elements"); + if (values == nullptr || values->size() == 0) + { + return py::array_t(std::vector{ 0, Width }); + } + return strided_field_view(owner, reinterpret_cast(values->Data()), + static_cast(sizeof(StructT)), values->size(), Width); +} + +// Rows of a (count, Width) array as structs that are exactly Width packed T, for a table +// constructor. Raises ValueError naming `field` on any other shape. +template +inline std::vector structs_from_rows(const py::array_t& rows, + const char* field) +{ + static_assert(sizeof(StructT) == sizeof(T) * Width, "struct must be exactly Width packed elements"); + if (rows.ndim() != 2 || rows.shape(1) != Width) + { + throw py::value_error(std::string(field) + ": expected an (N, " + std::to_string(Width) + ") array"); + } + std::vector out(static_cast(rows.shape(0))); + if (!out.empty()) + { + std::memcpy(static_cast(out.data()), rows.data(), out.size() * sizeof(StructT)); + } + return out; +} + } // namespace core diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index c76db6cc8a..d1d36d787b 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -28,7 +28,10 @@ #include "soma_hand_joint_rotations_bindings.h" #include "spacemouse_bindings.h" #include "steering_wheel_bindings.h" +#include "tactile_bindings.h" +#include "tactile_shape_bindings.h" #include "timestamp_bindings.h" +#include "tracker_metadata_bindings.h" namespace py = pybind11; @@ -60,6 +63,13 @@ PYBIND11_MODULE(_schema, m) // Bind OGLO tactile glove types (OgloGloveSample table). core::bind_oglo_tactile(m); + // Bind tactile types (TactileData, TactileShape tables). + core::bind_tactile(m); + core::bind_tactile_shape(m); + + // Bind the chunking helpers for producers of tracker metadata. + core::bind_tracker_metadata(m); + // Bind joint-state types (JointState, JointStateOutput tables) for generic joint-space devices. core::bind_joint_state(m); diff --git a/src/core/schema/python/schema_serialized.h b/src/core/schema/python/schema_serialized.h index 3fcb8a5855..4f45fecb49 100644 --- a/src/core/schema/python/schema_serialized.h +++ b/src/core/schema/python/schema_serialized.h @@ -40,6 +40,19 @@ template py::class_> serialized_class(py::module& m, const char* name, const char* doc) { return py::class_>(m, name, doc) + .def_static( + "from_bytes", + [](py::bytes payload) + { + const std::string encoded = payload; + flatbuffers::Verifier verifier(reinterpret_cast(encoded.data()), encoded.size()); + if (!verifier.VerifyBuffer(nullptr)) + { + throw py::value_error("payload is not a valid FlatBuffer for this schema type"); + } + return Serialized::adopt(std::vector(encoded.begin(), encoded.end())); + }, + py::arg("payload"), "Validate and adopt standalone FlatBuffer bytes rooted at this table type.") .def( "to_bytes", [](const Serialized& self) diff --git a/src/core/schema/python/tactile_bindings.h b/src/core/schema/python/tactile_bindings.h new file mode 100644 index 0000000000..1d83b19ee8 --- /dev/null +++ b/src/core/schema/python/tactile_bindings.h @@ -0,0 +1,59 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the TactileData FlatBuffer schema (see tactile.fbs). +// Types: TactileData (table) and TactileDataRecord, exposed as encoded views. + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include +#include +#include + +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_tactile(py::module& m) +{ + using FloatRows = py::array_t; + + serialized_class(m, "TactileData", "Encoded per-frame tactile reading.") + .def(py::init( + [](const std::optional& taxel_force) + { + TactileDataT native; + if (taxel_force.has_value()) + { + native.taxel_force = structs_from_rows(*taxel_force, "taxel_force"); + } + return pack(native); + }), + py::arg("taxel_force") = py::none(), "Encode a tactile frame. taxel_force is an (N, 3) array.") + .def_property_readonly( + "taxel_force", + [](py::object self) + { + const auto& view = self.cast&>(); + return struct_vector_view(self, view->taxel_force()); + }, + "Per-taxel force as an (N, 3) float32 view aliasing this object's buffer; call .copy() to own it.") + .def("__repr__", + [](const Serialized& self) + { + const auto* forces = self->taxel_force(); + return "TactileData(taxels=" + std::to_string(forces != nullptr ? forces->size() : 0) + ")"; + }); + + bind_record(m, "TactileDataRecord", "TactileData"); +} + +} // namespace core diff --git a/src/core/schema/python/tactile_shape_bindings.h b/src/core/schema/python/tactile_shape_bindings.h new file mode 100644 index 0000000000..2a2661840a --- /dev/null +++ b/src/core/schema/python/tactile_shape_bindings.h @@ -0,0 +1,218 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the TactileShape FlatBuffer schema (see tactile_shape.fbs), exposed as an +// encoded view. Each joint and taxel attribute reads as a zero-copy NumPy view strided over the +// TactileJoint / TactileTaxel structs; see schema_array_views.h for the aliasing caveat. + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +// TactileJoint.parent of a root joint: XR_HAND_JOINT_MAX_ENUM_EXT. +constexpr uint32_t TACTILE_ROOT_PARENT = 0x7FFFFFFF; + +inline void bind_tactile_shape(py::module& m) +{ + using FloatRows = py::array_t; + using IndexRows = py::array_t; + + constexpr auto joint_stride = static_cast(sizeof(TactileJoint)); + constexpr auto taxel_stride = static_cast(sizeof(TactileTaxel)); + + // (count, width) view of one field across a vector of structs; absent reads as (0, width). + auto struct_field_view = [](py::object self, const auto* first, py::ssize_t stride, size_t count, py::ssize_t width) + { + using T = std::remove_cv_t>; + if (count == 0) + { + return width > 0 ? py::array_t(std::vector{ 0, width }) : py::array_t(py::ssize_t{ 0 }); + } + return strided_field_view(self, first, stride, static_cast(count), width); + }; + auto joints_of = [](py::object self) { return self.cast&>()->joints(); }; + auto taxels_of = [](py::object self) { return self.cast&>()->taxels(); }; + + auto shape = serialized_class(m, "TactileShape", "Encoded tactile skeleton and taxel layout."); + shape + .def(py::init( + [](const FloatRows& joint_bind_positions, const FloatRows& joint_bind_orientations, + const IndexRows& joint_parents, const FloatRows& taxel_positions, const FloatRows& taxel_normals, + const FloatRows& taxel_tangents, const IndexRows& taxel_joint_indices, + const FloatRows& taxel_joint_weights) + { + const auto positions = + structs_from_rows(joint_bind_positions, "joint_bind_positions"); + const auto orientations = + structs_from_rows(joint_bind_orientations, "joint_bind_orientations"); + if (joint_parents.ndim() != 1) + { + throw py::value_error("joint_parents: expected an (J,) array"); + } + const size_t joints = positions.size(); + if (orientations.size() != joints || static_cast(joint_parents.shape(0)) != joints) + { + throw py::value_error( + "joint_bind_positions, joint_bind_orientations and joint_parents must have the same length"); + } + + TactileShapeT native; + native.joints.reserve(joints); + for (size_t j = 0; j < joints; ++j) + { + const uint32_t parent = joint_parents.data()[j]; + if (parent != TACTILE_ROOT_PARENT && parent >= joints) + { + throw py::value_error("joint_parents: " + std::to_string(parent) + + " is neither a joint index nor ROOT_PARENT"); + } + native.joints.emplace_back(Pose(positions[j], orientations[j]), parent); + } + + const auto points = structs_from_rows(taxel_positions, "taxel_positions"); + const auto normals = structs_from_rows(taxel_normals, "taxel_normals"); + const auto tangents = structs_from_rows(taxel_tangents, "taxel_tangents"); + const auto indices = structs_from_rows, uint32_t, 4>( + taxel_joint_indices, "taxel_joint_indices"); + const auto weights = + structs_from_rows, float, 4>(taxel_joint_weights, "taxel_joint_weights"); + const size_t taxels = points.size(); + if (normals.size() != taxels || tangents.size() != taxels || indices.size() != taxels || + weights.size() != taxels) + { + throw py::value_error("every taxel_* array must have the same length"); + } + native.taxels.reserve(taxels); + for (size_t t = 0; t < taxels; ++t) + { + for (uint32_t joint : indices[t]) + { + if (joint >= joints) + { + throw py::value_error("taxel_joint_indices: " + std::to_string(joint) + + " is not a joint index"); + } + } + native.taxels.emplace_back(points[t], normals[t], tangents[t], + ::flatbuffers::span(indices[t].data(), 4), + ::flatbuffers::span(weights[t].data(), 4)); + } + return pack(native); + }), + py::arg("joint_bind_positions"), py::arg("joint_bind_orientations"), py::arg("joint_parents"), + py::arg("taxel_positions"), py::arg("taxel_normals"), py::arg("taxel_tangents"), + py::arg("taxel_joint_indices"), py::arg("taxel_joint_weights"), + "Encode a tactile shape. Joint arrays are (J, 3), (J, 4) and (J,); taxel arrays are (N, 3) or " + "(N, 4). Mismatched lengths and out-of-range joint indices raise ValueError; weights are not " + "checked.") + .def_property_readonly( + "joint_bind_positions", + [=](py::object self) + { + const auto* joints = joints_of(self); + const size_t count = joints != nullptr ? joints->size() : 0; + const float* first = + count > 0 ? reinterpret_cast(&joints->Get(0)->bind_pose().position()) : nullptr; + return struct_field_view(self, first, joint_stride, count, 3); + }, + "Joint bind positions as a (J, 3) float32 strided view.") + .def_property_readonly( + "joint_bind_orientations", + [=](py::object self) + { + const auto* joints = joints_of(self); + const size_t count = joints != nullptr ? joints->size() : 0; + const float* first = + count > 0 ? reinterpret_cast(&joints->Get(0)->bind_pose().orientation()) : nullptr; + return struct_field_view(self, first, joint_stride, count, 4); + }, + "Joint bind orientations (XYZW) as a (J, 4) float32 strided view.") + .def_property_readonly( + "joint_parents", + [=, offset = FBS_FIELD_OFFSET(TactileJoint, parent)](py::object self) + { + const auto* joints = joints_of(self); + const size_t count = joints != nullptr ? joints->size() : 0; + const uint32_t* first = count > 0 ? fbs_field_address(*joints->Get(0), offset) : nullptr; + return struct_field_view(self, first, joint_stride, count, 0); + }, + "Parent joint per joint as a (J,) uint32 strided view; ROOT_PARENT marks a root.") + .def_property_readonly( + "taxel_positions", + [=](py::object self) + { + const auto* taxels = taxels_of(self); + const size_t count = taxels != nullptr ? taxels->size() : 0; + const float* first = count > 0 ? reinterpret_cast(&taxels->Get(0)->position()) : nullptr; + return struct_field_view(self, first, taxel_stride, count, 3); + }, + "Taxel bind positions as an (N, 3) float32 strided view.") + .def_property_readonly( + "taxel_normals", + [=](py::object self) + { + const auto* taxels = taxels_of(self); + const size_t count = taxels != nullptr ? taxels->size() : 0; + const float* first = count > 0 ? reinterpret_cast(&taxels->Get(0)->normal()) : nullptr; + return struct_field_view(self, first, taxel_stride, count, 3); + }, + "Taxel unit normals as an (N, 3) float32 strided view.") + .def_property_readonly( + "taxel_tangents", + [=](py::object self) + { + const auto* taxels = taxels_of(self); + const size_t count = taxels != nullptr ? taxels->size() : 0; + const float* first = count > 0 ? reinterpret_cast(&taxels->Get(0)->tangent()) : nullptr; + return struct_field_view(self, first, taxel_stride, count, 3); + }, + "Taxel unit tangents as an (N, 3) float32 strided view.") + .def_property_readonly( + "taxel_joint_indices", + [=](py::object self) + { + const auto* taxels = taxels_of(self); + const size_t count = taxels != nullptr ? taxels->size() : 0; + const uint32_t* first = count > 0 ? taxels->Get(0)->joint_indices()->data() : nullptr; + return struct_field_view(self, first, taxel_stride, count, 4); + }, + "Skinning joint per slot as an (N, 4) uint32 strided view.") + .def_property_readonly( + "taxel_joint_weights", + [=](py::object self) + { + const auto* taxels = taxels_of(self); + const size_t count = taxels != nullptr ? taxels->size() : 0; + const float* first = count > 0 ? taxels->Get(0)->joint_weights()->data() : nullptr; + return struct_field_view(self, first, taxel_stride, count, 4); + }, + "Skinning weight per slot as an (N, 4) float32 strided view.") + .def("__repr__", + [](const Serialized& self) + { + const auto* joints = self->joints(); + const auto* taxels = self->taxels(); + return "TactileShape(joints=" + std::to_string(joints != nullptr ? joints->size() : 0) + + ", taxels=" + std::to_string(taxels != nullptr ? taxels->size() : 0) + ")"; + }); + shape.attr("ROOT_PARENT") = TACTILE_ROOT_PARENT; +} + +} // namespace core diff --git a/src/core/schema/python/tracker_metadata_bindings.h b/src/core/schema/python/tracker_metadata_bindings.h new file mode 100644 index 0000000000..26a20f2a59 --- /dev/null +++ b/src/core/schema/python/tracker_metadata_bindings.h @@ -0,0 +1,52 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python helpers for producers of tracker metadata (see schema/tracker_metadata.hpp). A Python +// producer pushes the chunks with a TensorPushTracker on tracker_metadata_collection_id(), a few +// per frame, in a loop. + +#pragma once + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_tracker_metadata(py::module& m) +{ + m.attr("TRACKER_METADATA_CHUNK_SIZE") = TRACKER_METADATA_CHUNK_SIZE; + m.attr("TRACKER_METADATA_TENSOR_IDENTIFIER") = std::string(TRACKER_METADATA_TENSOR_IDENTIFIER); + + m.def( + "tracker_metadata_collection_id", + [](const std::string& collection_id) { return tracker_metadata_collection_id(collection_id); }, + py::arg("collection_id"), "Collection the metadata of the tracker reading `collection_id` is pushed on."); + + m.def( + "split_tracker_metadata", + [](const py::bytes& metadata, size_t max_chunk_size) + { + const std::string bytes = metadata; + const auto chunks = split_tracker_metadata( + std::span(reinterpret_cast(bytes.data()), bytes.size()), max_chunk_size); + py::list out; + for (const auto& chunk : chunks) + { + const auto encoded = chunk.buffer(); + out.append(py::bytes(reinterpret_cast(encoded.data()), encoded.size())); + } + return out; + }, + py::arg("metadata"), py::arg("max_chunk_size") = TRACKER_METADATA_CHUNK_SIZE, + "Split an encoded metadata table (e.g. TactileShape.to_bytes()) into chunk payloads of at most " + "max_chunk_size bytes each. Raises ValueError on an empty or oversized table."); +} + +} // namespace core diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py index e0c0deabd3..ecba0847fb 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py @@ -41,6 +41,12 @@ messageChannelConfig, ) from .haptic_sink import HapticSink +from .tactile_source import ( + TactileForces, + TactileShapeIndex, + TactileShapeLayout, + TactileSource, +) from .deviceio_tensor_types import ( HeadPoseTrackedType, HandPoseTrackedType, @@ -54,6 +60,8 @@ SomaBodyJointPosesTrackedType, SomaHandJointRotationsTrackedType, SomaHandJointPosesTrackedType, + TactileDataTrackedType, + TactileShapeTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, @@ -66,6 +74,7 @@ DeviceIOSomaBodyJointPosesTracked, DeviceIOSomaHandJointRotationsTracked, DeviceIOSomaHandJointPosesTracked, + DeviceIOTactileTracked, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, MessageChannelStatusType, @@ -106,6 +115,10 @@ "message_channel_config", "messageChannelConfig", "HapticSink", + "TactileSource", + "TactileShapeIndex", + "TactileForces", + "TactileShapeLayout", "HeadPoseTrackedType", "HandPoseTrackedType", "ControllerSnapshotTrackedType", @@ -118,6 +131,8 @@ "SomaBodyJointPosesTrackedType", "SomaHandJointRotationsTrackedType", "SomaHandJointPosesTrackedType", + "TactileDataTrackedType", + "TactileShapeTrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", "MessageChannelStatusType", @@ -133,6 +148,7 @@ "DeviceIOSomaBodyJointPosesTracked", "DeviceIOSomaHandJointRotationsTracked", "DeviceIOSomaHandJointPosesTracked", + "DeviceIOTactileTracked", "DeviceIOMessageChannelMessagesTracked", "MessageChannelMessagesTrackedGroup", "MessageChannelStatusGroup", diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py index 474d11bedf..d9fbdaca0f 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/deviceio_tensor_types.py @@ -28,6 +28,8 @@ SomaHandJointPoses, SomaHandJointRotations, MessageChannelMessagesTracked, + TactileData, + TactileShape, ) @@ -150,6 +152,18 @@ class SomaHandJointPosesTrackedType(_PayloadTensorType): _payload_cls = SomaHandJointPoses +class TactileDataTrackedType(_PayloadTensorType): + """TactileData payload from DeviceIO TactileTracker.""" + + _payload_cls = TactileData + + +class TactileShapeTrackedType(_PayloadTensorType): + """TactileShape metadata from DeviceIO TactileTracker.""" + + _payload_cls = TactileShape + + class MessageChannelMessagesTrackedType(_RequiredPayloadTensorType): """MessageChannelMessagesTracked batch from DeviceIO MessageChannelTracker.""" @@ -294,6 +308,22 @@ def DeviceIOSomaHandJointPosesTracked() -> TensorGroupType: ) +def DeviceIOTactileTracked() -> TensorGroupType: + """Tracked tactile frame and its shape from DeviceIO TactileTracker. + + Contains: + tactile_tracked: TactileData handle, or None when inactive + tactile_shape: TactileShape metadata handle, or None until it arrives + """ + return TensorGroupType( + "deviceio_tactile", + [ + TactileDataTrackedType("tactile_tracked"), + TactileShapeTrackedType("tactile_shape"), + ], + ) + + def DeviceIOMessageChannelMessagesTracked() -> TensorGroupType: """Message batch from DeviceIO MessageChannelTracker.""" return TensorGroupType( diff --git a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/tactile_source.py b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/tactile_source.py new file mode 100644 index 0000000000..b82955c33e --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/tactile_source.py @@ -0,0 +1,177 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Tactile Source Node - DeviceIO to Retargeting Engine converter. + +``TactileSource`` turns per-frame ``TactileData`` into a force array, and the tracker's +``TactileShape`` metadata into the arrays that lay those forces out. Taxel and joint counts are +fixed at construction so the retargeting graph has a static spec. +""" + +from __future__ import annotations + +from enum import IntEnum +from typing import Any, Optional, TYPE_CHECKING + +import numpy as np + +from .interface import IDeviceIOSource +from ..interface.retargeter_core_types import RetargeterIO, RetargeterIOType +from ..interface.tensor_group import TensorGroup +from ..interface.tensor_group_type import OptionalType, TensorGroupType +from ..tensor_types import DLDataType, NDArrayType +from .deviceio_tensor_types import DeviceIOTactileTracked + +if TYPE_CHECKING: + from isaaccapture.deviceio_trackers import ITracker + from isaaccapture.schema import TactileData, TactileShape + + +def _float_array(name: str, rows: int, width: int) -> NDArrayType: + return NDArrayType(name, shape=(rows, width), dtype=DLDataType.FLOAT, dtype_bits=32) + + +def _int_array(name: str, shape: tuple[int, ...]) -> NDArrayType: + return NDArrayType(name, shape=shape, dtype=DLDataType.INT, dtype_bits=32) + + +def TactileForces(num_taxels: int) -> TensorGroupType: + """Per-taxel force vectors: one ``(num_taxels, 3)`` float32 array.""" + return TensorGroupType( + f"tactile_forces_{num_taxels}", + [_float_array("taxel_force", num_taxels, 3)], + ) + + +class TactileShapeIndex(IntEnum): + """Positions of the arrays in a :func:`TactileShapeLayout` group.""" + + JOINT_BIND_POSITIONS = 0 + JOINT_BIND_ORIENTATIONS = 1 + JOINT_PARENTS = 2 + TAXEL_POSITIONS = 3 + TAXEL_NORMALS = 4 + TAXEL_TANGENTS = 5 + TAXEL_JOINT_INDICES = 6 + TAXEL_JOINT_WEIGHTS = 7 + + +def TactileShapeLayout(num_joints: int, num_taxels: int) -> TensorGroupType: + """``TactileShape`` as arrays, ordered by :class:`TactileShapeIndex`. + + Each array is named after the ``TactileShape`` property it is read from. Joint parents and + joint indices are int32; the root parent stays ``TactileShape.ROOT_PARENT``. + """ + return TensorGroupType( + f"tactile_shape_{num_joints}x{num_taxels}", + [ + _float_array("joint_bind_positions", num_joints, 3), + _float_array("joint_bind_orientations", num_joints, 4), + _int_array("joint_parents", (num_joints,)), + _float_array("taxel_positions", num_taxels, 3), + _float_array("taxel_normals", num_taxels, 3), + _float_array("taxel_tangents", num_taxels, 3), + _int_array("taxel_joint_indices", (num_taxels, 4)), + _float_array("taxel_joint_weights", num_taxels, 4), + ], + ) + + +class TactileSource(IDeviceIOSource): + """Converter: DeviceIO ``TactileTracker`` -> per-taxel forces and the shape that lays them out. + + Outputs (Optional): + - :data:`FORCES`: :func:`TactileForces` of ``num_taxels``, in ``TactileShape`` taxel + order; absent when the device is inactive. + - :data:`SHAPE`: :func:`TactileShapeLayout` of ``num_joints`` x ``num_taxels``; absent + until the tracker holds the device's shape. The tracker publishes no forces before then. + + A frame or shape whose counts differ from the constructor's raises ``ValueError``. + """ + + FORCES = "taxel_forces" + SHAPE = "tactile_shape" + + def __init__( + self, + name: str, + collection_id: str, + num_joints: int, + num_taxels: int, + max_flatbuffer_size: Optional[int] = None, + ) -> None: + """Initialize the tactile source. + + Args: + name: Unique name for this source node. + collection_id: Tensor collection ID of the producer's ``TactileData`` stream. + num_joints: Joint count of the device skeleton; fixes the shape output spec. + num_taxels: Taxel count of the device; fixes the output specs. + max_flatbuffer_size: Override for ``TactileTracker``'s default collection size. + """ + from isaaccapture.deviceio_trackers import TactileTracker + + self._tracker = ( + TactileTracker(collection_id) + if max_flatbuffer_size is None + else TactileTracker(collection_id, max_flatbuffer_size) + ) + self._num_joints = int(num_joints) + self._num_taxels = int(num_taxels) + super().__init__(name) + + def get_tracker(self) -> "ITracker": + """Return the ``TactileTracker`` instance for ``TeleopSession`` to initialize.""" + return self._tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + """Poll the tracker's frame and metadata for the compute step.""" + tg = TensorGroup(DeviceIOTactileTracked()) + tg[0] = self._tracker.get_data(deviceio_session) + tg[1] = self._tracker.get_metadata(deviceio_session) + return {"deviceio_tactile": tg} + + def input_spec(self) -> RetargeterIOType: + """Declare the raw DeviceIO tactile input.""" + return {"deviceio_tactile": DeviceIOTactileTracked()} + + def output_spec(self) -> RetargeterIOType: + """Declare the force and shape outputs (Optional -- may be absent).""" + return { + self.FORCES: OptionalType(TactileForces(self._num_taxels)), + self.SHAPE: OptionalType( + TactileShapeLayout(self._num_joints, self._num_taxels) + ), + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Copy the forces and shape out, or call ``set_none()`` on whichever is absent.""" + data: "TactileData | None" = inputs["deviceio_tactile"][0] + shape: "TactileShape | None" = inputs["deviceio_tactile"][1] + + forces_out = outputs[self.FORCES] + if data is None: + forces_out.set_none() + else: + forces = data.taxel_force + if len(forces) != self._num_taxels: + raise ValueError( + f"{self.name}: frame has {len(forces)} taxels, source expects {self._num_taxels}" + ) + forces_out[0] = forces + + shape_out = outputs[self.SHAPE] + if shape is None: + shape_out.set_none() + return + joints, taxels = len(shape.joint_parents), len(shape.taxel_positions) + if (joints, taxels) != (self._num_joints, self._num_taxels): + raise ValueError( + f"{self.name}: shape has {joints} joints and {taxels} taxels, " + f"source expects {self._num_joints} and {self._num_taxels}" + ) + for index, tensor_type in enumerate(shape_out.group_type.types): + value = getattr(shape, tensor_type.name) + if value.dtype == np.uint32: + value = value.astype(np.int32) + shape_out[index] = value diff --git a/src/python/isaaccapture/schema/__init__.py b/src/python/isaaccapture/schema/__init__.py index f1e7d1348d..4cc587961a 100644 --- a/src/python/isaaccapture/schema/__init__.py +++ b/src/python/isaaccapture/schema/__init__.py @@ -52,6 +52,15 @@ # OGLO tactile glove types. OgloGloveSample, OgloGloveSampleRecord, + # Tactile types. + TactileData, + TactileDataRecord, + TactileShape, + # Tracker metadata chunking, for producers of a tracker's metadata. + TRACKER_METADATA_CHUNK_SIZE, + TRACKER_METADATA_TENSOR_IDENTIFIER, + tracker_metadata_collection_id, + split_tracker_metadata, # Joint-state types (generic joint-space devices: leader arms, exoskeletons, ...). JointState, JointStateOutput, @@ -180,6 +189,15 @@ def __getattr__(name: str): # OGLO tactile glove types. "OgloGloveSample", "OgloGloveSampleRecord", + # Tactile types. + "TactileData", + "TactileDataRecord", + "TactileShape", + # Tracker metadata chunking. + "TRACKER_METADATA_CHUNK_SIZE", + "TRACKER_METADATA_TENSOR_IDENTIFIER", + "tracker_metadata_collection_id", + "split_tracker_metadata", # Joint-state types (generic joint-space devices). "JointState", "JointStateOutput", diff --git a/tests/cpp/core/replay_deviceio_session/test_replay_session.cpp b/tests/cpp/core/replay_deviceio_session/test_replay_session.cpp index 1b43f72a98..557dab36a3 100644 --- a/tests/cpp/core/replay_deviceio_session/test_replay_session.cpp +++ b/tests/cpp/core/replay_deviceio_session/test_replay_session.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -22,6 +23,8 @@ #include #include #include +#include +#include #include #include @@ -824,3 +827,95 @@ TEST_CASE("ReplaySession: keyboard tracker round-trip ignores live provider inpu session->update(); CHECK_FALSE(keyboard_tracker.get_data(*session)); } + +// ============================================================================= +// Tracker with metadata — TactileTracker (TactileShape on its own channel) +// ============================================================================= + +namespace +{ + +// Live writes the metadata once, before any data it lets through; mirror that order. +void write_tactile_recording(const std::string& path, const std::string& base_name, bool with_metadata) +{ + auto writer = open_writer(path); + core::McapTrackerChannels data( + *writer, base_name, to_string_vec(core::TactileRecordingTraits::recording_channels)); + core::McapTrackerChannels metadata( + *writer, base_name, to_string_vec(core::TactileRecordingTraits::metadata_channels)); + + if (with_metadata) + { + core::TactileShapeT shape; + shape.joints = { core::TactileJoint(make_pose(0, 0, 0), 0x7FFFFFFF) }; + shape.taxels = { core::TactileTaxel(), core::TactileTaxel() }; + metadata.write(0, core::pack(shape), 500); + } + + for (int i = 0; i < 2; ++i) + { + core::TactileDataT frame; + frame.taxel_force = { core::Point(static_cast(i), 0, 0), core::Point(0, 0, 1) }; + const int64_t time_ns = (i + 1) * 1000; + const auto record = + core::pack_record(&frame, core::DeviceDataTimestamp(time_ns, time_ns, time_ns)); + data.write(0, record); + data.write(1, record); + } + writer->close(); +} + +} // namespace + +TEST_CASE("ReplaySession: tracker metadata is replayed before its data", "[replay][session][metadata]") +{ + // Frozen wire strings, as for the record channels above. + CHECK(std::string_view(core::TactileShape::GetFullyQualifiedName()) == "core.TactileShape"); + REQUIRE(core::TactileRecordingTraits::metadata_channels.size() == 1); + CHECK(std::string_view(core::TactileRecordingTraits::metadata_channels[0]) == "tactile_metadata"); + + auto path = get_temp_mcap_path(); + TempFileCleanup cleanup(path); + write_tactile_recording(path, "tactile", true); + + core::TactileTracker tracker("tactile/right"); + core::McapReplayConfig config; + config.filename = path; + config.tracker_names = { { &tracker, "tactile" } }; + auto session = core::ReplaySession::run(config); + + // Read when the session opens, so it is there before the first frame. + const auto& shape = tracker.get_metadata(*session); + REQUIRE(shape); + CHECK(shape->taxels()->size() == 2); + + for (int i = 0; i < 2; ++i) + { + session->update(); + const auto& frame = tracker.get_data(*session); + REQUIRE(frame); + CHECK(frame->taxel_force()->Get(0)->x() == static_cast(i)); + CHECK(tracker.get_metadata(*session).get() == shape.get()); + } + + session->update(); + CHECK_FALSE(tracker.get_data(*session)); + CHECK(tracker.get_metadata(*session)); +} + +TEST_CASE("ReplaySession: data recorded without its metadata is withheld", "[replay][session][metadata]") +{ + auto path = get_temp_mcap_path(); + TempFileCleanup cleanup(path); + write_tactile_recording(path, "tactile", false); + + core::TactileTracker tracker("tactile/right"); + core::McapReplayConfig config; + config.filename = path; + config.tracker_names = { { &tracker, "tactile" } }; + auto session = core::ReplaySession::run(config); + + session->update(); + CHECK_FALSE(tracker.get_metadata(*session)); + CHECK_FALSE(tracker.get_data(*session)); +} diff --git a/tests/cpp/core/schema/CMakeLists.txt b/tests/cpp/core/schema/CMakeLists.txt index 9d66c6a949..facc1bb47c 100644 --- a/tests/cpp/core/schema/CMakeLists.txt +++ b/tests/cpp/core/schema/CMakeLists.txt @@ -21,6 +21,9 @@ add_executable(schema_tests test_soma_body_joint_poses.cpp test_soma_hand_joint_rotations.cpp test_soma_hand_joint_poses.cpp + test_tactile.cpp + test_tactile_shape.cpp + test_tracker_metadata.cpp test_schema_conform.cpp ) target_link_libraries(schema_tests PRIVATE diff --git a/tests/cpp/core/schema/test_tactile.cpp b/tests/cpp/core/schema/test_tactile.cpp new file mode 100644 index 0000000000..3f15e08326 --- /dev/null +++ b/tests/cpp/core/schema/test_tactile.cpp @@ -0,0 +1,70 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for the generated TactileData FlatBuffer message. + +#include +#include +#include +#include +#include +#include + +#include +#include + +// VT_ == (id + 2) * 2. +#define VT(field) (field + 2) * 2 +static_assert(core::TactileData::VT_TAXEL_FORCE == VT(0)); +static_assert(core::TactileDataRecord::VT_DATA == VT(0)); +static_assert(core::TactileDataRecord::VT_TIMESTAMP == VT(1)); + +// Producers copy XrVector3f arrays straight into taxel_force. +static_assert(sizeof(core::Point) == sizeof(XrVector3f)); + +namespace +{ + +core::TactileDataT make_frame() +{ + core::TactileDataT native; + native.taxel_force = { core::Point(0.0f, 0.0f, 1.0f), core::Point(0.5f, -0.5f, 2.0f) }; + return native; +} + +} // namespace + +TEST_CASE("TactileData round-trips every field", "[unit][tactile]") +{ + const auto data = core::pack(make_frame()); + + REQUIRE(data); + REQUIRE(data->taxel_force() != nullptr); + REQUIRE(data->taxel_force()->size() == 2); + CHECK(data->taxel_force()->Get(1)->x() == 0.5f); + CHECK(data->taxel_force()->Get(1)->z() == 2.0f); +} + +TEST_CASE("TactileData leaves an empty frame's vector absent", "[unit][tactile]") +{ + const auto data = core::pack(core::TactileDataT{}); + + REQUIRE(data); + CHECK(data->taxel_force() == nullptr); +} + +TEST_CASE("TactileDataRecord verifies and carries its timestamp", "[unit][tactile]") +{ + core::TactileDataRecordT record; + record.data = std::make_shared(make_frame()); + record.timestamp = std::make_shared(1, 2, 3); + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::TactileDataRecord::Pack(builder, &record)); + + flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); + REQUIRE(core::VerifyTactileDataRecordBuffer(verifier)); + const auto* read = core::GetTactileDataRecord(builder.GetBufferPointer()); + CHECK(read->timestamp()->sample_time_local_common_clock() == 2); + CHECK(read->data()->taxel_force()->size() == 2); +} diff --git a/tests/cpp/core/schema/test_tactile_shape.cpp b/tests/cpp/core/schema/test_tactile_shape.cpp new file mode 100644 index 0000000000..787f5ab7e4 --- /dev/null +++ b/tests/cpp/core/schema/test_tactile_shape.cpp @@ -0,0 +1,77 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for the generated TactileShape FlatBuffer message. + +#include +#include +#include +#include +#include + +#include +#include +#include + +// VT_ == (id + 2) * 2. +#define VT(field) (field + 2) * 2 +static_assert(core::TactileShape::VT_JOINTS == VT(0)); +static_assert(core::TactileShape::VT_TAXELS == VT(1)); + +// Producers copy OpenXR values straight into the structs. +static_assert(sizeof(core::Point) == sizeof(XrVector3f)); +static_assert(sizeof(core::TactileJoint) == sizeof(XrPosef) + sizeof(XrHandJointEXT)); +static_assert(sizeof(core::TactileTaxel) == 3 * sizeof(XrVector3f) + 4 * sizeof(XrHandJointEXT) + sizeof(XrVector4f)); + +namespace +{ + +constexpr uint32_t kRootParent = 0x7FFFFFFF; + +// Two joints (wrist -> index tip) and one taxel skinned to both. +core::TactileShapeT make_shape() +{ + core::TactileShapeT native; + native.joints = { core::TactileJoint(core::Pose(core::Point(0, 0, 0), core::Quaternion(0, 0, 0, 1)), kRootParent), + core::TactileJoint(core::Pose(core::Point(0, 0.1f, 0), core::Quaternion(0, 0, 0, 1)), 0) }; + + const std::array indices = { 1, 0, 0, 0 }; + const std::array weights = { 0.75f, 0.25f, 0.0f, 0.0f }; + native.taxels = { core::TactileTaxel(core::Point(0, 0.09f, 0.01f), core::Point(0, 0, 1), core::Point(1, 0, 0), + ::flatbuffers::span(indices.data(), 4), + ::flatbuffers::span(weights.data(), 4)) }; + return native; +} + +} // namespace + +TEST_CASE("TactileShape round-trips skeleton, taxels and skinning", "[unit][tactile_shape]") +{ + const auto shape = core::pack(make_shape()); + + REQUIRE(shape); + REQUIRE(shape->joints()->size() == 2); + CHECK(shape->joints()->Get(0)->parent() == kRootParent); + CHECK(shape->joints()->Get(1)->parent() == 0); + CHECK(shape->joints()->Get(1)->bind_pose().position().y() == 0.1f); + REQUIRE(shape->taxels()->size() == 1); + const auto* taxel = shape->taxels()->Get(0); + CHECK(taxel->normal().z() == 1.0f); + CHECK(taxel->joint_indices()->Get(0) == 1); + CHECK(taxel->joint_weights()->Get(1) == 0.25f); +} + +TEST_CASE("TactileShape is its own root and verifies", "[unit][tactile_shape]") +{ + const auto shape = core::pack(make_shape()); + const auto bytes = shape.buffer(); + + flatbuffers::Verifier verifier(bytes.data(), bytes.size()); + REQUIRE(core::VerifyTactileShapeBuffer(verifier)); + CHECK(std::string(core::TactileShape::GetFullyQualifiedName()) == "core.TactileShape"); + + core::TactileShapeT unpacked; + core::GetTactileShape(bytes.data())->UnPackTo(&unpacked); + REQUIRE(unpacked.taxels.size() == 1); + CHECK(unpacked.taxels[0].joint_weights()->Get(0) == 0.75f); +} diff --git a/tests/cpp/core/schema/test_tracker_metadata.cpp b/tests/cpp/core/schema/test_tracker_metadata.cpp new file mode 100644 index 0000000000..978e4fa9a9 --- /dev/null +++ b/tests/cpp/core/schema/test_tracker_metadata.cpp @@ -0,0 +1,153 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for tracker metadata chunking: split_tracker_metadata and TrackerMetadataAssembler. + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// VT_ == (id + 2) * 2. +#define VT(field) (field + 2) * 2 +static_assert(core::TrackerMetadataChunk::VT_METADATA_HASH == VT(0)); +static_assert(core::TrackerMetadataChunk::VT_TOTAL_SIZE == VT(1)); +static_assert(core::TrackerMetadataChunk::VT_CHUNK_INDEX == VT(2)); +static_assert(core::TrackerMetadataChunk::VT_CHUNK_COUNT == VT(3)); +static_assert(core::TrackerMetadataChunk::VT_OFFSET == VT(4)); +static_assert(core::TrackerMetadataChunk::VT_BYTES == VT(5)); + +namespace +{ + +using Status = core::TrackerMetadataAssembler::Status; + +std::vector make_metadata(size_t size, uint8_t seed = 0) +{ + std::vector bytes(size); + for (size_t i = 0; i < size; ++i) + { + bytes[i] = static_cast(i * 31 + seed); + } + return bytes; +} + +// A chunk built by hand, for headers split_tracker_metadata() would never produce. +core::Serialized make_chunk( + uint64_t hash, uint32_t total, uint32_t index, uint32_t count, uint32_t offset, const std::vector& bytes) +{ + flatbuffers::FlatBufferBuilder builder; + const auto payload = builder.CreateVector(bytes); + builder.Finish(core::CreateTrackerMetadataChunk(builder, hash, total, index, count, offset, payload)); + return core::Serialized::adopt(builder); +} + +} // namespace + +TEST_CASE("metadata collection sits next to the data collection", "[unit][tracker_metadata]") +{ + CHECK(core::tracker_metadata_collection_id("tactile/right") == "tactile/right/metadata"); +} + +TEST_CASE("split_tracker_metadata keeps every chunk within max_chunk_size", "[unit][tracker_metadata]") +{ + constexpr size_t max_chunk_size = 1024; + constexpr size_t capacity = max_chunk_size - core::TRACKER_METADATA_CHUNK_OVERHEAD; + // One byte short of, exactly, and one byte past a whole number of chunks. + for (const size_t size : { 3 * capacity - 1, 3 * capacity, 3 * capacity + 1 }) + { + const auto metadata = make_metadata(size); + const auto chunks = core::split_tracker_metadata(metadata, max_chunk_size); + + REQUIRE(chunks.size() == (size + capacity - 1) / capacity); + for (const auto& chunk : chunks) + { + CHECK(chunk.buffer().size() <= max_chunk_size); + CHECK(chunk->total_size() == size); + CHECK(chunk->chunk_count() == chunks.size()); + CHECK(chunk->metadata_hash() == core::tracker_metadata_hash(metadata)); + } + } +} + +TEST_CASE("split_tracker_metadata rejects what it cannot carry", "[unit][tracker_metadata]") +{ + CHECK_THROWS_AS(core::split_tracker_metadata({}), std::invalid_argument); + CHECK_THROWS_AS( + core::split_tracker_metadata(make_metadata(10), core::TRACKER_METADATA_CHUNK_OVERHEAD), std::invalid_argument); + CHECK_THROWS_AS( + core::split_tracker_metadata(make_metadata(10), core::TRACKER_METADATA_CHUNK_SIZE + 1), std::invalid_argument); + const std::vector oversized(core::TRACKER_METADATA_MAX_SIZE + 1); + CHECK_THROWS_AS(core::split_tracker_metadata(oversized), std::invalid_argument); +} + +TEST_CASE("assembler restores metadata from chunks in any order, with repeats", "[unit][tracker_metadata]") +{ + const auto metadata = make_metadata(5000); + const auto chunks = core::split_tracker_metadata(metadata, 1024); + REQUIRE(chunks.size() == 6); + + core::TrackerMetadataAssembler assembler; + // Joins mid-loop, sees one chunk twice, then wraps around. + for (const size_t index : { 3, 4, 4, 5, 0, 1 }) + { + CHECK(assembler.add(*chunks[index]) == Status::Pending); + } + REQUIRE(assembler.add(*chunks[2]) == Status::Complete); + CHECK(assembler.take() == metadata); + + // take() leaves it empty: the next pass is assembled from scratch. + CHECK(assembler.add(*chunks[0]) == Status::Pending); +} + +TEST_CASE("assembler restarts on chunks of other metadata", "[unit][tracker_metadata]") +{ + const auto first = core::split_tracker_metadata(make_metadata(3000, 1), 1024); + const auto second_metadata = make_metadata(3000, 2); + const auto second = core::split_tracker_metadata(second_metadata, 1024); + REQUIRE(first.size() == second.size()); + + core::TrackerMetadataAssembler assembler; + for (size_t i = 0; i + 1 < first.size(); ++i) + { + CHECK(assembler.add(*first[i]) == Status::Pending); + } + // The producer changed: the partial first buffer must not be completed with its chunks. + for (size_t i = 0; i + 1 < second.size(); ++i) + { + CHECK(assembler.add(*second[i]) == Status::Pending); + } + REQUIRE(assembler.add(*second.back()) == Status::Complete); + CHECK(assembler.take() == second_metadata); +} + +TEST_CASE("assembler rejects inconsistent chunks", "[unit][tracker_metadata]") +{ + core::TrackerMetadataAssembler assembler; + const std::vector payload(8, 1); + + SECTION("payload past total_size") + { + CHECK(assembler.add(*make_chunk(1, 10, 0, 2, 4, payload)) == Status::Rejected); + } + SECTION("index past chunk_count") + { + CHECK(assembler.add(*make_chunk(1, 16, 2, 2, 0, payload)) == Status::Rejected); + } + SECTION("total_size past the limit") + { + const auto total = static_cast(core::TRACKER_METADATA_MAX_SIZE + 1); + CHECK(assembler.add(*make_chunk(1, total, 0, 1, 0, payload)) == Status::Rejected); + } + SECTION("bytes that do not match the hash") + { + CHECK(assembler.add(*make_chunk(1, 8, 0, 1, 0, payload)) == Status::Rejected); + CHECK(assembler.rejection().find("hash") != std::string::npos); + } +} diff --git a/tests/python/core/retargeting_engine/test_tactile_sources.py b/tests/python/core/retargeting_engine/test_tactile_sources.py new file mode 100644 index 0000000000..068bec1401 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_tactile_sources.py @@ -0,0 +1,146 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for the TactileSource DeviceIO converter. + +Payloads are built through the real schema bindings; no OpenXR device is involved. +""" + +import numpy as np +import pytest + +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + TactileShapeIndex, + TactileShapeLayout, + TactileSource, +) +from isaaccapture.retargeting_engine.interface.base_retargeter import _make_output_group +from isaaccapture.retargeting_engine.interface.tensor_group import TensorGroup +from isaaccapture.schema import TactileData, TactileShape + +NUM_JOINTS = 2 +NUM_TAXELS = 3 + + +def _source(**kwargs) -> TactileSource: + args = dict( + collection_id="tactile/right", num_joints=NUM_JOINTS, num_taxels=NUM_TAXELS + ) + args.update(kwargs) + return TactileSource("tactile", **args) + + +def _inputs(source, data, shape) -> dict: + tg = TensorGroup(source.input_spec()["deviceio_tactile"]) + tg[0] = data + tg[1] = shape + return {"deviceio_tactile": tg} + + +def _outputs(source) -> dict: + return {name: _make_output_group(gt) for name, gt in source.output_spec().items()} + + +def _shape(num_joints: int = NUM_JOINTS, num_taxels: int = NUM_TAXELS) -> TactileShape: + indices = np.zeros((num_taxels, 4), np.uint32) + weights = np.zeros((num_taxels, 4), np.float32) + weights[:, 0] = 1.0 + return TactileShape( + joint_bind_positions=np.zeros((num_joints, 3), np.float32), + joint_bind_orientations=np.tile([0.0, 0.0, 0.0, 1.0], (num_joints, 1)), + joint_parents=[TactileShape.ROOT_PARENT] + list(range(num_joints - 1)), + taxel_positions=np.full((num_taxels, 3), 0.01, np.float32), + taxel_normals=np.tile([0.0, 0.0, 1.0], (num_taxels, 1)), + taxel_tangents=np.tile([1.0, 0.0, 0.0], (num_taxels, 1)), + taxel_joint_indices=indices, + taxel_joint_weights=weights, + ) + + +class _FakeTracker: + """Stands in for a TactileTracker: fixed data and metadata.""" + + def __init__(self, data, metadata): + self._data = data + self._metadata = metadata + + def get_data(self, _session): + return self._data + + def get_metadata(self, _session): + return self._metadata + + +class TestTactileSource: + def test_creation_and_tracker(self): + assert _source().get_tracker().get_name() == "TactileTracker" + + def test_specs(self): + src = _source() + assert list(src.input_spec()) == ["deviceio_tactile"] + forces = src.output_spec()[TactileSource.FORCES] + assert forces.is_optional + assert forces.inner_type.types[0].shape == (NUM_TAXELS, 3) + assert src.output_spec()[TactileSource.SHAPE].is_optional + + def test_index_matches_layout_order(self): + names = [t.name for t in TactileShapeLayout(NUM_JOINTS, NUM_TAXELS).types] + assert names == [member.name.lower() for member in TactileShapeIndex] + + def test_poll_reads_data_and_metadata_from_one_tracker(self): + src = _source() + data = TactileData(np.zeros((NUM_TAXELS, 3), np.float32)) + shape = _shape() + src._tracker = _FakeTracker(data, shape) + + polled = src.poll_tracker(None)["deviceio_tactile"] + + assert polled[0] is data + assert polled[1] is shape + + def test_active_conversion(self): + src = _source() + forces = np.arange(9, dtype=np.float32).reshape(3, 3) + outputs = _outputs(src) + src.compute(_inputs(src, TactileData(forces), _shape()), outputs) + + np.testing.assert_array_equal( + np.asarray(outputs[TactileSource.FORCES][0]), forces + ) + group = outputs[TactileSource.SHAPE] + assert not group.is_none + parents = np.asarray(group[TactileShapeIndex.JOINT_PARENTS]) + assert parents.dtype == np.int32 + assert parents.tolist() == [TactileShape.ROOT_PARENT, 0] + assert ( + np.asarray(group[TactileShapeIndex.TAXEL_JOINT_INDICES]).dtype == np.int32 + ) + np.testing.assert_array_equal( + np.asarray(group[TactileShapeIndex.TAXEL_NORMALS]), + np.tile([0.0, 0.0, 1.0], (NUM_TAXELS, 1)), + ) + + def test_shape_without_data(self): + src = _source() + outputs = _outputs(src) + src.compute(_inputs(src, None, _shape()), outputs) + assert outputs[TactileSource.FORCES].is_none + assert not outputs[TactileSource.SHAPE].is_none + + def test_inactive_sets_none(self): + src = _source() + outputs = _outputs(src) + src.compute(_inputs(src, None, None), outputs) + assert outputs[TactileSource.FORCES].is_none + assert outputs[TactileSource.SHAPE].is_none + + def test_taxel_count_mismatch_raises(self): + src = _source(num_taxels=4) + frame = TactileData(np.zeros((3, 3), np.float32)) + with pytest.raises(ValueError, match="3 taxels"): + src.compute(_inputs(src, frame, None), _outputs(src)) + + def test_shape_count_mismatch_raises(self): + src = _source(num_joints=NUM_JOINTS + 1) + with pytest.raises(ValueError, match="2 joints"): + src.compute(_inputs(src, None, _shape()), _outputs(src)) diff --git a/tests/python/core/schema/test_tactile.py b/tests/python/core/schema/test_tactile.py new file mode 100644 index 0000000000..f6d91cb243 --- /dev/null +++ b/tests/python/core/schema/test_tactile.py @@ -0,0 +1,51 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Unit tests for TactileData / TactileDataRecord in isaaccapture.schema.""" + +import gc + +import numpy as np +import pytest + +from isaaccapture.schema import DeviceDataTimestamp, TactileData, TactileDataRecord + +FORCES = np.array([[0.0, 0.0, 1.0], [0.5, -0.5, 2.0], [0.0, 0.1, 0.0]], np.float32) + + +class TestTactileData: + def test_default_construction(self): + data = TactileData() + assert data.taxel_force.shape == (0, 3) + + def test_round_trip(self): + data = TactileData(FORCES) + assert data.taxel_force.dtype == np.float32 + np.testing.assert_array_equal(data.taxel_force, FORCES) + + def test_accepts_nested_lists(self): + data = TactileData([[1, 2, 3]]) + np.testing.assert_array_equal(data.taxel_force, [[1.0, 2.0, 3.0]]) + + def test_view_outlives_its_table(self): + forces = TactileData(FORCES).taxel_force + gc.collect() + np.testing.assert_array_equal(forces, FORCES) + + def test_rejects_wrong_width(self): + with pytest.raises(ValueError, match="taxel_force"): + TactileData(np.zeros((2, 4), np.float32)) + + def test_repr(self): + assert "taxels=3" in repr(TactileData(FORCES)) + + +class TestTactileDataRecord: + def test_record_carries_payload_and_timestamp(self): + record = TactileDataRecord(TactileData(FORCES), DeviceDataTimestamp(1, 2, 3)) + np.testing.assert_array_equal(record.data.taxel_force, FORCES) + assert record.timestamp.sample_time_raw_device_clock == 3 + + def test_payload_less_record(self): + record = TactileDataRecord(None, DeviceDataTimestamp(1, 2, 3)) + assert record.data is None diff --git a/tests/python/core/schema/test_tactile_shape.py b/tests/python/core/schema/test_tactile_shape.py new file mode 100644 index 0000000000..36e5642689 --- /dev/null +++ b/tests/python/core/schema/test_tactile_shape.py @@ -0,0 +1,95 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Unit tests for TactileShape in isaaccapture.schema.""" + +import numpy as np +import pytest + +from isaaccapture.schema import TactileShape + +ROOT = TactileShape.ROOT_PARENT + + +def shape_arrays(num_joints: int = 2, num_taxels: int = 3) -> dict: + """A wrist-rooted chain with every taxel skinned to its last two joints.""" + indices = np.zeros((num_taxels, 4), np.uint32) + indices[:, 0] = num_joints - 1 + weights = np.zeros((num_taxels, 4), np.float32) + weights[:, 0] = 1.0 + return dict( + joint_bind_positions=np.arange(num_joints * 3, dtype=np.float32).reshape(-1, 3), + joint_bind_orientations=np.tile([0.0, 0.0, 0.0, 1.0], (num_joints, 1)).astype( + np.float32 + ), + joint_parents=np.array([ROOT] + list(range(num_joints - 1)), np.uint32), + taxel_positions=np.full((num_taxels, 3), 0.01, np.float32), + taxel_normals=np.tile([0.0, 0.0, 1.0], (num_taxels, 1)).astype(np.float32), + taxel_tangents=np.tile([1.0, 0.0, 0.0], (num_taxels, 1)).astype(np.float32), + taxel_joint_indices=indices, + taxel_joint_weights=weights, + ) + + +class TestTactileShape: + def test_root_parent_is_the_openxr_max_enum(self): + assert ROOT == 0x7FFFFFFF + + def test_round_trip(self): + arrays = shape_arrays() + shape = TactileShape(**arrays) + + for name, expected in arrays.items(): + value = getattr(shape, name) + assert value.dtype == expected.dtype, name + np.testing.assert_array_equal(value, expected, err_msg=name) + + def test_bytes_round_trip(self): + shape = TactileShape(**shape_arrays()) + + restored = TactileShape.from_bytes(shape.to_bytes()) + + np.testing.assert_array_equal( + restored.taxel_joint_weights, shape.taxel_joint_weights + ) + assert restored.to_bytes() == shape.to_bytes() + + @pytest.mark.parametrize("payload", [b"", b"not a flatbuffer"]) + def test_from_bytes_rejects_invalid_payload(self, payload): + with pytest.raises(ValueError, match="valid FlatBuffer"): + TactileShape.from_bytes(payload) + + def test_rejects_mismatched_joint_lengths(self): + arrays = shape_arrays() + arrays["joint_parents"] = arrays["joint_parents"][:1] + with pytest.raises(ValueError, match="same length"): + TactileShape(**arrays) + + def test_rejects_mismatched_taxel_lengths(self): + arrays = shape_arrays() + arrays["taxel_normals"] = arrays["taxel_normals"][:2] + with pytest.raises(ValueError, match="same length"): + TactileShape(**arrays) + + def test_rejects_parent_out_of_range(self): + arrays = shape_arrays() + arrays["joint_parents"][1] = 5 + with pytest.raises(ValueError, match="joint_parents"): + TactileShape(**arrays) + + def test_rejects_skinning_joint_out_of_range(self): + arrays = shape_arrays() + arrays["taxel_joint_indices"][0, 3] = 2 + with pytest.raises(ValueError, match="taxel_joint_indices"): + TactileShape(**arrays) + + def test_rejects_wrong_width(self): + arrays = shape_arrays() + arrays["taxel_joint_weights"] = np.zeros((3, 3), np.float32) + with pytest.raises(ValueError, match="taxel_joint_weights"): + TactileShape(**arrays) + + def test_repr(self): + assert ( + repr(TactileShape(**shape_arrays())) == "TactileShape(joints=2, taxels=3)" + ) diff --git a/tests/python/core/schema/test_tracker_metadata.py b/tests/python/core/schema/test_tracker_metadata.py new file mode 100644 index 0000000000..75ba996f51 --- /dev/null +++ b/tests/python/core/schema/test_tracker_metadata.py @@ -0,0 +1,45 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Unit tests for the tracker metadata chunking helpers in isaaccapture.schema.""" + +import pytest + +from isaaccapture.schema import ( + TRACKER_METADATA_CHUNK_SIZE, + TRACKER_METADATA_TENSOR_IDENTIFIER, + split_tracker_metadata, + tracker_metadata_collection_id, +) + + +def test_collection_id_sits_next_to_the_data_collection(): + assert tracker_metadata_collection_id("tactile/right") == "tactile/right/metadata" + assert TRACKER_METADATA_TENSOR_IDENTIFIER == "metadata" + + +def test_split_respects_the_chunk_size(): + metadata = bytes(range(256)) * 200 + chunks = split_tracker_metadata(metadata, 1024) + assert len(chunks) > 1 + assert all(len(chunk) <= 1024 for chunk in chunks) + + +def test_small_metadata_is_one_chunk(): + assert len(split_tracker_metadata(b"\x01" * 100)) == 1 + assert TRACKER_METADATA_CHUNK_SIZE == 16384 + + +def test_empty_metadata_raises(): + with pytest.raises(ValueError, match="metadata size"): + split_tracker_metadata(b"") + + +def test_chunk_size_without_room_for_payload_raises(): + with pytest.raises(ValueError, match="max_chunk_size"): + split_tracker_metadata(b"\x01", 64) + + +def test_chunk_size_above_the_reader_limit_raises(): + with pytest.raises(ValueError, match="max_chunk_size"): + split_tracker_metadata(b"\x01", TRACKER_METADATA_CHUNK_SIZE + 1) diff --git a/tests/python/examples/CMakeLists.txt b/tests/python/examples/CMakeLists.txt index a0cc7b40cc..64ae92a359 100644 --- a/tests/python/examples/CMakeLists.txt +++ b/tests/python/examples/CMakeLists.txt @@ -5,3 +5,4 @@ add_subdirectory(camera_viz) add_subdirectory(deviceio_live_view) add_subdirectory(soma_body_publisher) add_subdirectory(soma_hand_publisher) +add_subdirectory(tactile_visualizer) diff --git a/tests/python/examples/tactile_visualizer/CMakeLists.txt b/tests/python/examples/tactile_visualizer/CMakeLists.txt new file mode 100644 index 0000000000..66ccd34e07 --- /dev/null +++ b/tests/python/examples/tactile_visualizer/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 "tactile_visualizer_${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("tactile_visualizer_${test_name}" PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$:${CMAKE_SOURCE_DIR}/examples/tactile_visualizer/python" + LABELS "tactile_visualizer;unit" + ) +endforeach() diff --git a/tests/python/examples/tactile_visualizer/minimal_hand.urdf b/tests/python/examples/tactile_visualizer/minimal_hand.urdf new file mode 100644 index 0000000000..e1ab3147f9 --- /dev/null +++ b/tests/python/examples/tactile_visualizer/minimal_hand.urdf @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + diff --git a/tests/python/examples/tactile_visualizer/pyproject.toml b/tests/python/examples/tactile_visualizer/pyproject.toml new file mode 100644 index 0000000000..1b5d6caa16 --- /dev/null +++ b/tests/python/examples/tactile_visualizer/pyproject.toml @@ -0,0 +1,20 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "tactile-visualizer-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = [ + "mcap>=1.2.0", + "numpy", + "pytest", + "pyyaml>=6.0", + "viser>=1.1.1,<2", + "yourdfpy>=0.0.58", +] + +[tool.pytest.ini_options] +pythonpath = ["../../../../examples/tactile_visualizer/python"] diff --git a/tests/python/examples/tactile_visualizer/test_app_smoke.py b/tests/python/examples/tactile_visualizer/test_app_smoke.py new file mode 100644 index 0000000000..3570931aac --- /dev/null +++ b/tests/python/examples/tactile_visualizer/test_app_smoke.py @@ -0,0 +1,65 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +from pathlib import Path + +import viser + +from isaaccapture_examples.tactile_visualizer.app import TactileVisualizer +from isaaccapture_examples.tactile_visualizer.model import ( + SessionState, + default_openxr_shape, +) +from isaaccapture_examples.tactile_visualizer.urdf_config import UrdfConfig + + +def test_offline_app_constructs_with_viser_1_1_api(): + server = viser.ViserServer(host="127.0.0.1", port=0, verbose=False) + try: + app = TactileVisualizer( + server=server, + urdf_path=Path(__file__).with_name("minimal_hand.urdf"), + urdf_config_path=None, + urdf_config=UrdfConfig(), + document=default_openxr_shape(2), + document_name="fixture", + source=None, + side="right", + liveness_timeout_s=1.0, + taxel_size_m=0.004, + write_root=None, + ) + + assert app.controller.state is SessionState.OFFLINE + assert len(app._taxel_handles) == 2 + assert app.urdf_joint_names == ("index_MCP_FE",) + assert len(app._tree_joint_inputs) == 26 + assert len(app._tree_taxel_inputs) == 2 + finally: + server.stop() + + +def test_invalid_urdf_is_reported_without_stopping_server(tmp_path): + urdf = tmp_path / "bad.urdf" + urdf.write_text("not XML", encoding="utf-8") + server = viser.ViserServer(host="127.0.0.1", port=0, verbose=False) + try: + app = TactileVisualizer( + server=server, + urdf_path=urdf, + urdf_config_path=None, + urdf_config=UrdfConfig(), + document=default_openxr_shape(), + document_name="untitled", + source=None, + side="right", + liveness_timeout_s=1.0, + taxel_size_m=0.004, + write_root=None, + ) + + assert app.urdf is None + assert "URDF unavailable" in app._last_error + assert app.controller.state is SessionState.OFFLINE + finally: + server.stop() diff --git a/tests/python/examples/tactile_visualizer/test_model.py b/tests/python/examples/tactile_visualizer/test_model.py new file mode 100644 index 0000000000..154e82eb2b --- /dev/null +++ b/tests/python/examples/tactile_visualizer/test_model.py @@ -0,0 +1,205 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import pytest + +from isaaccapture_examples.tactile_visualizer.model import ( + LatestMailbox, + SessionController, + SessionState, + ShapeDocument, + append_grid_taxels, + atomic_write_within, + default_openxr_shape, + force_colors, + project_points, + resolve_current_joints, + set_taxel_display_position, + skin_taxels, +) + + +def test_default_shape_is_valid_openxr_layout(): + shape = default_openxr_shape(37) + + shape.validate() + assert shape.num_joints == 26 + assert shape.num_taxels == 37 + assert int(shape.joint_parents[1]) == 0x7FFFFFFF + + +def test_default_new_shape_has_no_taxels(): + shape = default_openxr_shape() + + assert shape.num_joints == 26 + assert shape.num_taxels == 0 + + +def test_append_taxels_uses_documented_grid_order_and_wrist_attachment(): + shape = default_openxr_shape() + + append_grid_taxels(shape, 6) + + np.testing.assert_allclose(shape.taxel_positions[0], (-0.04, -0.04, -0.04)) + np.testing.assert_allclose(shape.taxel_positions[1], (-0.04, -0.04, -0.02)) + np.testing.assert_allclose(shape.taxel_positions[5], (-0.04, -0.02, -0.04)) + np.testing.assert_array_equal(shape.taxel_joint_indices, 1) + np.testing.assert_array_equal(shape.taxel_joint_weights[:, 0], 1.0) + np.testing.assert_array_equal(shape.taxel_joint_weights[:, 1:], 0.0) + + +def test_shape_file_round_trip(): + shape = default_openxr_shape(13) + + restored = ShapeDocument.from_bytes(shape.to_bytes()) + + for name, value in shape.arrays().items(): + np.testing.assert_array_equal(restored.arrays()[name], value) + + +def test_shape_file_rejects_malformed_bytes(): + with pytest.raises(ValueError, match="valid FlatBuffer"): + ShapeDocument.from_bytes(b"not a shape") + + +def test_shape_validation_rejects_bad_weights(): + shape = default_openxr_shape(1) + shape.taxel_joint_weights[0] = (0.5, 0.0, 0.0, 0.0) + + with pytest.raises(ValueError, match="sum to one"): + shape.validate() + + +def test_shape_validation_rejects_self_parent_and_cycle(): + shape = default_openxr_shape() + shape.joint_parents[3] = 3 + with pytest.raises(ValueError, match="cycle"): + shape.validate() + + shape = default_openxr_shape() + shape.joint_parents[1] = 2 + with pytest.raises(ValueError, match="cycle"): + shape.validate() + + +def test_skinning_moves_taxel_with_attached_joint(): + shape = default_openxr_shape(1) + current_positions, current_orientations = resolve_current_joints( + shape, None, None, None + ) + joint = int(shape.taxel_joint_indices[0, 0]) + current_positions[joint] += (0.01, -0.02, 0.03) + + positions, normals, tangents = skin_taxels( + shape, current_positions, current_orientations + ) + + np.testing.assert_allclose( + positions[0], shape.taxel_positions[0] + (0.01, -0.02, 0.03), atol=1e-6 + ) + np.testing.assert_allclose(normals[0], shape.taxel_normals[0], atol=1e-6) + np.testing.assert_allclose(tangents[0], shape.taxel_tangents[0], atol=1e-6) + + +def test_missing_wrist_pose_preserves_bind_skeleton(): + shape = default_openxr_shape() + live_positions = shape.joint_bind_positions + 1.0 + valid = np.ones(shape.num_joints, dtype=bool) + valid[1] = False + + positions, orientations = resolve_current_joints( + shape, live_positions, shape.joint_bind_orientations, valid + ) + + np.testing.assert_array_equal(positions, shape.joint_bind_positions) + np.testing.assert_array_equal(orientations, shape.joint_bind_orientations) + + +def test_display_translation_inverts_skinning_and_left_mirror(): + shape = default_openxr_shape(1) + current_positions, current_orientations = resolve_current_joints( + shape, None, None, None + ) + + set_taxel_display_position( + shape, + current_positions, + current_orientations, + 0, + np.array([0.02, 0.03, 0.04]), + "left", + ) + + np.testing.assert_allclose(shape.taxel_positions[0], (-0.02, 0.03, 0.04)) + + +def test_force_color_gradient_and_missing_fallback(): + colors = force_colors( + np.array([[0, 0, 0], [0, 0, 15], [0, 0, 30], [0, 0, 60]], np.float32), + 4, + ) + + np.testing.assert_array_equal(colors[0], (0, 0, 255)) + np.testing.assert_array_equal(colors[1], (128, 0, 128)) + np.testing.assert_array_equal(colors[2], (255, 0, 0)) + np.testing.assert_array_equal(colors[3], (255, 0, 0)) + assert force_colors(None, 3).shape == (3, 3) + + +def test_project_points_uses_viser_opencv_camera_convention(): + screen, visible = project_points( + np.array([[0.0, 0.0, 1.0], [1.0, 0.0, -1.0]]), + np.zeros(3), + np.array([1.0, 0.0, 0.0, 0.0]), + np.pi / 2, + 1.0, + ) + + np.testing.assert_allclose(screen[0], (0.5, 0.5)) + np.testing.assert_array_equal(visible, (True, False)) + + +def test_state_machine_pause_disconnect_reconnect_and_resume(): + controller = SessionController(live_source=True, timeout_s=1.0) + assert controller.state is SessionState.OFFLINE + assert controller.message_received(9.0, ready=False) is SessionState.OFFLINE + assert controller.message_received(10.0, ready=True) is SessionState.LIVE_PLAYING + assert controller.pause() is SessionState.LIVE_PAUSED + assert controller.tick(12.0) is SessionState.DISCONNECTED + assert controller.editing_enabled + assert controller.sample_received(12.1) is SessionState.LIVE_PAUSED + assert controller.resume(12.2) is SessionState.LIVE_PLAYING + assert controller.requirements_changed(12.3, ready=False) is SessionState.OFFLINE + + +def test_latest_mailbox_drops_unread_values(): + mailbox: LatestMailbox[str] = LatestMailbox() + mailbox.put("old") + mailbox.put("new") + + sequence, value, error = mailbox.latest_after(0) + + assert sequence == 2 + assert value == "new" + assert error is None + assert mailbox.latest_after(sequence) == (sequence, None, None) + + +def test_latest_mailbox_clears_transient_error_on_next_value(): + mailbox: LatestMailbox[str] = LatestMailbox() + mailbox.fail(ValueError("bad frame")) + mailbox.put("recovered") + + _, value, error = mailbox.latest_after(0) + + assert value == "recovered" + assert error is None + + +def test_atomic_write_is_bounded_to_root(tmp_path): + path = atomic_write_within(tmp_path, "nested/shape.bin", b"shape") + + assert path.read_bytes() == b"shape" + with pytest.raises(ValueError, match="inside"): + atomic_write_within(tmp_path, "../escape.bin", b"bad") diff --git a/tests/python/examples/tactile_visualizer/test_sources_and_config.py b/tests/python/examples/tactile_visualizer/test_sources_and_config.py new file mode 100644 index 0000000000..0b8ba502bf --- /dev/null +++ b/tests/python/examples/tactile_visualizer/test_sources_and_config.py @@ -0,0 +1,84 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +import time + +import numpy as np +import pytest + +from isaaccapture_examples.tactile_visualizer.app import TactileVisualizer +from isaaccapture_examples.tactile_visualizer.model import default_openxr_shape +from isaaccapture_examples.tactile_visualizer.sources import ( + HandSample, + MockSource, + SourceSnapshot, +) +from isaaccapture_examples.tactile_visualizer.urdf_config import ( + load_urdf_config, + map_raw_joints, +) + + +def test_mock_source_publishes_complete_latest_sample(): + source = MockSource(num_taxels=11, rate_hz=100.0) + source.start() + deadline = time.monotonic() + 2.0 + sequence = 0 + snapshot = None + try: + while snapshot is None and time.monotonic() < deadline: + sequence, snapshot, error = source.mailbox.latest_after(sequence) + assert error is None + time.sleep(0.005) + finally: + source.stop() + + assert snapshot is not None + assert snapshot.hands["right"].positions.shape == (26, 3) + assert snapshot.forces["right"].shape == (11, 3) + assert set(snapshot.raw_joints["right"]) == set(snapshot.raw_joints["left"]) + + +def test_malformed_live_snapshot_is_rejected_before_replacing_display(): + malformed = SourceSnapshot( + received_at=1.0, + hands={ + "right": HandSample( + positions=np.zeros((26, 2), np.float32), + orientations=np.zeros((26, 4), np.float32), + valid=np.ones(26, bool), + ) + }, + shapes={"right": default_openxr_shape()}, + forces={"right": np.zeros((0, 3), np.float32)}, + ) + + with pytest.raises(ValueError, match="hand pose"): + TactileVisualizer._validate_snapshot(malformed) + + +def test_urdf_config_and_joint_mapping(tmp_path): + path = tmp_path / "urdf.yaml" + path.write_text( + "scale: 0.01\noffset_cm: [1, 2, 3]\njoint_map:\n robot_index: index_MCP_FE\n", + encoding="utf-8", + ) + + config = load_urdf_config(path) + values = map_raw_joints( + ["robot_index", "same_name", "missing"], + {"index_MCP_FE": 1.5, "same_name": 2.5}, + config.joint_map, + ) + + assert config.scale == 0.01 + assert config.offset_cm == (1.0, 2.0, 3.0) + np.testing.assert_array_equal(values, (1.5, 2.5, 0.0)) + + +def test_urdf_config_rejects_unknown_keys(tmp_path): + path = tmp_path / "bad.yaml" + path.write_text("units: centimeters\n", encoding="utf-8") + + with pytest.raises(ValueError, match="unknown"): + load_urdf_config(path)