diff --git a/docs/source/device/tactile/index.rst b/docs/source/device/tactile/index.rst index 57b8522573..dfb386122a 100644 --- a/docs/source/device/tactile/index.rst +++ b/docs/source/device/tactile/index.rst @@ -7,6 +7,224 @@ Tactile devices Tactile devices capture contact measurements. For actuator output, see :doc:`/references/haptic_feedback`. +The common tactile interface separates each frame's force measurements from the +device's static layout. A *taxel* is one tactile sensing element: its force has +three components, and its layout describes where it is attached to a skeleton. +The current geometry mapping is linear blend skinning (LBS) over hand joints. + +.. contents:: On this page + :local: + :depth: 2 + +Data contract +------------- + +The contract is defined by +:code-file:`src/core/schema/fbs/tactile_array.fbs` and +:code-file:`src/core/schema/fbs/tactile_metadata.fbs`: + +.. list-table:: + :header-rows: 1 + :widths: 25 75 + + * - Type + - Meaning + * - ``TactileDataArray`` + - One force vector per taxel, in newtons, ordered exactly as the metadata. + The Python view is an ``(N, 3)`` float32 array. + * - ``TactileMetadata`` + - The static ``geometry_mapping`` and ``segment_source`` for that stream. + Currently these select ``LBS`` and ``HandSkeletonRef``. + * - ``HandSkeletonRef`` + - An ordered list of unique ``HandJoint`` identifiers from + :code-file:`src/core/schema/fbs/hand.fbs`. ``NUM_JOINTS`` is a count, + not a usable joint. + * - ``LBS`` + - ``J`` segment bind poses, ``N`` taxel bind poses, and four segment indices + and weights per taxel. An index addresses the ordered segment list, + rather than a ``HandJoint`` ordinal. + +Python pose arrays have shape ``(count, 7)``: position ``(x, y, z)`` in meters, +then a unit quaternion ``(x, y, z, w)``. Identity orientation is ``(0, 0, 0, 1)``. +All frames are right-handed. A pose maps coordinates from its local frame into +its reference frame: ``p_reference = R(q) * p_local + position``. + +Coordinate conventions +---------------------- + +Hand joints and the skeleton root +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +``HandSkeletonRef`` uses ``HandJoint.WRIST`` as the skeleton root. Its origin +is the wrist pivot. The joint names and local orientations follow +`OpenXR hand-joint conventions +`_: + +.. list-table:: + :header-rows: 1 + :widths: 15 85 + + * - Root axis + - Anatomical direction + * - ``+Y`` + - Out through the back of the hand at the wrist. + * - ``+Z`` + - Along the line through the wrist and middle-finger metacarpal, pointing + toward the forearm, away from the fingertips. + * - ``+X`` + - ``+Y cross +Z``, completing the right-handed basis. + +Finger-joint local ``+Z`` follows its bone away from the fingertip; local ``+Y`` +points toward the nail/back side in the fully open hand. The palm uses the +middle metacarpal for its longitudinal axis. These local frames move with the +joints when the hand bends; their axes are not fixed world directions. + +``segment_bind_poses[j]`` maps joint-local coordinates into the wrist frame in +the calibrated bind configuration. All segments share that root; the poses are +not relative to each joint's parent. If WRIST appears in the segment list, its +bind pose is identity. ``taxel_bind_poses`` use the same root and bind +configuration. The bind configuration must match the device's calibration; +the schema does not prescribe joint angles or a particular hand size. + +**HandPose is not automatically wrist-relative.** The live ``HandTracker`` +copies OpenXR joint poses in the session base space, currently ``STAGE``. +To obtain current joint poses in the tactile skeleton's wrist frame, use the +wrist and joint poses from the same valid hand sample: + +.. code-block:: text + + T_root_joint(t) = inverse(T_base_wrist(t)) * T_base_joint(t) + +Here ``T_A_B`` maps a point from frame B to frame A. This conversion uses both +the wrist translation and rotation. Subtracting the wrist position alone is +insufficient. A downstream consumer must perform this conversion when using +root-relative joint poses; ``TactileSource`` does not rebase a hand stream. + +Taxel frames +~~~~~~~~~~~~ + +A taxel pose's origin is its sensing location. Its orientation defines: + +.. list-table:: + :header-rows: 1 + :widths: 15 85 + + * - Taxel axis + - Direction + * - ``+Z = N`` + - Unit normal pointing out of the sensing surface, away from the device. + * - ``+X = T`` + - A calibrated unit tangent perpendicular to ``N``. + * - ``+Y`` + - ``N cross T``. Thus ``+X cross +Y = +Z``. + +The producer chooses and calibrates ``T`` for each taxel, then stores the full +orientation in ``taxel_bind_poses``. A normal alone cannot determine the tangent +direction. In particular, taxel ``+Z`` is a surface normal, whereas hand-joint +``+Z`` follows a bone; the two local frames generally differ. + +Forces ``(fx, fy, fz)`` are components in these taxel-local axes, rather than in +the wrist or world frame. Positive ``fz`` points outward; this does not imply +that a vendor's positive compression reading can be copied directly. The schema +does not identify which body the reported force acts on. A device integration +must document that choice and calibrate its units, axes, and signs accordingly. +Raw pressure or ADC counts require conversion to force before publication. + +Left and right hands +~~~~~~~~~~~~~~~~~~~~ + +Both hands use the same joint identifiers, right-handed algebra, units, +quaternion ordering, and taxel normal/tangent rules. Hand side does not reverse +the cross product or automatically negate a force component. Anatomical geometry +and the calibrated poses can differ between hands. For example, with an open +hand, wrist ``+X`` points toward the thumb side on the left hand and the little +finger side on the right hand. + +``HandSkeletonRef`` carries neither a side nor the identity of a hand tracker. +The application must associate each tactile stream with the correct hand stream +and calibration, using configuration and distinct collection IDs such as +``tactile/left`` and ``tactile/right``. The runtime does not infer side from +these names. Mirroring positions alone is insufficient: a reflection cannot be +encoded as a rotation quaternion, so each hand needs a proper right-handed +orientation for every joint and taxel. + +Mapping taxels onto the moving hand +----------------------------------- + +For a taxel's homogeneous bind position ``p_bind`` in the skeleton root, LBS +computes its current root-relative position as: + +.. code-block:: text + + p_root(t) = sum_k w[k] * C[j[k]](t) * inverse(B[j[k]]) * p_bind + +``B[j]`` is ``segment_bind_poses[j]`` and ``C[j](t)`` is the current pose of +``joint_ids[j]`` in the current wrist frame. Each taxel has four slots; weights +must be finite, nonnegative, and sum to one (validation tolerance ``1e-5``). +Every slot's index must be valid, including slots with zero weight. A taxel +rigidly attached to one segment can use weights ``(1, 0, 0, 0)`` and repeat that +segment's index in all four slots. + +To express the resulting position in the hand tracker's base space: + +.. code-block:: text + + p_base(t) = T_base_wrist(t) * p_root(t) + +LBS specifies blended positions; it does not define a unique rigid taxel frame +after blending multiple segment rotations. A consumer that needs animated +normals or world-space forces must choose an orientation/normal mapping rule. +For a single rigid attachment, the taxel transform is unambiguous: +``C[j](t) * inverse(B[j]) * T_root_taxel_bind``. Transform force directions using +rotation only, without adding the pose translation. + +Publishing and consuming +------------------------ + +A C++ plugin publishes serialized samples with ``SchemaPusher``; a Python +producer can use ``TensorPushTracker``. The paired streams are: + +.. code-block:: text + + tensor "tactile" -> TactileDataArray + /metadata tensor "metadata" -> TactileMetadata + +Publish metadata on startup and periodically so consumers that start later can +discover it. Publish the same encoded metadata throughout the tracker session. +``TactileTracker`` validates and adopts the first metadata, records it once, +and exposes it through ``get_metadata()``. It withholds force data until metadata +is confirmed. Different or invalid metadata causes an error; after a producer +restart, its metadata must be confirmed again. See :ref:`tracker-metadata`. + +The consumer's ``max_flatbuffer_size`` must be at least the producer's declared +sample size, including FlatBuffer overhead. The default is +``TactileTracker.DEFAULT_MAX_FLATBUFFER_SIZE``; pass a larger +``max_flatbuffer_size`` to ``TactileTracker`` when the producer uses larger force +samples. Metadata has its own +``TRACKER_METADATA_MAX_SIZE`` limit, including FlatBuffer overhead. +The data array length must match the metadata's taxel count. + +Applications can read ``TactileTracker`` directly for live or replay data. +Retargeting graphs can instead use ``TactileSource(name, collection_id, +num_segments, num_taxels)`` from +``isaaccapture.retargeting_engine.deviceio_source_nodes``. It emits +``taxel_forces`` and ``tactile_layout`` arrays; constructor counts fix their +shapes. It adapts the frame and static layout to graph tensors; the application +supplies hand motion and evaluates the geometry mapping separately. + +The runnable feeder, direct tracker consumer, and MCAP replay commands are in +:code-file:`examples/tactile/README.md`. The feeder uses synthetic geometry to +exercise transport; it is not an anatomical calibration model. Live operation +requires an OpenXR runtime and an available HMD system, including when using a +headless session; replay does not require the runtime. + +Vendor integrations +------------------- + +The guides below describe individual devices. The existing OGLO integration +uses its own ``OgloGloveSample`` schema with raw measurements; it does not +automatically publish the common force-and-layout contract described above. + .. toctree:: :maxdepth: 1 :titlesonly: diff --git a/docs/source/references/generated_trackers.rst b/docs/source/references/generated_trackers.rst index 1147ed9ce6..0e9d55533f 100644 --- a/docs/source/references/generated_trackers.rst +++ b/docs/source/references/generated_trackers.rst @@ -85,11 +85,12 @@ example) declares that description as the tracker's ``metadata`` table: [[tracker]] name = "tactile" - table = "TactileData" - metadata = "TactileShape" + table = "TactileDataArray" + schema = "tactile_array" + metadata = "TactileMetadata" The metadata table is the ``root_type`` of a schema of its own, named after the table in snake case -(``tactile_shape.fbs``), with no Record wrapper: MCAP records the table itself. Like the data table, +(``tactile_metadata.fbs``), with no Record wrapper: MCAP records the table itself. Like the data table, it needs a hand-written ``schema/python/*_bindings.h`` before ``get_metadata()`` can return it to Python. Only ``direction = "pull"`` supports ``metadata``. diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 9b96fea7f9..b1855c9a1d 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -19,6 +19,7 @@ if(BUILD_EXAMPLES) add_subdirectory(deviceio_live_view) add_subdirectory(soma_body_publisher) add_subdirectory(soma_hand_publisher) + add_subdirectory(tactile) 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..8dc7486fe6 --- /dev/null +++ b/examples/tactile/README.md @@ -0,0 +1,56 @@ + + +# Tactile + +A mock tactile device and a consumer for exercising the tactile tracker without tactile hardware. + +- `feeder` publishes a 26-joint hand with 200 taxels by default. Each frame carries one + `TactileDataArray`; it publishes `TactileMetadata` when it starts and every second after that. + It publishes through `TensorPushTracker` on a `DeviceIOSession`, as a tactile plugin would + through `SchemaPusher`. +- `printer` reads one `TactileTracker` directly. It discovers the joint and taxel counts from + `TactileMetadata`, prints the first and last taxel forces, and records the session to MCAP. + +```text +feeder -> "tactile" (TactileDataArray, every frame) + -> "tactile/metadata" (TactileMetadata, at start and every --metadata-interval s) + -> 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. Live initialization requires an +OpenXR HMD system; if none is available, it waits for a headset client 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 `TactileMetadata` must fit `TRACKER_METADATA_MAX_SIZE`. +The two processes need only agree on `--collection-id`. + +Metadata contains an ordered `HandSkeletonRef` and an `LBS` binding. Segment indices address +that joint list, not OpenXR ordinals. Python pose arrays have seven columns: position XYZ in +meters followed by unit quaternion XYZW; per-frame force components are in taxel-local axes +and measured in newtons. 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..d0ae37e0c2 --- /dev/null +++ b/examples/tactile/python/isaaccapture_examples/tactile/common.py @@ -0,0 +1,6 @@ +# 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" 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..5b6841e8a0 --- /dev/null +++ b/examples/tactile/python/isaaccapture_examples/tactile/feeder.py @@ -0,0 +1,222 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Publish mock tactile frames and their TactileMetadata metadata, as a tactile device would. + +Each frame pushes one TactileDataArray sample on the data collection. The encoded TactileMetadata goes +on "/metadata" when the feeder starts and every ``--metadata-interval`` seconds +after that. Pair it with ``python -m isaaccapture_examples.tactile.printer``. +""" + +import argparse +import logging +import math +import sys +import time + +import numpy as np + +from isaaccapture.cloudxr import CloudXRLauncher +from isaaccapture.deviceio_session import DeviceIOSession +from isaaccapture.deviceio_trackers import TactileTracker, TensorPushTracker +from isaaccapture.oxr import OpenXRSession +from isaaccapture.schema import ( + TRACKER_METADATA_MAX_SIZE, + TRACKER_METADATA_TENSOR_IDENTIFIER, + TactileDataArray, + TactileMetadata, + HandJoint, + HandSkeletonRef, + LBS, + tracker_metadata_collection_id, +) + +from .common import ( + DEFAULT_COLLECTION_ID, +) + +logger = logging.getLogger("isaaccapture.examples.tactile.feeder") + +MIN_NUM_JOINTS = 2 +MAX_NUM_JOINTS = int(HandJoint.NUM_JOINTS) +DEFAULT_NUM_JOINTS = MAX_NUM_JOINTS +DEFAULT_NUM_TAXELS = 200 + +WRIST = int(HandJoint.WRIST) +# First and distal joint of each finger, thumb to little, as XrHandJointEXT ordinals. +FINGER_FIRST_JOINTS = ( + int(HandJoint.THUMB_METACARPAL), + int(HandJoint.INDEX_METACARPAL), + int(HandJoint.MIDDLE_METACARPAL), + int(HandJoint.RING_METACARPAL), + int(HandJoint.LITTLE_METACARPAL), +) +FINGER_DISTAL_JOINTS = ( + int(HandJoint.THUMB_DISTAL), + int(HandJoint.INDEX_DISTAL), + int(HandJoint.MIDDLE_DISTAL), + int(HandJoint.RING_DISTAL), + int(HandJoint.LITTLE_DISTAL), +) + + +def mock_metadata(joint_ids: list[HandJoint], num_taxels: int) -> TactileMetadata: + """Build metadata for a flat hand with taxels on its distal segments.""" + num_segments = len(joint_ids) + # LBS indices address joint_ids rows, not HandJoint ordinals. + segment_by_joint = {int(joint): segment for segment, joint in enumerate(joint_ids)} + positions = np.zeros((num_segments, 3), np.float32) + positions[segment_by_joint[int(HandJoint.PALM)]] = (0.0, 0.05, 0.0) + + taxel_segments = [] + for finger, first in enumerate(FINGER_FIRST_JOINTS): + finger_end = ( + FINGER_FIRST_JOINTS[finger + 1] + if finger < len(FINGER_FIRST_JOINTS) - 1 + else int(HandJoint.NUM_JOINTS) + ) + finger_joints = [ + joint for joint in range(first, finger_end) if joint in segment_by_joint + ] + if not finger_joints: + continue + for joint in finger_joints: + positions[segment_by_joint[joint]] = ( + -0.04 + 0.02 * finger, + 0.02 + 0.025 * (joint - first), + 0.0, + ) + joint = min( + finger_joints, key=lambda joint: abs(joint - FINGER_DISTAL_JOINTS[finger]) + ) + taxel_segments.append(segment_by_joint[joint]) + + if not taxel_segments: + taxel_segments.append(segment_by_joint[WRIST]) + + taxel_positions = np.zeros((num_taxels, 3), np.float32) + taxel_segment_indices = np.zeros((num_taxels, 4), np.uint32) + taxel_segment_weights = np.zeros((num_taxels, 4), np.float32) + per_segment = math.ceil(num_taxels / len(taxel_segments)) + for taxel in range(num_taxels): + segment_index = taxel_segments[taxel % len(taxel_segments)] + slot = taxel // len(taxel_segments) + taxel_positions[taxel] = positions[segment_index] + ( + 0.0, + 0.02 * slot / per_segment, + -0.005, + ) + taxel_segment_indices[taxel] = segment_index + taxel_segment_weights[taxel, 0] = 1.0 + + segment_poses = np.column_stack( + (positions, np.tile([0.0, 0.0, 0.0, 1.0], (num_segments, 1))) + ) + taxel_poses = np.column_stack( + (taxel_positions, np.tile([1.0, 0.0, 0.0, 0.0], (num_taxels, 1))) + ) + return TactileMetadata( + geometry_mapping=LBS( + segment_poses, taxel_poses, taxel_segment_indices, taxel_segment_weights + ), + segment_source=HandSkeletonRef(joint_ids), + ) + + +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( + "--metadata-interval", + type=float, + default=1.0, + help="Seconds between pushes of the TactileMetadata", + ) + 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: + parser.error("--taxels must be positive") + if not math.isfinite(args.rate) or args.rate <= 0: + parser.error("--rate must be finite and positive") + if not math.isfinite(args.metadata_interval) or args.metadata_interval <= 0: + parser.error("--metadata-interval must be finite and positive") + + joint_ids = [HandJoint(i) for i in range(args.joints)] + metadata_bytes = mock_metadata(joint_ids, args.taxels).to_bytes() + if len(metadata_bytes) > TRACKER_METADATA_MAX_SIZE: + parser.error( + f"{args.joints} joints and {args.taxels} taxels make a {len(metadata_bytes)} B TactileMetadata; " + f"a tracker accepts at most {TRACKER_METADATA_MAX_SIZE} B" + ) + logger.info( + "Publishing %d joints and %d taxels on '%s'; TactileMetadata is %d B", + args.joints, + args.taxels, + args.collection_id, + len(metadata_bytes), + ) + + frames = 0 + last_metadata_push = -math.inf + data_tracker = TensorPushTracker( + args.collection_id, "tactile", TactileTracker.DEFAULT_MAX_FLATBUFFER_SIZE + ) + metadata_tracker = TensorPushTracker( + tracker_metadata_collection_id(args.collection_id), + TRACKER_METADATA_TENSOR_IDENTIFIER, + len(metadata_bytes), + ) + trackers = [data_tracker, metadata_tracker] + with ( + CloudXRLauncher.launch_context(args), + OpenXRSession( + "TactileFeeder", DeviceIOSession.get_required_extensions(trackers) + ) as oxr_session, + DeviceIOSession.run(trackers, oxr_session.get_handles()) as session, + ): + 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, TactileDataArray(mock_forces(args.taxels, t)).to_bytes() + ) + if t - last_metadata_push >= args.metadata_interval: + metadata_tracker.push(session, metadata_bytes) + last_metadata_push = t + 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..1fd1993b5f --- /dev/null +++ b/examples/tactile/python/isaaccapture_examples/tactile/printer.py @@ -0,0 +1,185 @@ +# 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 TactileMetadata 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() + metadata_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 + + metadata = tracker.get_metadata(session) + if not metadata_seen and metadata is not None: + geometry_mapping, segment_source = ( + metadata.geometry_mapping, + metadata.segment_source, + ) + if geometry_mapping is None or segment_source is None: + raise ValueError( + "TactileMetadata must specify geometry_mapping and segment_source" + ) + expected_taxels = len(geometry_mapping.taxel_bind_poses) + print( + f"{GREEN}[{elapsed:6.2f}s] (metadata received) " + f"{len(segment_source.joint_ids)} joints, " + f"{expected_taxels} taxels{RESET}" + ) + metadata_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_forces) + if expected_taxels is not None and len(force_array) != expected_taxels: + raise ValueError( + f"frame has {len(force_array)} taxels, " + f"TactileMetadata 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=f"Seconds between live prints; replay prints every {REPLAY_PRINT_EVERY_RECORDS} 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/src/core/codegen/templates.py b/src/core/codegen/templates.py index e755929b98..026ced75a9 100644 --- a/src/core/codegen/templates.py +++ b/src/core/codegen/templates.py @@ -21,7 +21,7 @@ def _header() -> str: def camel_to_snake(value: str) -> str: - """Convert CamelCase to snake_case (TactileShape -> tactile_shape).""" + """Convert CamelCase to snake_case (TactileMetadata -> tactile_metadata).""" return re.sub( r"(?<=[a-z0-9])(?=[A-Z])|(?<=[A-Z])(?=[A-Z][a-z])", "_", value ).lower() diff --git a/src/core/codegen/templates/fragments/pybind_pull.template b/src/core/codegen/templates/fragments/pybind_pull.template index f059b299c4..389e886a73 100644 --- a/src/core/codegen/templates/fragments/pybind_pull.template +++ b/src/core/codegen/templates/fragments/pybind_pull.template @@ -1,4 +1,7 @@ - py::class_>(m, "@CLASS@") + py::class_> @NAME@_pull(m, "@CLASS@"); + @NAME@_pull.attr("DEFAULT_MAX_FLATBUFFER_SIZE") = + static_cast(core::@CLASS@::DEFAULT_MAX_FLATBUFFER_SIZE); + @NAME@_pull .def(py::init(), py::arg("collection_id"), py::arg("max_flatbuffer_size") = core::@CLASS@::DEFAULT_MAX_FLATBUFFER_SIZE) .def( diff --git a/src/core/codegen/templates/pull/replay.cpp.template b/src/core/codegen/templates/pull/replay.cpp.template index 9f2c6e0e99..eaf038dac8 100644 --- a/src/core/codegen/templates/pull/replay.cpp.template +++ b/src/core/codegen/templates/pull/replay.cpp.template @@ -5,10 +5,12 @@ #include #if @HAS_METADATA@ +#include #include #endif #include +#include #include #include @@ -23,7 +25,13 @@ Serialized<@METADATA@> @REPLAY_IMPL@::read_metadata(std::unique_ptr viewers( std::move(reader), base_name, std::vector(@TRAITS@::metadata_channels.begin(), @TRAITS@::metadata_channels.end()), recorded); - return viewers.read(0); + auto metadata = viewers.read(0); + // MCAP verifies structure; apply the same schema semantics as live ingestion. + if (metadata) + { + verify_metadata(*metadata, base_name); + } + return metadata; } @REPLAY_IMPL@::@REPLAY_IMPL@(std::unique_ptr reader, std::string_view base_name, diff --git a/src/core/codegen/test_manifest.py b/src/core/codegen/test_manifest.py index 5fdc5aaf51..df705836d5 100644 --- a/src/core/codegen/test_manifest.py +++ b/src/core/codegen/test_manifest.py @@ -213,7 +213,12 @@ class MetadataTest(unittest.TestCase): def _context(self, **overrides): from templates import enrich_context - raw = {"name": "tactile", "table": "TactileData", **overrides} + raw = { + "name": "tactile", + "table": "TactileDataArray", + "schema": "tactile_array", + **overrides, + } return enrich_context(resolve_tracker_entry(raw, load_defaults(DEFAULTS_PATH))) def test_unset_by_default(self) -> None: @@ -223,11 +228,13 @@ def test_names_derive_from_the_table_and_channel(self) -> None: from templates import MetadataGenContext self.assertEqual( - self._context(metadata="TactileShape").metadata, - MetadataGenContext("TactileShape", "tactile_shape", "tactile_metadata"), + self._context(metadata="TactileMetadata").metadata, + MetadataGenContext( + "TactileMetadata", "tactile_metadata", "tactile_metadata" + ), ) self.assertEqual( - self._context(metadata="TactileShape", channel="glove").metadata.channel, + self._context(metadata="TactileMetadata", channel="glove").metadata.channel, "glove_metadata", ) @@ -241,7 +248,7 @@ def test_invalid_values_are_rejected(self) -> None: for overrides in ( {"metadata": ""}, {"metadata": 1}, - {"metadata": "TactileShape", "direction": "push"}, + {"metadata": "TactileMetadata", "direction": "push"}, ): with self.subTest(**overrides), self.assertRaises(ValueError) as ctx: self._context(**overrides) @@ -255,7 +262,7 @@ def test_every_placeholder_is_filled(self) -> None: render_for_entry, ) - for metadata in (None, "TactileShape"): + for metadata in (None, "TactileMetadata"): ctx = self._context(**({"metadata": metadata} if metadata else {})) files = render_for_entry(ctx) for fragment in ( diff --git a/src/core/deviceio_trackers/trackers.toml b/src/core/deviceio_trackers/trackers.toml index d27518d43c..5220ecbde3 100644 --- a/src/core/deviceio_trackers/trackers.toml +++ b/src/core/deviceio_trackers/trackers.toml @@ -111,3 +111,12 @@ python_accessor = "get_spacemouse_data" name = "gamepad" table = "GamepadSnapshot" python_accessor = "get_gamepad_data" + +# Defaults fit about 1300 taxels (12 B each); larger devices pass max_flatbuffer_size. The +# TactileMetadata must also fit the separate TRACKER_METADATA_MAX_SIZE limit. +[[tracker]] +name = "tactile" +table = "TactileDataArray" +schema = "tactile_array" +metadata = "TactileMetadata" +max_flatbuffer_size = 16384 diff --git a/src/core/live_trackers/cpp/inc/live_trackers/schema_metadata_tracker.hpp b/src/core/live_trackers/cpp/inc/live_trackers/schema_metadata_tracker.hpp index dadf1b51f8..c1d96f2866 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/schema_metadata_tracker.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/schema_metadata_tracker.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -63,8 +64,14 @@ class SchemaMetadataTracker : public SchemaTrackerBase */ bool update() { + const auto previous_collection = collection_instance_id(); std::vector samples; const bool present = read_all_samples(samples); + if (collection_instance_id() != previous_collection) + { + // Recreated collections must confirm their own sample, even without an observed gap. + confirmed_ = false; + } for (auto& sample : samples) { accept(sample); @@ -102,6 +109,9 @@ class SchemaMetadataTracker : public SchemaTrackerBase if (!metadata_) { + // Schema-specific dispatch belongs to the schema layer, not this generic reader. + verify_metadata(*flatbuffers::GetRoot(sample.buffer.data()), collection_id); + const int64_t log_time_ns = sample.timestamp.available_time_local_common_clock(); metadata_ = Serialized::adopt(std::move(sample.buffer)); if (mcap_channels_ != nullptr) diff --git a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp index 119cfe3ba5..4cf02b1204 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/schema_tracker_base.hpp @@ -115,6 +115,17 @@ class SchemaTrackerBase */ const SchemaTrackerConfig& config() const; +protected: + //! Runtime identity of the discovered collection, distinct from its reusable list index. + std::optional collection_instance_id() const + { + if (!m_target_collection) + { + return std::nullopt; + } + return m_target_collection->id; + } + private: void initialize_tensor_data_functions(); void create_tensor_list(); @@ -136,7 +147,12 @@ class SchemaTrackerBase PFN_xrUpdateTensorListNV m_update_list_fn{ nullptr }; PFN_xrDestroyTensorListNV m_destroy_list_fn{ nullptr }; - std::optional m_target_collection_index; + struct TargetCollection + { + uint32_t index; + XrTensorCollectionIDNV id; + }; + std::optional m_target_collection; uint32_t m_sample_batch_stride{ 0 }; uint32_t m_sample_size{ 0 }; diff --git a/src/core/live_trackers/cpp/schema_tracker_base.cpp b/src/core/live_trackers/cpp/schema_tracker_base.cpp index d99cdb39b1..e7b1c75c7c 100644 --- a/src/core/live_trackers/cpp/schema_tracker_base.cpp +++ b/src/core/live_trackers/cpp/schema_tracker_base.cpp @@ -75,13 +75,13 @@ bool SchemaTrackerBase::ensure_collection() { if (is_known_non_fatal_tensor_unavailable(result)) { - m_target_collection_index.reset(); + m_target_collection.reset(); return false; } throw std::runtime_error("Failed to get latest generation, result=" + std::to_string(result)); } - if (latest_generation == m_cached_generation && m_target_collection_index.has_value()) + if (latest_generation == m_cached_generation && m_target_collection.has_value()) { return true; } @@ -93,7 +93,7 @@ bool SchemaTrackerBase::ensure_collection() { if (is_known_non_fatal_tensor_unavailable(result)) { - m_target_collection_index.reset(); + m_target_collection.reset(); return false; } throw std::runtime_error("Failed to update tensor list, result=" + std::to_string(result)); @@ -107,7 +107,7 @@ bool SchemaTrackerBase::ensure_collection() { if (is_known_non_fatal_tensor_unavailable(result)) { - m_target_collection_index.reset(); + m_target_collection.reset(); return false; } throw std::runtime_error("Failed to get list properties, result=" + std::to_string(result)); @@ -121,7 +121,7 @@ bool SchemaTrackerBase::ensure_collection() { if (is_known_non_fatal_tensor_unavailable(result)) { - m_target_collection_index.reset(); + m_target_collection.reset(); return false; } throw std::runtime_error("Failed to get collection properties, result=" + std::to_string(result)); @@ -143,16 +143,18 @@ bool SchemaTrackerBase::ensure_collection() m_sample_batch_stride = coll_props.sampleBatchStride; m_sample_size = coll_props.data.totalSampleSize; - if (i != m_target_collection_index) + // A recreated collection can reuse its list index, but never its runtime ID. + if (!m_target_collection || i != m_target_collection->index || + coll_props.tensorCollectionId != m_target_collection->id) { m_last_sample_index.reset(); m_logger->info("Found target collection at index {}", i); } - m_target_collection_index = i; + m_target_collection = TargetCollection{ i, coll_props.tensorCollectionId }; return true; } - m_target_collection_index.reset(); + m_target_collection.reset(); return false; } @@ -169,9 +171,9 @@ bool SchemaTrackerBase::read_all_samples(std::vector& samples) samples.push_back(std::move(result)); } - // read_next_sample clears m_target_collection_index on API failure. + // read_next_sample clears m_target_collection on API failure. // If it's now empty we hit an error mid-drain, not a normal end-of-data. - if (!m_target_collection_index.has_value()) + if (!m_target_collection.has_value()) { return false; } @@ -216,7 +218,7 @@ void SchemaTrackerBase::create_tensor_list() bool SchemaTrackerBase::read_next_sample(SampleResult& out) { - if (!m_target_collection_index.has_value()) + if (!m_target_collection.has_value()) { return false; } @@ -224,7 +226,7 @@ bool SchemaTrackerBase::read_next_sample(SampleResult& out) // Prepare retrieval info XrTensorDataRetrievalInfoNV retrievalInfo{ XR_TYPE_TENSOR_DATA_RETRIEVAL_INFO_NV }; retrievalInfo.next = nullptr; - retrievalInfo.tensorCollectionIndex = m_target_collection_index.value(); + retrievalInfo.tensorCollectionIndex = m_target_collection->index; retrievalInfo.startSampleIndex = m_last_sample_index.has_value() ? m_last_sample_index.value() + 1 : 0; // Prepare output buffers (read one sample at a time for simplicity) @@ -248,14 +250,14 @@ bool SchemaTrackerBase::read_next_sample(SampleResult& out) // Policy: temporary collection loss is treated as non-fatal. m_logger->warn( "xrGetTensorDataNV: XR_ERROR_TENSOR_LOST_NV -- tensor/collection data lost; " - "clearing m_target_collection_index."); + "clearing target collection."); } else { - m_target_collection_index = std::nullopt; + m_target_collection.reset(); throw std::runtime_error("[SchemaTracker] xrGetTensorDataNV failed, result=" + std::to_string(result)); } - m_target_collection_index = std::nullopt; + m_target_collection.reset(); return false; } diff --git a/src/core/schema/cpp/inc/schema/metadata_validation.hpp b/src/core/schema/cpp/inc/schema/metadata_validation.hpp new file mode 100644 index 0000000000..8cc6579df0 --- /dev/null +++ b/src/core/schema/cpp/inc/schema/metadata_validation.hpp @@ -0,0 +1,41 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tactile_metadata_validation.hpp" + +#include +#include +#include + +namespace core +{ + +//! Throws on invalid semantics after FlatBuffers verification; schemas without extra rules are a no-op. +template +inline void verify_metadata(const MetadataT&) +{ +} + +template <> +inline void verify_metadata(const TactileMetadata& metadata) +{ + verify_tactile_metadata(metadata); +} + +//! Keep stream context here so live and replay report the same diagnostic. +template +inline void verify_metadata(const MetadataT& metadata, std::string_view name) +{ + try + { + verify_metadata(metadata); + } + catch (const std::runtime_error& error) + { + throw std::runtime_error(std::string(name) + ": " + error.what()); + } +} + +} // namespace core diff --git a/src/core/schema/cpp/inc/schema/tactile_metadata_validation.hpp b/src/core/schema/cpp/inc/schema/tactile_metadata_validation.hpp new file mode 100644 index 0000000000..64dde27a6a --- /dev/null +++ b/src/core/schema/cpp/inc/schema/tactile_metadata_validation.hpp @@ -0,0 +1,135 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include +#include + +namespace core +{ + +//! Checks the semantics of the supported LBS/HandSkeletonRef layout after FlatBuffers verification. +//! Weight sums may differ from one by at most 1e-5 to allow floating-point rounding. +//! Squared quaternion norms may differ from one by at most 1e-4. +//! Throws std::runtime_error naming the invalid field and the violated constraint. +inline void verify_tactile_metadata(const TactileMetadata& metadata) +{ + const auto* lbs = metadata.geometry_mapping_as_LBS(); + const auto* hand = metadata.segment_source_as_HandSkeletonRef(); + if (lbs == nullptr) + { + throw std::runtime_error("TactileMetadata.geometry_mapping: expected LBS"); + } + if (hand == nullptr) + { + throw std::runtime_error("TactileMetadata.segment_source: expected HandSkeletonRef"); + } + + const auto taxel_count = lbs->taxel_bind_poses()->size(); + if (lbs->taxel_segment_indices()->size() != taxel_count) + { + throw std::runtime_error("TactileMetadata.taxel_segment_indices: expected " + std::to_string(taxel_count) + + " rows to match taxel_bind_poses, got " + + std::to_string(lbs->taxel_segment_indices()->size())); + } + if (lbs->taxel_segment_weights()->size() != taxel_count) + { + throw std::runtime_error("TactileMetadata.taxel_segment_weights: expected " + std::to_string(taxel_count) + + " rows to match taxel_bind_poses, got " + + std::to_string(lbs->taxel_segment_weights()->size())); + } + if (lbs->segment_bind_poses()->size() != hand->joint_ids()->size()) + { + throw std::runtime_error( + "TactileMetadata.segment_bind_poses: expected " + std::to_string(hand->joint_ids()->size()) + + " poses to match segment_source.joint_ids, got " + std::to_string(lbs->segment_bind_poses()->size())); + } + + std::array seen{}; + for (flatbuffers::uoffset_t i = 0; i < hand->joint_ids()->size(); ++i) + { + const auto joint_id = hand->joint_ids()->Get(i); + if (joint_id >= HandJoint_NUM_JOINTS) + { + throw std::runtime_error("TactileMetadata.joint_ids[" + std::to_string(i) + + "]: invalid HandJoint identifier " + std::to_string(joint_id)); + } + if (seen[joint_id]) + { + throw std::runtime_error("TactileMetadata.joint_ids[" + std::to_string(i) + + "]: duplicate HandJoint identifier " + std::to_string(joint_id)); + } + seen[joint_id] = true; + } + + for (flatbuffers::uoffset_t i = 0; i < taxel_count; ++i) + { + const auto* indices = lbs->taxel_segment_indices()->Get(i); + for (flatbuffers::uoffset_t k = 0; k < indices->values()->size(); ++k) + { + const auto segment_index = indices->values()->Get(k); + if (segment_index >= lbs->segment_bind_poses()->size()) + { + throw std::runtime_error("TactileMetadata.taxel_segment_indices[" + std::to_string(i) + "][" + + std::to_string(k) + "]: segment index " + std::to_string(segment_index) + + " is outside segment_bind_poses (size " + + std::to_string(lbs->segment_bind_poses()->size()) + ")"); + } + } + } + + for (flatbuffers::uoffset_t i = 0; i < taxel_count; ++i) + { + const auto* weights = lbs->taxel_segment_weights()->Get(i); + double sum = 0.0; + for (flatbuffers::uoffset_t k = 0; k < weights->values()->size(); ++k) + { + const float weight = weights->values()->Get(k); + if (!std::isfinite(weight) || weight < 0.0f) + { + throw std::runtime_error("TactileMetadata.taxel_segment_weights[" + std::to_string(i) + "][" + + std::to_string(k) + "]: weights must be finite and nonnegative"); + } + sum += weight; + } + if (std::abs(sum - 1.0) > 1e-5) + { + throw std::runtime_error("TactileMetadata.taxel_segment_weights[" + std::to_string(i) + + "]: weights must sum to one, got " + std::to_string(sum)); + } + } + + const auto verify_poses = [](const flatbuffers::Vector* poses, const char* field) + { + for (flatbuffers::uoffset_t i = 0; i < poses->size(); ++i) + { + const auto& position = poses->Get(i)->position(); + const auto& orientation = poses->Get(i)->orientation(); + for (const float component : std::array{ position.x(), position.y(), position.z(), orientation.x(), + orientation.y(), orientation.z(), orientation.w() }) + { + if (!std::isfinite(component)) + { + throw std::runtime_error("TactileMetadata." + std::string(field) + "[" + std::to_string(i) + + "]: poses must be finite"); + } + } + const float norm2 = orientation.x() * orientation.x() + orientation.y() * orientation.y() + + orientation.z() * orientation.z() + orientation.w() * orientation.w(); + if (std::abs(norm2 - 1.0f) > 1e-4f) + { + throw std::runtime_error("TactileMetadata." + std::string(field) + "[" + std::to_string(i) + + "]: orientations must be unit quaternions (x, y, z, w)"); + } + } + }; + verify_poses(lbs->segment_bind_poses(), "segment_bind_poses"); + verify_poses(lbs->taxel_bind_poses(), "taxel_bind_poses"); +} + +} // namespace core diff --git a/src/core/schema/fbs/float4.fbs b/src/core/schema/fbs/float4.fbs new file mode 100644 index 0000000000..695ac5a883 --- /dev/null +++ b/src/core/schema/fbs/float4.fbs @@ -0,0 +1,8 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +namespace core; + +struct Float4 { + values: [float:4]; +} diff --git a/src/core/schema/fbs/hand.fbs b/src/core/schema/fbs/hand.fbs index 18908155eb..93540bafce 100644 --- a/src/core/schema/fbs/hand.fbs +++ b/src/core/schema/fbs/hand.fbs @@ -44,9 +44,15 @@ enum HandJoint : uint8 { NUM_JOINTS = 26, } -// Describe a HandJoint pose. +// OpenXR joint-local axes: +Z follows the bone away from the fingertip; +Y +// points toward the nail/back of the open hand; +X = +Y cross +Z. +// At WRIST, +Z follows the wrist-to-middle-metacarpal line toward the forearm; +// at PALM, it follows the middle metacarpal toward the wrist. +// Both hands use right-handed axes, meters, and unit quaternions (x, y, z, w). +// https://registry.khronos.org/OpenXR/specs/1.1/html/xrspec.html#convention-of-hand-joints struct HandJointPose { - // The concrete pose data + // Joint-local to the shared tracking reference frame (the session base space + // for live HandTracker), not parent-local or automatically wrist-relative. pose: Pose; // Whether the hand pose data is valid diff --git a/src/core/schema/fbs/tactile_array.fbs b/src/core/schema/fbs/tactile_array.fbs new file mode 100644 index 0000000000..6e56f27fb1 --- /dev/null +++ b/src/core/schema/fbs/tactile_array.fbs @@ -0,0 +1,26 @@ +// 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 forces. TactileMetadata (tactile_metadata.fbs) defines the +// taxel order and reference geometry for the same tracker. +table TactileDataArray { + // Force vectors in newtons, expressed in taxel-local axes. For LBS, taxel_bind_poses + // define these axes at bind; the vector length equals the metadata taxel count. + // +X is the calibrated tangent T, +Z is the outward normal N, +Y = N cross T. + // Component signs follow these axes for both hands, without a side-based flip. + taxel_forces: [Point] (id: 0, required); +} + +// MCAP recording wrapper for TactileDataArray. +table TactileDataArrayRecord { + // Absent when tracking data is unavailable at this timestamp. + data: TactileDataArray (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type TactileDataArrayRecord; diff --git a/src/core/schema/fbs/tactile_metadata.fbs b/src/core/schema/fbs/tactile_metadata.fbs new file mode 100644 index 0000000000..25f5ac9ff4 --- /dev/null +++ b/src/core/schema/fbs/tactile_metadata.fbs @@ -0,0 +1,66 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "float4.fbs"; +include "hand.fbs"; +include "pose.fbs"; +include "uint4.fbs"; + +namespace core; + +// The skeleton root is WRIST, with the OpenXR axes from hand.fbs: +Y toward +// the back of the hand, +Z toward the forearm, +X = +Y cross +Z. +// Both hands use these right-handed rules. Hand side and source hand stream +// are associated by the application; this table does not imply mirroring. +table HandSkeletonRef { + // Ordered, unique HandJoint identifiers; NUM_JOINTS is not a joint. + // Segment indices address this vector, not HandJoint ordinals. Bind and current + // segment poses map joint-local coordinates into the same hand-root frame. + joint_ids: [HandJoint] (id: 0, required); // defined in hand.fbs +} + +union SegmentSource { + HandSkeletonRef = 1, +} + +table LBS { + // Segment-local-to-skeleton-root bind poses, in SegmentSource order. + // For HandSkeletonRef the root is the WRIST frame at bind, not a joint's + // parent or the session/world frame. Joint-local axes follow hand.fbs. + // Positions are in meters; orientations are unit quaternions (x, y, z, w). + segment_bind_poses: [Pose] (id: 0, required); + + // Taxel-local-to-skeleton-root bind poses, in TactileDataArray.taxel_forces order. + // The origin is the sensing location. +Z is the outward unit surface normal N; + // +X is a calibrated unit tangent T orthogonal to N; +Y = N cross T. + // These axes are right-handed for either hand; T is stored by the orientation, + // not inferred from hand side. Units and root match segment_bind_poses. + taxel_bind_poses: [Pose] (id: 1, required); + + // One four-slot row per taxel; all taxel_* vectors have the same length. + // Every slot indexes segment_bind_poses, including slots whose weight is zero. + taxel_segment_indices: [UInt4] (id: 2, required); + + // Finite, nonnegative weights, summing to one per taxel. Bind positions move as + // p(t) = sum_k w_k * T_current[j_k] * inverse(T_bind[j_k]) * p_bind. + // LBS blends transformed points; it does not define a rigid animated taxel frame. + taxel_segment_weights: [Float4] (id: 3, required); +} + +// Static parameters for the selected taxel geometry-mapping method. +union AlgorithmSource { + LBS = 1, +} + +// Static layout published as tracker metadata, without a timestamp or Record wrapper. +// Required fields enforce presence only; empty vectors still pass verification. +// Lengths, indices and weights require semantic validation by producers/consumers. +table TactileMetadata { + // The implicit geometry_mapping_type discriminator occupies id 0. + geometry_mapping: AlgorithmSource (id: 1, required); + + // The implicit segment_source_type discriminator occupies id 2. + segment_source: SegmentSource (id: 3, required); +} + +root_type TactileMetadata; diff --git a/src/core/schema/fbs/uint4.fbs b/src/core/schema/fbs/uint4.fbs new file mode 100644 index 0000000000..e752b0703e --- /dev/null +++ b/src/core/schema/fbs/uint4.fbs @@ -0,0 +1,8 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +namespace core; + +struct UInt4 { + values: [uint32:4]; +} diff --git a/src/core/schema/golden/float4.bfbs b/src/core/schema/golden/float4.bfbs new file mode 100644 index 0000000000..f03d7b8aa7 Binary files /dev/null and b/src/core/schema/golden/float4.bfbs differ diff --git a/src/core/schema/golden/tactile_array.bfbs b/src/core/schema/golden/tactile_array.bfbs new file mode 100644 index 0000000000..22d12e1a69 Binary files /dev/null and b/src/core/schema/golden/tactile_array.bfbs differ diff --git a/src/core/schema/golden/tactile_metadata.bfbs b/src/core/schema/golden/tactile_metadata.bfbs new file mode 100644 index 0000000000..355bac21ed Binary files /dev/null and b/src/core/schema/golden/tactile_metadata.bfbs differ diff --git a/src/core/schema/golden/uint4.bfbs b/src/core/schema/golden/uint4.bfbs new file mode 100644 index 0000000000..53c10fac65 Binary files /dev/null and b/src/core/schema/golden/uint4.bfbs differ diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index b34cac9c90..183f172d22 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -28,6 +28,8 @@ pybind11_add_module(schema_py schema_module.cpp spacemouse_bindings.h steering_wheel_bindings.h + tactile_array_bindings.h + tactile_metadata_bindings.h tracker_metadata_bindings.h ) 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 7d7a950be7..d9889f851f 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -29,6 +29,8 @@ #include "soma_hand_joint_rotations_bindings.h" #include "spacemouse_bindings.h" #include "steering_wheel_bindings.h" +#include "tactile_array_bindings.h" +#include "tactile_metadata_bindings.h" #include "timestamp_bindings.h" #include "tracker_metadata_bindings.h" @@ -62,6 +64,10 @@ PYBIND11_MODULE(_schema, m) // Bind OGLO tactile glove types (OgloGloveSample table). core::bind_oglo_tactile(m); + // Bind tactile types (TactileDataArray, TactileMetadata tables). + core::bind_tactile_array(m); + core::bind_tactile_metadata(m); + // Bind the helpers for producers of tracker metadata. core::bind_tracker_metadata(m); diff --git a/src/core/schema/python/tactile_array_bindings.h b/src/core/schema/python/tactile_array_bindings.h new file mode 100644 index 0000000000..351947a0ba --- /dev/null +++ b/src/core/schema/python/tactile_array_bindings.h @@ -0,0 +1,55 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the TactileDataArray FlatBuffer schema (see tactile_array.fbs). +// Types: TactileDataArray (table) and TactileDataArrayRecord, exposed as encoded views. + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include +#include +#include + +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_tactile_array(py::module& m) +{ + using FloatRows = py::array_t; + + serialized_class(m, "TactileDataArray", "Encoded per-frame tactile reading.") + .def(py::init( + [](const FloatRows& taxel_forces) + { + TactileDataArrayT native; + native.taxel_forces = structs_from_rows(taxel_forces, "taxel_forces"); + return pack(native); + }), + py::arg("taxel_forces").none(false), "Encode a tactile frame. taxel_forces is an (N, 3) array.") + .def_property_readonly( + "taxel_forces", + [](py::object self) + { + const auto& view = self.cast&>(); + return struct_vector_view(self, view->taxel_forces()); + }, + "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_forces(); + return "TactileDataArray(taxels=" + std::to_string(forces != nullptr ? forces->size() : 0) + ")"; + }); + + bind_record(m, "TactileDataArrayRecord", "TactileDataArray"); +} + +} // namespace core diff --git a/src/core/schema/python/tactile_metadata_bindings.h b/src/core/schema/python/tactile_metadata_bindings.h new file mode 100644 index 0000000000..945158955c --- /dev/null +++ b/src/core/schema/python/tactile_metadata_bindings.h @@ -0,0 +1,148 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Encoded tactile metadata views. Pose arrays use rows (x, y, z, qx, qy, qz, qw). +// Complete-layout semantics are validated by TactileMetadata using the shared schema validator. + +#pragma once + +#include "schema_array_views.h" +#include "schema_serialized.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_tactile_metadata(py::module& m) +{ + using FloatRows = py::array_t; + using IndexRows = py::array_t; + + py::enum_(m, "SegmentSource") + .value("NONE", SegmentSource_NONE) + .value("HandSkeletonRef", SegmentSource_HandSkeletonRef); + py::enum_(m, "AlgorithmSource").value("NONE", AlgorithmSource_NONE).value("LBS", AlgorithmSource_LBS); + + serialized_class( + m, "HandSkeletonRef", "Ordered hand joint identifiers referenced by tactile segments.") + .def(py::init( + [](const std::vector& joint_ids) + { + HandSkeletonRefT native; + native.joint_ids = joint_ids; + return pack(native); + }), + py::arg("joint_ids")) + .def_property_readonly("joint_ids", + [](const Serialized& self) + { + // FlatBuffers exposes enum-vector storage as uint8_t. + std::vector joint_ids; + joint_ids.reserve(self->joint_ids()->size()); + for (const auto joint : *self->joint_ids()) + { + joint_ids.push_back(static_cast(joint)); + } + return joint_ids; + }); + + serialized_class(m, "LBS", "Linear blend skinning bind poses and four-slot influences.") + .def(py::init( + [](const FloatRows& segment_bind_poses, const FloatRows& taxel_bind_poses, + const IndexRows& taxel_segment_indices, const FloatRows& taxel_segment_weights) + { + LBST native; + native.segment_bind_poses = + structs_from_rows(segment_bind_poses, "segment_bind_poses"); + native.taxel_bind_poses = structs_from_rows(taxel_bind_poses, "taxel_bind_poses"); + native.taxel_segment_indices = + structs_from_rows(taxel_segment_indices, "taxel_segment_indices"); + native.taxel_segment_weights = + structs_from_rows(taxel_segment_weights, "taxel_segment_weights"); + return pack(native); + }), + py::arg("segment_bind_poses"), py::arg("taxel_bind_poses"), py::arg("taxel_segment_indices"), + py::arg("taxel_segment_weights"), + "Encode bind poses as (J, 7)/(N, 7) XYZ/XYZW rows and influences as (N, 4) arrays.") + .def_property_readonly( + "segment_bind_poses", [](py::object self) + { return struct_vector_view(self, self.cast&>()->segment_bind_poses()); }) + .def_property_readonly( + "taxel_bind_poses", [](py::object self) + { return struct_vector_view(self, self.cast&>()->taxel_bind_poses()); }) + .def_property_readonly("taxel_segment_indices", + [](py::object self) { + return struct_vector_view( + self, self.cast&>()->taxel_segment_indices()); + }) + .def_property_readonly( + "taxel_segment_weights", [](py::object self) + { return struct_vector_view(self, self.cast&>()->taxel_segment_weights()); }); + + serialized_class(m, "TactileMetadata", "Static tactile layout and its segment source.") + .def(py::init( + [](const Serialized& geometry_mapping, const Serialized& segment_source) + { + TactileMetadataT native; + LBST lbs; + geometry_mapping->UnPackTo(&lbs); + native.geometry_mapping.Set(std::move(lbs)); + HandSkeletonRefT hand; + segment_source->UnPackTo(&hand); + native.segment_source.Set(std::move(hand)); + auto metadata = pack(native); + try + { + verify_tactile_metadata(*metadata); + } + catch (const std::runtime_error& error) + { + throw py::value_error(error.what()); + } + return metadata; + }), + py::arg("geometry_mapping"), py::arg("segment_source")) + .def_property_readonly("geometry_mapping_type", field(&TactileMetadata::geometry_mapping_type)) + .def_property_readonly("segment_source_type", field(&TactileMetadata::segment_source_type)) + .def_property_readonly("geometry_mapping", + [](const Serialized& self) -> std::optional> + { + if (self->geometry_mapping_type() == AlgorithmSource_NONE) + { + return std::nullopt; + } + if (const auto* geometry_mapping = self->geometry_mapping_as_LBS()) + { + return self.narrow(geometry_mapping); + } + throw py::value_error("unsupported tactile geometry mapping"); + }) + .def_property_readonly("segment_source", + [](const Serialized& self) -> std::optional> + { + if (self->segment_source_type() == SegmentSource_NONE) + { + return std::nullopt; + } + if (const auto* segment_source = self->segment_source_as_HandSkeletonRef()) + { + return self.narrow(segment_source); + } + throw py::value_error("unsupported tactile segment source"); + }); +} + +} // 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 338327a763..37a9c27205 100644 --- a/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/__init__.py @@ -42,6 +42,7 @@ messageChannelConfig, ) from .haptic_sink import HapticSink +from .tactile_source import TactileSource from .deviceio_tensor_types import ( HeadPoseTrackedType, HandPoseTrackedType, @@ -56,6 +57,8 @@ SomaBodyJointPosesTrackedType, SomaHandJointRotationsTrackedType, SomaHandJointPosesTrackedType, + TactileDataTrackedType, + TactileMetadataTrackedType, DeviceIOHeadPoseTracked, DeviceIOHandPoseTracked, DeviceIOControllerSnapshotTracked, @@ -69,6 +72,8 @@ DeviceIOSomaBodyJointPosesTracked, DeviceIOSomaHandJointRotationsTracked, DeviceIOSomaHandJointPosesTracked, + DeviceIOTactileTracked, + DeviceIOTactileIndex, MessageChannelMessagesTrackedType, MessageChannelConnectionStatus, MessageChannelStatusType, @@ -110,6 +115,7 @@ "message_channel_config", "messageChannelConfig", "HapticSink", + "TactileSource", "HeadPoseTrackedType", "HandPoseTrackedType", "ControllerSnapshotTrackedType", @@ -123,6 +129,8 @@ "SomaBodyJointPosesTrackedType", "SomaHandJointRotationsTrackedType", "SomaHandJointPosesTrackedType", + "TactileDataTrackedType", + "TactileMetadataTrackedType", "MessageChannelMessagesTrackedType", "MessageChannelConnectionStatus", "MessageChannelStatusType", @@ -139,6 +147,8 @@ "DeviceIOSomaBodyJointPosesTracked", "DeviceIOSomaHandJointRotationsTracked", "DeviceIOSomaHandJointPosesTracked", + "DeviceIOTactileTracked", + "DeviceIOTactileIndex", "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 c062178318..f37a98013f 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 @@ -14,6 +14,7 @@ from typing import Any from ..interface.tensor_type import TensorType from ..interface.tensor_group_type import TensorGroupType +from ..tensor_types.indices import _create_index_enum from isaaccapture.schema import ( HeadPose, HandPose, @@ -29,6 +30,8 @@ SomaHandJointPoses, SomaHandJointRotations, MessageChannelMessagesTracked, + TactileDataArray, + TactileMetadata, ) @@ -157,6 +160,18 @@ class SomaHandJointPosesTrackedType(_PayloadTensorType): _payload_cls = SomaHandJointPoses +class TactileDataTrackedType(_PayloadTensorType): + """TactileDataArray payload from DeviceIO TactileTracker.""" + + _payload_cls = TactileDataArray + + +class TactileMetadataTrackedType(_PayloadTensorType): + """TactileMetadata metadata from DeviceIO TactileTracker.""" + + _payload_cls = TactileMetadata + + class MessageChannelMessagesTrackedType(_RequiredPayloadTensorType): """MessageChannelMessagesTracked batch from DeviceIO MessageChannelTracker.""" @@ -313,6 +328,27 @@ def DeviceIOSomaHandJointPosesTracked() -> TensorGroupType: ) +def DeviceIOTactileTracked() -> TensorGroupType: + """Tracked tactile frame and its metadata from DeviceIO TactileTracker. + + Contains: + tactile_tracked: TactileDataArray handle, or None when inactive + tactile_metadata: TactileMetadata metadata handle, or None until it arrives + """ + return TensorGroupType( + "deviceio_tactile", + [ + TactileDataTrackedType("tactile_tracked"), + TactileMetadataTrackedType("tactile_metadata"), + ], + ) + + +DeviceIOTactileIndex: Any = _create_index_enum( + "DeviceIOTactileIndex", DeviceIOTactileTracked(), "tactile_" +) + + 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..95112c1086 --- /dev/null +++ b/src/python/isaaccapture/retargeting_engine/deviceio_source_nodes/tactile_source.py @@ -0,0 +1,168 @@ +# 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 ``TactileDataArray`` into a force array, and the tracker's +``TactileMetadata`` 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 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 +from ..tensor_types import ( + TactileForces, + TactileLBSLayout, + TactileLBSLayoutIndex, +) +from .deviceio_tensor_types import DeviceIOTactileIndex, DeviceIOTactileTracked + +if TYPE_CHECKING: + from isaaccapture.deviceio_trackers import ITracker + from isaaccapture.schema import TactileDataArray, TactileMetadata + + +class TactileSource(IDeviceIOSource): + """Convert tactile frames and metadata into per-taxel forces and an LBS layout. + + Outputs (Optional): + - :data:`FORCES`: :func:`TactileForces` of ``num_taxels``, in ``TactileMetadata`` taxel + order; absent when the device is inactive. + - :data:`LAYOUT`: :func:`TactileLBSLayout` of ``num_segments`` x ``num_taxels``; absent + until the tracker holds the device's metadata. The tracker publishes no forces before then. + + A frame or metadata whose counts differ from the constructor's raises ``ValueError``. + Layout arrays are reused across frames; copy them before modifying them. + """ + + FORCES = "taxel_forces" + LAYOUT = "tactile_layout" + + def __init__( + self, + name: str, + collection_id: str, + num_segments: 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 ``TactileDataArray`` stream. + num_segments: Referenced joint count; fixes the LBS layout 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_segments = int(num_segments) + self._num_taxels = int(num_taxels) + self._layout_cache: ( + tuple[dict[TactileLBSLayoutIndex, np.ndarray], np.ndarray] | None + ) = None + 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[DeviceIOTactileIndex.TRACKED] = self._tracker.get_data(deviceio_session) + tg[DeviceIOTactileIndex.METADATA] = 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 LBS layout outputs (Optional -- may be absent).""" + return { + self.FORCES: OptionalType(TactileForces(self._num_taxels)), + self.LAYOUT: OptionalType( + TactileLBSLayout(self._num_segments, self._num_taxels) + ), + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Publish forces and a cached LBS layout; mark absent outputs with ``set_none()``.""" + data: "TactileDataArray | None" = inputs["deviceio_tactile"][ + DeviceIOTactileIndex.TRACKED + ] + metadata: "TactileMetadata | None" = inputs["deviceio_tactile"][ + DeviceIOTactileIndex.METADATA + ] + + forces_out = outputs[self.FORCES] + if data is None: + forces_out.set_none() + else: + forces = data.taxel_forces + 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 + + layout_out = outputs[self.LAYOUT] + if metadata is None: + layout_out.set_none() + return + geometry_mapping = metadata.geometry_mapping + segment_source = metadata.segment_source + if geometry_mapping is None or segment_source is None: + raise ValueError( + f"{self.name}: metadata must specify geometry_mapping and segment_source" + ) + # Normalize the schema stub's array type without copying its storage. + segment_bind_poses = np.asarray(geometry_mapping.segment_bind_poses) + if self._layout_cache is None or not np.shares_memory( + segment_bind_poses, self._layout_cache[1] + ): + num_segments, num_taxels = ( + len(segment_source.joint_ids), + len(geometry_mapping.taxel_bind_poses), + ) + if (num_segments, num_taxels) != (self._num_segments, self._num_taxels): + raise ValueError( + f"{self.name}: metadata has {num_segments} segments and {num_taxels} taxels, " + f"source expects {self._num_segments} and {self._num_taxels}" + ) + values = { + TactileLBSLayoutIndex.SEGMENT_JOINT_IDS: np.asarray( + segment_source.joint_ids, dtype=np.int32 + ), + TactileLBSLayoutIndex.SEGMENT_BIND_POSES: segment_bind_poses, + TactileLBSLayoutIndex.TAXEL_BIND_POSES: geometry_mapping.taxel_bind_poses, + TactileLBSLayoutIndex.TAXEL_SEGMENT_INDICES: geometry_mapping.taxel_segment_indices, + TactileLBSLayoutIndex.TAXEL_SEGMENT_WEIGHTS: geometry_mapping.taxel_segment_weights, + } + arrays = {} + for index, value in values.items(): + # Copy once: graph consumers must not alter the tracker's metadata baseline. + array = np.asarray(value) + arrays[index] = array.astype( + np.int32 if array.dtype == np.uint32 else array.dtype + ) + # Views keep the buffer alive and identify it across fresh Python wrappers. + self._layout_cache = (arrays, segment_bind_poses) + layout, _ = self._layout_cache + for index, array in layout.items(): + layout_out[index] = array diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py b/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py index 2454c5c44d..322e4554b3 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/__init__.py @@ -24,6 +24,8 @@ RobotHandJoints, ) from .tactile_types import ( + TactileForces, + TactileLBSLayout, TactileVector, TactileHeatmap, FingerPowerVector, @@ -42,6 +44,7 @@ Generic3AxisPedalInputIndex, FullBodyInputIndex, SomaBodyInputIndex, + TactileLBSLayoutIndex, HandJointIndex, BodyJointIndex, FingerIndex, @@ -72,6 +75,8 @@ "NUM_SOMA_BODY_JOINTS", "RobotHandJoints", # Tactile / haptic types + "TactileForces", + "TactileLBSLayout", "TactileVector", "TactileHeatmap", "FingerPowerVector", @@ -89,6 +94,7 @@ "Generic3AxisPedalInputIndex", "FullBodyInputIndex", "SomaBodyInputIndex", + "TactileLBSLayoutIndex", "HandJointIndex", "BodyJointIndex", "FingerIndex", diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py b/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py index 32fa7abcea..85eb57a502 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/indices.py @@ -25,6 +25,7 @@ FullBodyInput, SomaBodyInput, ) +from .tactile_types import TactileLBSLayout def _create_index_enum(name: str, group_type, prefix: str = "") -> IntEnum: @@ -59,6 +60,9 @@ def _create_index_enum(name: str, group_type, prefix: str = "") -> IntEnum: SomaBodyInputIndex: Any = _create_index_enum( "SomaBodyInputIndex", SomaBodyInput(), "soma_body_" ) +TactileLBSLayoutIndex: Any = _create_index_enum( + "TactileLBSLayoutIndex", TactileLBSLayout(0, 0) +) class HandJointIndex(IntEnum): diff --git a/src/python/isaaccapture/retargeting_engine/tensor_types/tactile_types.py b/src/python/isaaccapture/retargeting_engine/tensor_types/tactile_types.py index b64da00e5a..d4048f1709 100644 --- a/src/python/isaaccapture/retargeting_engine/tensor_types/tactile_types.py +++ b/src/python/isaaccapture/retargeting_engine/tensor_types/tactile_types.py @@ -1,7 +1,7 @@ # SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""TensorGroupType definitions for tactile feedback and haptic output. +"""TensorGroupType definitions for tactile input, feedback, and haptic output. Sim-side schemas (``TactileVector``, ``TactileHeatmap``) carry contact data into the retargeting pipeline; device-side schemas (``FingerPowerVector``, @@ -31,6 +31,40 @@ """Components in an :func:`EndEffectorForce`: ``[fx, fy, fz]``.""" +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_forces", num_taxels, 3)], + ) + + +def TactileLBSLayout(num_segments: int, num_taxels: int) -> TensorGroupType: + """LBS and HandSkeletonRef arrays, ordered by :class:`TactileLBSLayoutIndex`. + + HandJoint identifiers and segment indices are int32. Pose rows contain + position XYZ followed by quaternion XYZW, all in hand-root space. + """ + return TensorGroupType( + f"tactile_lbs_layout_{num_segments}x{num_taxels}", + [ + _int_array("segment_joint_ids", (num_segments,)), + _float_array("segment_bind_poses", num_segments, 7), + _float_array("taxel_bind_poses", num_taxels, 7), + _int_array("taxel_segment_indices", (num_taxels, 4)), + _float_array("taxel_segment_weights", num_taxels, 4), + ], + ) + + # ============================================================================ # Sim-side types # ============================================================================ diff --git a/src/python/isaaccapture/schema/__init__.py b/src/python/isaaccapture/schema/__init__.py index 9b484fafb1..b2a94c440e 100644 --- a/src/python/isaaccapture/schema/__init__.py +++ b/src/python/isaaccapture/schema/__init__.py @@ -56,6 +56,14 @@ # OGLO tactile glove types. OgloGloveSample, OgloGloveSampleRecord, + # Tactile types. + TactileDataArray, + TactileDataArrayRecord, + TactileMetadata, + HandSkeletonRef, + SegmentSource, + AlgorithmSource, + LBS, # Tracker metadata, for producers of a tracker's metadata. TRACKER_METADATA_MAX_SIZE, TRACKER_METADATA_TENSOR_IDENTIFIER, @@ -192,6 +200,14 @@ def __getattr__(name: str): # OGLO tactile glove types. "OgloGloveSample", "OgloGloveSampleRecord", + # Tactile types. + "TactileDataArray", + "TactileDataArrayRecord", + "TactileMetadata", + "HandSkeletonRef", + "SegmentSource", + "AlgorithmSource", + "LBS", # Tracker metadata. "TRACKER_METADATA_MAX_SIZE", "TRACKER_METADATA_TENSOR_IDENTIFIER", 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..4759840d0e 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 @@ -30,6 +33,7 @@ #include #include #include +#include #include #ifdef _WIN32 @@ -824,3 +828,104 @@ TEST_CASE("ReplaySession: keyboard tracker round-trip ignores live provider inpu session->update(); CHECK_FALSE(keyboard_tracker.get_data(*session)); } + +// ============================================================================= +// Tracker with metadata — TactileTracker (TactileMetadata 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::HandSkeletonRefT segment_source; + segment_source.joint_ids = { core::HandJoint_WRIST }; + core::LBST lbs; + lbs.segment_bind_poses = { make_pose(0, 0, 0) }; + lbs.taxel_bind_poses = { make_pose(0, 0, 0), make_pose(0, 0, 0) }; + lbs.taxel_segment_indices.resize(2); + const std::array weights = { 1, 0, 0, 0 }; + lbs.taxel_segment_weights.assign(2, core::Float4(flatbuffers::span(weights.data(), 4))); + core::TactileMetadataT native; + native.geometry_mapping.Set(std::move(lbs)); + native.segment_source.Set(std::move(segment_source)); + metadata.write(0, core::pack(native), 500); + } + + for (int i = 0; i < 2; ++i) + { + core::TactileDataArrayT frame; + frame.taxel_forces = { 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::TactileMetadata::GetFullyQualifiedName()) == "core.TactileMetadata"); + 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& metadata = tracker.get_metadata(*session); + REQUIRE(metadata); + REQUIRE(metadata->geometry_mapping_as_LBS()); + CHECK(metadata->geometry_mapping_as_LBS()->taxel_bind_poses()->size() == 2); + + for (int i = 0; i < 2; ++i) + { + session->update(); + const auto& frame = tracker.get_data(*session); + REQUIRE(frame); + CHECK(frame->taxel_forces()->Get(0)->x() == static_cast(i)); + CHECK(tracker.get_metadata(*session).get() == metadata.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 3fa2548b78..d1e59cd488 100644 --- a/tests/cpp/core/schema/CMakeLists.txt +++ b/tests/cpp/core/schema/CMakeLists.txt @@ -22,6 +22,8 @@ add_executable(schema_tests test_soma_body_joint_poses.cpp test_soma_hand_joint_rotations.cpp test_soma_hand_joint_poses.cpp + test_tactile_array.cpp + test_tactile_metadata.cpp test_schema_conform.cpp ) target_link_libraries(schema_tests PRIVATE diff --git a/tests/cpp/core/schema/test_tactile_array.cpp b/tests/cpp/core/schema/test_tactile_array.cpp new file mode 100644 index 0000000000..2646e2cf48 --- /dev/null +++ b/tests/cpp/core/schema/test_tactile_array.cpp @@ -0,0 +1,97 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Unit tests for the generated TactileDataArray FlatBuffer message. + +#include +#include +#include +#include +#include +#include + +#include +#include + +// VT_ == (id + 2) * 2. +#define VT(field) (field + 2) * 2 +// Field renames change generated VT_* symbols without changing field IDs. +static_assert(core::TactileDataArray::VT_TAXEL_FORCES == VT(0)); +static_assert(core::TactileDataArrayRecord::VT_DATA == VT(0)); +static_assert(core::TactileDataArrayRecord::VT_TIMESTAMP == VT(1)); + +// Producers copy XrVector3f arrays straight into taxel_forces. +static_assert(sizeof(core::Point) == sizeof(XrVector3f)); + +namespace +{ + +core::TactileDataArrayT make_frame() +{ + core::TactileDataArrayT native; + native.taxel_forces = { core::Point(0.0f, 0.0f, 1.0f), core::Point(0.5f, -0.5f, 2.0f) }; + return native; +} + +} // namespace + +TEST_CASE("TactileDataArray round-trips every field", "[unit][tactile]") +{ + const auto data = core::pack(make_frame()); + + REQUIRE(data); + REQUIRE(data->taxel_forces() != nullptr); + REQUIRE(data->taxel_forces()->size() == 2); + CHECK(data->taxel_forces()->Get(1)->x() == 0.5f); + CHECK(data->taxel_forces()->Get(1)->z() == 2.0f); +} + +TEST_CASE("TactileDataArray keeps its required vector present for an empty frame", "[unit][tactile]") +{ + const auto data = core::pack(core::TactileDataArrayT{}); + + REQUIRE(data); + REQUIRE(data->taxel_forces() != nullptr); + CHECK(data->taxel_forces()->size() == 0); + flatbuffers::Verifier verifier(data.buffer().data(), data.buffer().size()); + CHECK(verifier.VerifyBuffer()); +} + +TEST_CASE("TactileDataArray verification rejects a missing force vector", "[unit][tactile]") +{ + flatbuffers::FlatBufferBuilder builder; + // Build an unchecked empty table to test verification without required-field builder assertions. + const auto start = builder.StartTable(); + builder.Finish(flatbuffers::Offset(builder.EndTable(start))); + flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); + CHECK_FALSE(verifier.VerifyBuffer()); +} + +TEST_CASE("TactileDataArrayRecord records unavailable data with its timestamp", "[unit][tactile]") +{ + core::TactileDataArrayRecordT native; + native.timestamp = std::make_shared(1, 2, 3); + const auto record = core::pack(native); + + flatbuffers::Verifier verifier(record.buffer().data(), record.buffer().size()); + REQUIRE(core::VerifyTactileDataArrayRecordBuffer(verifier)); + CHECK(record->data() == nullptr); + REQUIRE(record->timestamp() != nullptr); + CHECK(record->timestamp()->sample_time_local_common_clock() == 2); +} + +TEST_CASE("TactileDataArrayRecord verifies and carries its timestamp", "[unit][tactile]") +{ + core::TactileDataArrayRecordT record; + record.data = std::make_shared(make_frame()); + record.timestamp = std::make_shared(1, 2, 3); + + flatbuffers::FlatBufferBuilder builder; + builder.Finish(core::TactileDataArrayRecord::Pack(builder, &record)); + + flatbuffers::Verifier verifier(builder.GetBufferPointer(), builder.GetSize()); + REQUIRE(core::VerifyTactileDataArrayRecordBuffer(verifier)); + const auto* read = core::GetTactileDataArrayRecord(builder.GetBufferPointer()); + CHECK(read->timestamp()->sample_time_local_common_clock() == 2); + CHECK(read->data()->taxel_forces()->size() == 2); +} diff --git a/tests/cpp/core/schema/test_tactile_metadata.cpp b/tests/cpp/core/schema/test_tactile_metadata.cpp new file mode 100644 index 0000000000..ceca5e3169 --- /dev/null +++ b/tests/cpp/core/schema/test_tactile_metadata.cpp @@ -0,0 +1,306 @@ +// SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#define VT(field) (field + 2) * 2 +static_assert(core::TactileMetadata::VT_GEOMETRY_MAPPING_TYPE == VT(0)); +static_assert(core::TactileMetadata::VT_GEOMETRY_MAPPING == VT(1)); +static_assert(core::TactileMetadata::VT_SEGMENT_SOURCE_TYPE == VT(2)); +static_assert(core::TactileMetadata::VT_SEGMENT_SOURCE == VT(3)); +static_assert(core::HandSkeletonRef::VT_JOINT_IDS == VT(0)); +static_assert(core::LBS::VT_SEGMENT_BIND_POSES == VT(0)); +static_assert(core::LBS::VT_TAXEL_BIND_POSES == VT(1)); +static_assert(core::LBS::VT_TAXEL_SEGMENT_INDICES == VT(2)); +static_assert(core::LBS::VT_TAXEL_SEGMENT_WEIGHTS == VT(3)); +static_assert(sizeof(core::UInt4) == 4 * sizeof(uint32_t)); +static_assert(sizeof(core::Float4) == 4 * sizeof(float)); + +namespace +{ + +core::TactileMetadataT make_metadata() +{ + core::HandSkeletonRefT segment_source; + segment_source.joint_ids = { core::HandJoint_WRIST, core::HandJoint_INDEX_TIP }; + core::LBST lbs; + lbs.segment_bind_poses = { core::Pose(core::Point(0, 0, 0), core::Quaternion(0, 0, 0, 1)), + core::Pose(core::Point(0, 0.1f, 0), core::Quaternion(0, 0, 0, 1)) }; + lbs.taxel_bind_poses = { core::Pose(core::Point(0, 0.09f, 0.01f), core::Quaternion(1, 0, 0, 0)) }; + const std::array indices = { 1, 0, 0, 0 }; + const std::array weights = { 0.75f, 0.25f, 0.0f, 0.0f }; + lbs.taxel_segment_indices.emplace_back(flatbuffers::span(indices.data(), 4)); + lbs.taxel_segment_weights.emplace_back(flatbuffers::span(weights.data(), 4)); + core::TactileMetadataT native; + native.geometry_mapping.Set(std::move(lbs)); + native.segment_source.Set(std::move(segment_source)); + return native; +} + +} // namespace + +TEST_CASE("TactileMetadata round-trips union tags and segment-local indices", "[unit][tactile_metadata]") +{ + const auto metadata = core::pack(make_metadata()); + CHECK(metadata->geometry_mapping_type() == core::AlgorithmSource_LBS); + CHECK(metadata->segment_source_type() == core::SegmentSource_HandSkeletonRef); + const auto* segment_source = metadata->segment_source_as_HandSkeletonRef(); + const auto* lbs = metadata->geometry_mapping_as_LBS(); + REQUIRE(segment_source); + REQUIRE(lbs); + REQUIRE(segment_source->joint_ids()->size() == 2); + CHECK(segment_source->joint_ids()->Get(1) == core::HandJoint_INDEX_TIP); + CHECK(lbs->taxel_segment_indices()->Get(0)->values()->Get(0) == 1); + CHECK(lbs->taxel_segment_weights()->Get(0)->values()->Get(1) == 0.25f); + CHECK(lbs->segment_bind_poses()->Get(1)->position().y() == 0.1f); + CHECK(lbs->taxel_bind_poses()->Get(0)->orientation().x() == 1.0f); +} + +TEST_CASE("TactileMetadata verifies and retains nested union views", "[unit][tactile_metadata]") +{ + auto metadata = core::pack(make_metadata()); + const auto bytes = metadata.buffer(); + flatbuffers::Verifier verifier(bytes.data(), bytes.size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + CHECK(std::string(core::TactileMetadata::GetFullyQualifiedName()) == "core.TactileMetadata"); + auto lbs = metadata.narrow(metadata->geometry_mapping_as_LBS()); + metadata.reset(); + REQUIRE(lbs); + CHECK(lbs->taxel_segment_weights()->Get(0)->values()->Get(0) == 0.75f); +} + +TEST_CASE("TactileMetadata semantic verification accepts valid metadata", "[unit][tactile_metadata]") +{ + const auto metadata = core::pack(make_metadata()); + CHECK_NOTHROW(core::verify_metadata(*metadata)); + CHECK_NOTHROW(core::verify_metadata(*metadata, "tactile/right")); +} + +TEST_CASE("TactileMetadata semantic verification rejects inconsistent lengths", "[unit][tactile_metadata]") +{ + auto native = make_metadata(); + auto& lbs = *native.geometry_mapping.AsLBS(); + std::string reason; + SECTION("taxel bind poses") + { + lbs.taxel_bind_poses.push_back(lbs.taxel_bind_poses.front()); + reason = "taxel_segment_indices: expected 2 rows to match taxel_bind_poses, got 1"; + } + SECTION("taxel segment indices") + { + lbs.taxel_segment_indices.clear(); + reason = "taxel_segment_indices: expected 1 rows to match taxel_bind_poses, got 0"; + } + SECTION("taxel segment weights") + { + lbs.taxel_segment_weights.clear(); + reason = "taxel_segment_weights: expected 1 rows to match taxel_bind_poses, got 0"; + } + SECTION("joint identifiers and segment bind poses") + { + native.segment_source.AsHandSkeletonRef()->joint_ids.pop_back(); + reason = "segment_bind_poses: expected 1 poses to match segment_source.joint_ids, got 2"; + } + const auto metadata = core::pack(native); + flatbuffers::Verifier verifier(metadata.buffer().data(), metadata.buffer().size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + CHECK_THROWS_WITH(core::verify_metadata(*metadata), "TactileMetadata." + reason); + CHECK_THROWS_WITH(core::verify_metadata(*metadata, "tactile/right"), "tactile/right: TactileMetadata." + reason); +} + +TEST_CASE("TactileMetadata semantic verification rejects invalid or repeated joints", "[unit][tactile_metadata]") +{ + auto native = make_metadata(); + auto& joint_ids = native.segment_source.AsHandSkeletonRef()->joint_ids; + std::string reason = "invalid HandJoint identifier"; + SECTION("sentinel") + { + joint_ids[1] = core::HandJoint_NUM_JOINTS; + } + SECTION("unknown enum value") + { + joint_ids[1] = static_cast(255); + } + SECTION("duplicate identifier") + { + joint_ids[1] = joint_ids[0]; + reason = "duplicate HandJoint identifier"; + } + const auto metadata = core::pack(native); + flatbuffers::Verifier verifier(metadata.buffer().data(), metadata.buffer().size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + CHECK_THROWS_WITH(core::verify_metadata(*metadata), + Catch::Matchers::ContainsSubstring("TactileMetadata.joint_ids[1]: " + reason)); +} + +TEST_CASE("TactileMetadata semantic verification rejects out-of-range segment indices", "[unit][tactile_metadata]") +{ + auto native = make_metadata(); + auto& lbs = *native.geometry_mapping.AsLBS(); + std::array indices = { 1, 0, 0, 0 }; + std::string slot; + SECTION("nonzero-weight slot") + { + indices[0] = 2; + slot = "0"; + } + SECTION("zero-weight slot") + { + indices[3] = 2; + slot = "3"; + } + lbs.taxel_segment_indices[0] = core::UInt4(flatbuffers::span(indices.data(), 4)); + const auto metadata = core::pack(native); + flatbuffers::Verifier verifier(metadata.buffer().data(), metadata.buffer().size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + CHECK_THROWS_WITH(core::verify_metadata(*metadata), "TactileMetadata.taxel_segment_indices[0][" + slot + + "]: segment index 2 is outside segment_bind_poses (size 2)"); +} + +TEST_CASE("TactileMetadata semantic verification checks each taxel's weights", "[unit][tactile_metadata]") +{ + auto native = make_metadata(); + auto& lbs = *native.geometry_mapping.AsLBS(); + std::array weights = { 0.75f, 0.25f, 0.0f, 0.0f }; + bool valid = false; + std::string reason = "taxel_segment_weights[1]: weights must sum to one"; + SECTION("negative weight with sum one") + { + weights = { 1.25f, -0.25f, 0.0f, 0.0f }; + reason = "taxel_segment_weights[1][1]: weights must be finite and nonnegative"; + } + SECTION("sum below one") + { + weights = { 0.25f, 0.25f, 0.0f, 0.0f }; + } + SECTION("sum above one") + { + weights = { 0.75f, 0.75f, 0.0f, 0.0f }; + } + SECTION("NaN") + { + weights[3] = std::numeric_limits::quiet_NaN(); + reason = "taxel_segment_weights[1][3]: weights must be finite and nonnegative"; + } + SECTION("infinity") + { + weights[3] = std::numeric_limits::infinity(); + reason = "taxel_segment_weights[1][3]: weights must be finite and nonnegative"; + } + SECTION("rounding within tolerance") + { + weights = { 0.75f, 0.250005f, 0.0f, 0.0f }; + valid = true; + } + SECTION("rounding outside tolerance") + { + weights = { 0.75f, 0.25002f, 0.0f, 0.0f }; + } + // Place the changed weights after a valid taxel to exercise the whole vector. + lbs.taxel_bind_poses.push_back(lbs.taxel_bind_poses.front()); + lbs.taxel_segment_indices.push_back(lbs.taxel_segment_indices.front()); + lbs.taxel_segment_weights.emplace_back(flatbuffers::span(weights.data(), 4)); + const auto metadata = core::pack(native); + flatbuffers::Verifier verifier(metadata.buffer().data(), metadata.buffer().size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + if (valid) + { + CHECK_NOTHROW(core::verify_metadata(*metadata)); + } + else + { + CHECK_THROWS_WITH( + core::verify_metadata(*metadata), Catch::Matchers::ContainsSubstring("TactileMetadata." + reason)); + } +} + +TEST_CASE("TactileMetadata semantic verification rejects nonfinite bind poses", "[unit][tactile_metadata]") +{ + const bool segment = GENERATE(true, false); + const auto component = GENERATE(0, 1, 2, 3, 4, 5, 6); + const auto value = GENERATE(std::numeric_limits::quiet_NaN(), std::numeric_limits::infinity(), + -std::numeric_limits::infinity()); + const std::string field = segment ? "segment_bind_poses" : "taxel_bind_poses"; + CAPTURE(field, component, value); + auto native = make_metadata(); + auto& lbs = *native.geometry_mapping.AsLBS(); + auto& poses = segment ? lbs.segment_bind_poses : lbs.taxel_bind_poses; + std::array row = { 0, 0, 0, 0, 0, 0, 1 }; + row[component] = value; + poses.back() = core::Pose(core::Point(row[0], row[1], row[2]), core::Quaternion(row[3], row[4], row[5], row[6])); + const auto metadata = core::pack(native); + flatbuffers::Verifier verifier(metadata.buffer().data(), metadata.buffer().size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + CHECK_THROWS_WITH( + core::verify_metadata(*metadata), + Catch::Matchers::ContainsSubstring(field + "[" + std::to_string(poses.size() - 1) + "]: poses must be finite")); +} + +TEST_CASE("TactileMetadata semantic verification checks bind quaternion norms", "[unit][tactile_metadata]") +{ + const bool segment = GENERATE(true, false); + const std::string field = segment ? "segment_bind_poses" : "taxel_bind_poses"; + CAPTURE(field); + auto native = make_metadata(); + auto& lbs = *native.geometry_mapping.AsLBS(); + auto& poses = segment ? lbs.segment_bind_poses : lbs.taxel_bind_poses; + core::Quaternion orientation; + bool valid = false; + SECTION("zero quaternion") + { + } + SECTION("scaled quaternion") + { + orientation = core::Quaternion(0, 0, 0, 2); + } + SECTION("finite components with overflowing squared norm") + { + orientation = core::Quaternion(std::numeric_limits::max(), 0, 0, 0); + } + SECTION("rotation with all components nonzero") + { + orientation = core::Quaternion(0.5f, 0.5f, 0.5f, 0.5f); + valid = true; + } + SECTION("negative unit quaternion") + { + orientation = core::Quaternion(0, 0, 0, -1); + valid = true; + } + SECTION("rounding within squared-norm tolerance") + { + orientation = core::Quaternion(0, 0, 0, std::sqrt(1.0f + 0.5e-4f)); + valid = true; + } + SECTION("rounding outside squared-norm tolerance") + { + orientation = core::Quaternion(0, 0, 0, std::sqrt(1.0f + 2e-4f)); + } + poses.back() = core::Pose(poses.back().position(), orientation); + const auto metadata = core::pack(native); + flatbuffers::Verifier verifier(metadata.buffer().data(), metadata.buffer().size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + if (valid) + { + CHECK_NOTHROW(core::verify_metadata(*metadata)); + } + else + { + CHECK_THROWS_WITH(core::verify_metadata(*metadata), + Catch::Matchers::ContainsSubstring(field + "[" + std::to_string(poses.size() - 1) + + "]: orientations must be unit quaternions")); + } +} diff --git a/tests/cpp/core/tracker_metadata/fake_tensor_runtime.hpp b/tests/cpp/core/tracker_metadata/fake_tensor_runtime.hpp index 1aca73e27d..b0c01d7ff3 100644 --- a/tests/cpp/core/tracker_metadata/fake_tensor_runtime.hpp +++ b/tests/cpp/core/tracker_metadata/fake_tensor_runtime.hpp @@ -58,7 +58,8 @@ class FakeTensorRuntime //! Creates `collection_id` with samples of `sample_size` bytes, as a producer starting up does. void create(const std::string& collection_id, uint32_t sample_size) { - m_collections.push_back(std::make_shared(Collection{ collection_id, sample_size })); + m_collections.push_back( + std::make_shared(Collection{ collection_id, sample_size, m_next_collection_id++ })); ++m_generation; } @@ -96,6 +97,7 @@ class FakeTensorRuntime { std::string id; uint32_t sample_size; + XrTensorCollectionIDNV instance_id; std::vector samples{}; int64_t next_index = 0; bool destroyed = false; @@ -160,6 +162,7 @@ class FakeTensorRuntime properties->data.tensorCount = 1; properties->data.totalSampleSize = collection.sample_size; properties->sampleBatchStride = collection.sample_size; + properties->tensorCollectionId = collection.instance_id; return XR_SUCCESS; } @@ -231,6 +234,7 @@ class FakeTensorRuntime std::vector> m_collections; std::map m_lists; uintptr_t m_next_list = 1; + XrTensorCollectionIDNV m_next_collection_id = 1; uint64_t m_generation = 1; }; diff --git a/tests/cpp/core/tracker_metadata/test_tracker_metadata.cpp b/tests/cpp/core/tracker_metadata/test_tracker_metadata.cpp index 44bdd45385..e14b81323a 100644 --- a/tests/cpp/core/tracker_metadata/test_tracker_metadata.cpp +++ b/tests/cpp/core/tracker_metadata/test_tracker_metadata.cpp @@ -14,21 +14,28 @@ #include #include +#include #include #include #include #include +#include +#include #include +#include +#include #include #include #include +#include #include #include #include #include #include #include +#include #include namespace @@ -265,8 +272,118 @@ TEST_CASE("Replay withholds data recorded without metadata", "[unit][tracker_met CHECK_FALSE(replay->get_data()); } +TEST_CASE("Replay tactile metadata is semantically verified before force data is published", "[unit][tracker_metadata]") +{ + constexpr std::string_view base_name = "tactile"; + const auto path = temp_mcap_path("test_tactile_replay_metadata"); + const TempFileCleanup cleanup(path); + std::array indices{}; + std::array weights{ 1.0f, 0.0f, 0.0f, 0.0f }; + std::string reason; + SECTION("valid metadata") + { + } + SECTION("segment index outside the single segment") + { + indices[0] = 7; + reason = "taxel_segment_indices[0][0]: segment index 7 is outside segment_bind_poses (size 1)"; + } + SECTION("all four weights are zero") + { + weights = {}; + reason = "taxel_segment_weights[0]: weights must sum to one"; + } + + core::HandSkeletonRefT hand; + hand.joint_ids = { core::HandJoint_WRIST }; + core::LBST lbs; + lbs.segment_bind_poses.emplace_back(core::Point(0, 0, 0), core::Quaternion(0, 0, 0, 1)); + lbs.taxel_bind_poses = lbs.segment_bind_poses; + lbs.taxel_segment_indices.emplace_back(flatbuffers::span(indices.data(), 4)); + lbs.taxel_segment_weights.emplace_back(flatbuffers::span(weights.data(), 4)); + core::TactileMetadataT native; + native.geometry_mapping.Set(std::move(lbs)); + native.segment_source.Set(std::move(hand)); + const auto metadata = core::pack(native); + flatbuffers::Verifier verifier(metadata.buffer().data(), metadata.buffer().size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + + auto writer = open_writer(path); + auto channels = core::LiveTactileTrackerImpl::create_mcap_channels(*writer, base_name); + channels->metadata.write(0, metadata, 5); + core::TactileDataArrayT forces; + forces.taxel_forces.emplace_back(0.0f, 0.0f, 1.0f); + channels->data.write( + 1, core::pack_record(&forces, core::DeviceDataTimestamp(10, 10, 10))); + writer->close(); + + const auto recorded = recorded_schemas(path); + if (!reason.empty()) + { + REQUIRE_THROWS_WITH(core::verify_metadata(*metadata), Catch::Matchers::ContainsSubstring(reason)); + CHECK_THROWS_WITH(core::ReplayTactileTrackerImpl::read_metadata(open_reader(path), base_name, recorded), + Catch::Matchers::StartsWith(std::string(base_name) + ": TactileMetadata." + reason)); + return; + } + + REQUIRE_NOTHROW(core::verify_metadata(*metadata)); + auto loaded = core::ReplayTactileTrackerImpl::read_metadata(open_reader(path), base_name, recorded); + core::ReplayTactileTrackerImpl replay(open_reader(path), base_name, recorded, std::move(loaded)); + REQUIRE(replay.get_metadata()); + replay.update(0); + REQUIRE(replay.get_data()); + REQUIRE(replay.get_data()->taxel_forces()->size() == 1); + CHECK(replay.get_data()->taxel_forces()->Get(0)->z() == 1.0f); +} + #if defined(XR_USE_TIMESPEC) +TEST_CASE("Live tactile metadata is semantically verified before adoption and recording", "[unit][tracker_metadata]") +{ + const auto path = temp_mcap_path("test_tactile_metadata"); + const TempFileCleanup cleanup(path); + { + FakeTensorRuntime runtime; + auto writer = open_writer(path); + core::McapTrackerChannels channels(*writer, BASE_NAME, { "metadata" }); + core::SchemaMetadataTracker tracker(runtime.handles(), DATA_COLLECTION, &channels); + runtime.create(METADATA_COLLECTION, core::TRACKER_METADATA_MAX_SIZE); + + core::HandSkeletonRefT hand; + hand.joint_ids = { core::HandJoint_WRIST }; + core::LBST lbs; + lbs.segment_bind_poses.emplace_back(core::Point(0, 0, 0), core::Quaternion(0, 0, 0, 1)); + lbs.taxel_bind_poses = lbs.segment_bind_poses; + const std::array indices{}; + const std::array zero_weights{}; + lbs.taxel_segment_indices.emplace_back(flatbuffers::span(indices.data(), 4)); + lbs.taxel_segment_weights.emplace_back(flatbuffers::span(zero_weights.data(), 4)); + core::TactileMetadataT native; + native.geometry_mapping.Set(std::move(lbs)); + native.segment_source.Set(std::move(hand)); + const auto invalid = core::pack(native); + flatbuffers::Verifier verifier(invalid.buffer().data(), invalid.buffer().size()); + REQUIRE(core::VerifyTactileMetadataBuffer(verifier)); + + runtime.push(METADATA_COLLECTION, invalid.buffer(), 1); + CHECK_THROWS_WITH(tracker.update(), + Catch::Matchers::StartsWith(METADATA_COLLECTION + ": TactileMetadata.") && + Catch::Matchers::ContainsSubstring("taxel_segment_weights[0]: weights must sum to one")); + CHECK_FALSE(tracker.get()); + + const std::array weights = { 1.0f, 0.0f, 0.0f, 0.0f }; + native.geometry_mapping.AsLBS()->taxel_segment_weights[0] = + core::Float4(flatbuffers::span(weights.data(), 4)); + const auto valid = core::pack(native); + runtime.push(METADATA_COLLECTION, valid.buffer(), 2); + CHECK(tracker.update()); + REQUIRE(tracker.get()); + CHECK(tracker.get()->geometry_mapping_as_LBS()->taxel_segment_weights()->Get(0)->values()->Get(0) == 1.0f); + writer->close(); + } + CHECK(read_channel(path, "metadata").size() == 1); +} + TEST_CASE("Live publishes and records data only once the producer's metadata arrives", "[unit][tracker_metadata]") { const auto path = temp_mcap_path("test_tracker_metadata"); @@ -343,6 +460,41 @@ TEST_CASE("A restarted producer's data waits for its metadata, however it is pad CHECK(live.step() == 0.3f); } +TEST_CASE("A recreated metadata collection is rechecked at the same list index", "[unit][tracker_metadata]") +{ + LiveSession live; + live.start_data(); + live.start_metadata(); + live.push_metadata(make_metadata("glove")); + live.push_data(0.1f); + REQUIRE(live.step() == 0.1f); + const auto first = live.impl->get_metadata(); + + // No polling between destruction and recreation: the list position is reused. + live.runtime.destroy(METADATA_COLLECTION); + live.start_metadata(); + live.push_data(0.2f); + + SECTION("the original metadata is accepted") + { + live.push_metadata(make_metadata("glove")); + CHECK(live.step() == 0.2f); + CHECK(live.impl->get_metadata().buffer().data() == first.buffer().data()); + } + SECTION("data waits for the recreated collection's sample") + { + CHECK_FALSE(live.step()); + live.push_metadata(make_metadata("glove")); + live.push_data(0.3f); + CHECK(live.step() == 0.3f); + } + SECTION("different metadata is rejected") + { + live.push_metadata(make_metadata("other")); + CHECK_THROWS_WITH(live.step(), Catch::Matchers::ContainsSubstring("differs")); + } +} + TEST_CASE("Different metadata is an error naming its collection", "[unit][tracker_metadata]") { using Catch::Matchers::ContainsSubstring; diff --git a/tests/python/core/CMakeLists.txt b/tests/python/core/CMakeLists.txt index 1c96b71c32..c96d5abc4c 100644 --- a/tests/python/core/CMakeLists.txt +++ b/tests/python/core/CMakeLists.txt @@ -6,6 +6,7 @@ add_subdirectory(retargeting_engine) add_subdirectory(teleop_session_manager) if(BUILD_PYTHON_BINDINGS) + add_subdirectory(deviceio_trackers) add_subdirectory(cloudxr) # Needs `import isaaccapture`, and drives the C++ half through the emitter # built under tests/cpp/core/log_bridge. diff --git a/tests/python/core/deviceio_trackers/CMakeLists.txt b/tests/python/core/deviceio_trackers/CMakeLists.txt new file mode 100644 index 0000000000..101a9234ba --- /dev/null +++ b/tests/python/core/deviceio_trackers/CMakeLists.txt @@ -0,0 +1,12 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +add_test( + NAME deviceio_trackers_test_defaults + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev pytest -v --tb=short test_defaults.py + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" +) + +set_tests_properties(deviceio_trackers_test_defaults PROPERTIES + ENVIRONMENT "PYTHONPATH=${CMAKE_BINARY_DIR}/python_package/$" +) diff --git a/tests/python/core/deviceio_trackers/pyproject.toml b/tests/python/core/deviceio_trackers/pyproject.toml new file mode 100644 index 0000000000..deeaa91e92 --- /dev/null +++ b/tests/python/core/deviceio_trackers/pyproject.toml @@ -0,0 +1,13 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "isaaccapture-deviceio-trackers-tests" +version = "0.0.0" +requires-python = ">=3.11,<3.14" + +[project.optional-dependencies] +dev = ["pytest", "numpy"] + +[tool.pytest.ini_options] +pythonpath = [".", "../.."] diff --git a/tests/python/core/deviceio_trackers/test_defaults.py b/tests/python/core/deviceio_trackers/test_defaults.py new file mode 100644 index 0000000000..fe77503ede --- /dev/null +++ b/tests/python/core/deviceio_trackers/test_defaults.py @@ -0,0 +1,30 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Check generated tracker defaults through the real Python bindings.""" + +import runpy + +import pytest +from repo_paths import repo_root + +from isaaccapture import deviceio_trackers + +_core = repo_root() / "src" / "core" +_manifest = runpy.run_path(str(_core / "codegen" / "manifest.py")) +_entries = _manifest["load_manifest"]( + _core / "deviceio_trackers" / "trackers.toml", + _core / "deviceio_trackers" / "defaults.toml", +) + + +@pytest.mark.parametrize( + "entry", + [entry for entry in _entries if entry["direction"] == "pull"], + ids=lambda entry: entry["class"], +) +def test_default_flatbuffer_size_matches_manifest(entry): + tracker_type = getattr(deviceio_trackers, entry["class"]) + size = tracker_type.DEFAULT_MAX_FLATBUFFER_SIZE + assert isinstance(size, int) + assert size == entry["max_flatbuffer_size"] 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..4d6839dc05 --- /dev/null +++ b/tests/python/core/retargeting_engine/test_tactile_sources.py @@ -0,0 +1,215 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for the TactileSource DeviceIO converter. + +Payloads use real schema bindings; polling uses a mock tracker. +""" + +from unittest.mock import MagicMock + +import numpy as np +import pytest + +from isaaccapture.retargeting_engine.deviceio_source_nodes import ( + DeviceIOTactileIndex, + TactileSource, +) +from isaaccapture.retargeting_engine.tensor_types import TactileLBSLayoutIndex +from isaaccapture.retargeting_engine.interface.base_retargeter import _make_output_group +from isaaccapture.retargeting_engine.interface.tensor_group import TensorGroup +from isaaccapture.schema import ( + HandJoint, + HandSkeletonRef, + LBS, + TactileDataArray, + TactileMetadata, +) + +NUM_SEGMENTS = 2 +NUM_TAXELS = 3 + + +def _source(**kwargs) -> TactileSource: + args = dict( + collection_id="tactile/right", num_segments=NUM_SEGMENTS, num_taxels=NUM_TAXELS + ) + args.update(kwargs) + return TactileSource("tactile", **args) + + +def _inputs(source, data, metadata) -> dict: + tg = TensorGroup(source.input_spec()["deviceio_tactile"]) + tg[DeviceIOTactileIndex.TRACKED] = data + tg[DeviceIOTactileIndex.METADATA] = metadata + return {"deviceio_tactile": tg} + + +def _outputs(source) -> dict: + return {name: _make_output_group(gt) for name, gt in source.output_spec().items()} + + +def _metadata( + num_segments: int = NUM_SEGMENTS, + num_taxels: int = NUM_TAXELS, + *, + taxel_x: float = 0.01, +) -> TactileMetadata: + indices = np.zeros((num_taxels, 4), np.uint32) + weights = np.zeros((num_taxels, 4), np.float32) + weights[:, 0] = 1.0 + segment_poses = np.zeros((num_segments, 7), np.float32) + segment_poses[:, 6] = 1 + taxel_poses = np.zeros((num_taxels, 7), np.float32) + taxel_poses[:, :3] = (taxel_x, 0.01, 0.01) + taxel_poses[:, 6] = 1 + return TactileMetadata( + LBS(segment_poses, taxel_poses, indices, weights), + HandSkeletonRef([HandJoint(i) for i in range(num_segments)]), + ) + + +class TestTactileSource: + def test_creation_and_tracker(self): + assert _source().get_tracker().get_name() == "TactileTracker" + + def test_poll_reads_data_and_metadata_from_one_tracker(self): + src = _source() + data = TactileDataArray(np.zeros((NUM_TAXELS, 3), np.float32)) + metadata = _metadata() + tracker = MagicMock() + tracker.get_data.return_value = data + tracker.get_metadata.return_value = metadata + src._tracker = tracker + session = object() + + inputs = src.poll_tracker(session) + + tracker.get_data.assert_called_once_with(session) + tracker.get_metadata.assert_called_once_with(session) + assert inputs["deviceio_tactile"][DeviceIOTactileIndex.TRACKED] is data + assert inputs["deviceio_tactile"][DeviceIOTactileIndex.METADATA] is metadata + + 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.LAYOUT].is_optional + + def test_active_conversion(self): + src = _source() + forces = np.arange(9, dtype=np.float32).reshape(3, 3) + outputs = _outputs(src) + src.compute(_inputs(src, TactileDataArray(forces), _metadata()), outputs) + + np.testing.assert_array_equal( + np.asarray(outputs[TactileSource.FORCES][0]), forces + ) + group = outputs[TactileSource.LAYOUT] + assert not group.is_none + joints = np.asarray(group[TactileLBSLayoutIndex.SEGMENT_JOINT_IDS]) + assert joints.dtype == np.int32 + assert joints.tolist() == [0, 1] + assert ( + np.asarray(group[TactileLBSLayoutIndex.TAXEL_SEGMENT_INDICES]).dtype + == np.int32 + ) + np.testing.assert_array_equal( + np.asarray(group[TactileLBSLayoutIndex.TAXEL_BIND_POSES])[:, 3:], + np.tile([0.0, 0.0, 0.0, 1.0], (NUM_TAXELS, 1)), + ) + + def test_layout_outputs_do_not_alias_the_metadata(self): + src = _source() + metadata = _metadata() + encoded = metadata.to_bytes() + outputs = _outputs(src) + src.compute(_inputs(src, None, metadata), outputs) + + group = outputs[TactileSource.LAYOUT] + for index in TactileLBSLayoutIndex: + array = np.asarray(group[index]) + array[...] = 7 + assert metadata.to_bytes() == encoded + + def test_metadata_without_data(self): + src = _source() + outputs = _outputs(src) + src.compute(_inputs(src, None, _metadata()), outputs) + assert outputs[TactileSource.FORCES].is_none + assert not outputs[TactileSource.LAYOUT].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.LAYOUT].is_none + + def test_taxel_count_mismatch_raises(self): + src = _source(num_taxels=4) + frame = TactileDataArray(np.zeros((3, 3), np.float32)) + with pytest.raises(ValueError, match="3 taxels"): + src.compute(_inputs(src, frame, None), _outputs(src)) + + def test_metadata_count_mismatch_raises(self): + src = _source(num_segments=NUM_SEGMENTS + 1) + with pytest.raises(ValueError, match="2 segments"): + src.compute(_inputs(src, None, _metadata()), _outputs(src)) + + def test_layout_arrays_are_reused_while_forces_update(self): + src = _source() + metadata = _metadata() + first = src( + _inputs( + src, TactileDataArray(np.zeros((NUM_TAXELS, 3), np.float32)), metadata + ) + ) + second = src( + _inputs( + src, TactileDataArray(np.ones((NUM_TAXELS, 3), np.float32)), metadata + ) + ) + + assert first[TactileSource.LAYOUT] is not second[TactileSource.LAYOUT] + for index in TactileLBSLayoutIndex: + array = first[TactileSource.LAYOUT][index] + assert second[TactileSource.LAYOUT][index] is array + np.testing.assert_array_equal(np.from_dlpack(array), array) + np.testing.assert_array_equal(first[TactileSource.FORCES][0], 0) + np.testing.assert_array_equal(second[TactileSource.FORCES][0], 1) + + def test_layout_cache_follows_the_metadata_buffer(self): + src = _source() + metadata = _metadata() + first = src(_inputs(src, None, metadata)) + replacement = _metadata(taxel_x=0.02) + second = src(_inputs(src, None, replacement)) + + for index in TactileLBSLayoutIndex: + assert ( + second[TactileSource.LAYOUT][index] + is not first[TactileSource.LAYOUT][index] + ) + taxel_poses = TactileLBSLayoutIndex.TAXEL_BIND_POSES + np.testing.assert_array_equal( + first[TactileSource.LAYOUT][taxel_poses][:, 0], np.float32(0.01) + ) + np.testing.assert_array_equal( + second[TactileSource.LAYOUT][taxel_poses][:, 0], np.float32(0.02) + ) + + with pytest.raises(ValueError, match="4 taxels"): + src(_inputs(src, None, _metadata(num_taxels=NUM_TAXELS + 1))) + + outputs = _outputs(src) + src.compute(_inputs(src, None, None), outputs) + assert outputs[TactileSource.LAYOUT].is_none + src.compute(_inputs(src, None, replacement), outputs) + for index in TactileLBSLayoutIndex: + assert ( + outputs[TactileSource.LAYOUT][index] + is second[TactileSource.LAYOUT][index] + ) diff --git a/tests/python/core/schema/test_tactile_array.py b/tests/python/core/schema/test_tactile_array.py new file mode 100644 index 0000000000..0b3b235474 --- /dev/null +++ b/tests/python/core/schema/test_tactile_array.py @@ -0,0 +1,67 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Unit tests for TactileDataArray / TactileDataArrayRecord in isaaccapture.schema.""" + +import gc + +import numpy as np +import pytest + +from isaaccapture.schema import ( + DeviceDataTimestamp, + TactileDataArray, + TactileDataArrayRecord, +) + +FORCES = np.array([[0.0, 0.0, 1.0], [0.5, -0.5, 2.0], [0.0, 0.1, 0.0]], np.float32) + + +class TestTactileDataArray: + @pytest.mark.parametrize("args", [(), (None,)]) + def test_requires_force_array(self, args): + with pytest.raises(TypeError, match="taxel_forces"): + TactileDataArray(*args) + + def test_explicit_empty_force_array(self): + data = TactileDataArray(np.empty((0, 3), np.float32)) + assert data.taxel_forces.shape == (0, 3) + record = TactileDataArrayRecord(data, DeviceDataTimestamp(1, 2, 3)) + assert record.data.taxel_forces.shape == (0, 3) + + def test_round_trip(self): + data = TactileDataArray(FORCES) + assert data.taxel_forces.dtype == np.float32 + np.testing.assert_array_equal(data.taxel_forces, FORCES) + + def test_accepts_nested_lists(self): + data = TactileDataArray([[1, 2, 3]]) + np.testing.assert_array_equal(data.taxel_forces, [[1.0, 2.0, 3.0]]) + + def test_view_outlives_its_table(self): + forces = TactileDataArray(FORCES).taxel_forces + gc.collect() + np.testing.assert_array_equal(forces, FORCES) + + def test_rejects_wrong_width(self): + with pytest.raises(ValueError, match="taxel_forces"): + TactileDataArray(np.zeros((2, 4), np.float32)) + + def test_repr(self): + assert "taxels=3" in repr(TactileDataArray(FORCES)) + + +class TestTactileDataArrayRecord: + def test_record_carries_payload_and_timestamp(self): + record = TactileDataArrayRecord( + TactileDataArray(FORCES), DeviceDataTimestamp(1, 2, 3) + ) + np.testing.assert_array_equal(record.data.taxel_forces, FORCES) + assert record.timestamp.sample_time_raw_device_clock == 3 + + def test_record_without_payload_preserves_timestamp(self): + record = TactileDataArrayRecord(None, DeviceDataTimestamp(1, 2, 3)) + assert record.data is None + assert record.timestamp.available_time_local_common_clock == 1 + assert record.timestamp.sample_time_local_common_clock == 2 + assert record.timestamp.sample_time_raw_device_clock == 3 diff --git a/tests/python/core/schema/test_tactile_metadata.py b/tests/python/core/schema/test_tactile_metadata.py new file mode 100644 index 0000000000..81d6915d97 --- /dev/null +++ b/tests/python/core/schema/test_tactile_metadata.py @@ -0,0 +1,182 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Tactile metadata union composition, binding invariants, and nested view lifetime.""" + +import gc + +import numpy as np +import pytest + +from isaaccapture.schema import ( + AlgorithmSource, + HandJoint, + HandSkeletonRef, + LBS, + SegmentSource, + TactileMetadata, +) + + +def lbs_arrays(num_joints: int = 2, num_taxels: int = 3) -> dict: + segment_bind_poses = np.zeros((num_joints, 7), np.float32) + segment_bind_poses[:, 6] = 1 + taxels = np.zeros((num_taxels, 7), np.float32) + taxels[:, :3] = 0.01 + taxels[:, 6] = 1 + indices = np.zeros((num_taxels, 4), np.uint32) + indices[:, 0] = num_joints - 1 + weights = np.zeros((num_taxels, 4), np.float32) + weights[:, 0] = 1 + return dict( + segment_bind_poses=segment_bind_poses, + taxel_bind_poses=taxels, + taxel_segment_indices=indices, + taxel_segment_weights=weights, + ) + + +def test_metadata_round_trip(): + arrays = lbs_arrays() + metadata = TactileMetadata( + LBS(**arrays), HandSkeletonRef([HandJoint.WRIST, HandJoint.INDEX_TIP]) + ) + assert metadata.geometry_mapping_type == AlgorithmSource.LBS + assert metadata.segment_source_type == SegmentSource.HandSkeletonRef + assert metadata.segment_source.joint_ids == [HandJoint.WRIST, HandJoint.INDEX_TIP] + for name, expected in arrays.items(): + actual = getattr(metadata.geometry_mapping, name) + assert actual.dtype == expected.dtype + np.testing.assert_array_equal(actual, expected) + assert metadata.geometry_mapping.taxel_segment_indices[0, 0] == 1 + + +@pytest.mark.parametrize("joint_ids", [[], [HandJoint.WRIST, HandJoint.INDEX_TIP]]) +def test_joint_ids_can_reconstruct_hand_reference(joint_ids): + hand = HandSkeletonRef(joint_ids) + rebuilt = HandSkeletonRef(hand.joint_ids) + assert all(isinstance(joint, HandJoint) for joint in hand.joint_ids) + assert rebuilt.joint_ids == joint_ids + assert rebuilt.to_bytes() == hand.to_bytes() + + +def test_nested_views_and_arrays_retain_the_buffer(): + metadata = TactileMetadata( + LBS(**lbs_arrays()), HandSkeletonRef([HandJoint.WRIST, HandJoint.INDEX_TIP]) + ) + geometry_mapping = metadata.geometry_mapping + segment_source = metadata.segment_source + rows = geometry_mapping.taxel_bind_poses + del metadata, geometry_mapping + gc.collect() + assert segment_source.joint_ids == [HandJoint.WRIST, HandJoint.INDEX_TIP] + np.testing.assert_array_equal(rows, lbs_arrays()["taxel_bind_poses"]) + + +@pytest.mark.parametrize( + "field", ["taxel_bind_poses", "taxel_segment_indices", "taxel_segment_weights"] +) +def test_rejects_mismatched_taxel_lengths(field): + arrays = lbs_arrays() + arrays[field] = arrays[field][:2] + geometry_mapping = LBS(**arrays) + with pytest.raises(ValueError, match=r"taxel_segment_(indices|weights): expected"): + TactileMetadata( + geometry_mapping, HandSkeletonRef([HandJoint.WRIST, HandJoint.INDEX_TIP]) + ) + + +def test_rejects_mismatched_segment_mapping(): + with pytest.raises(ValueError, match=r"segment_bind_poses.*joint_ids"): + TactileMetadata(LBS(**lbs_arrays()), HandSkeletonRef([HandJoint.WRIST])) + + +def test_rejects_out_of_range_zero_weight_slot(): + arrays = lbs_arrays() + arrays["taxel_segment_indices"][0, 3] = 2 + geometry_mapping = LBS(**arrays) + with pytest.raises(ValueError, match="taxel_segment_indices"): + TactileMetadata( + geometry_mapping, HandSkeletonRef([HandJoint.WRIST, HandJoint.INDEX_TIP]) + ) + + +@pytest.mark.parametrize("weight", [-1.0, np.inf, np.nan, 0.5, 0.0]) +def test_rejects_invalid_weights(weight): + arrays = lbs_arrays() + arrays["taxel_segment_weights"][0, 0] = weight + geometry_mapping = LBS(**arrays) + with pytest.raises(ValueError, match="taxel_segment_weights"): + TactileMetadata( + geometry_mapping, HandSkeletonRef([HandJoint.WRIST, HandJoint.INDEX_TIP]) + ) + + +@pytest.mark.parametrize("field", ["segment_bind_poses", "taxel_bind_poses"]) +@pytest.mark.parametrize( + "qw", [0, 2, np.finfo(np.float32).max, np.sqrt(np.float32(1 + 2e-4))] +) +def test_rejects_nonunit_quaternions(field, qw): + arrays = lbs_arrays() + arrays[field][-1, 6] = qw + geometry_mapping = LBS(**arrays) + with pytest.raises(ValueError, match="unit quaternions"): + TactileMetadata( + geometry_mapping, HandSkeletonRef([HandJoint.WRIST, HandJoint.INDEX_TIP]) + ) + + +@pytest.mark.parametrize("field", ["segment_bind_poses", "taxel_bind_poses"]) +@pytest.mark.parametrize( + "quaternion", + [ + [0.5, 0.5, 0.5, 0.5], + [0, 0, 0, -1], + [0, 0, 0, np.sqrt(np.float32(1 + 0.5e-4))], + ], +) +def test_accepts_unit_quaternions_with_rounding_tolerance(field, quaternion): + arrays = lbs_arrays() + arrays[field][-1, 3:] = quaternion + TactileMetadata( + LBS(**arrays), HandSkeletonRef([HandJoint.WRIST, HandJoint.INDEX_TIP]) + ) + + +@pytest.mark.parametrize("field", ["segment_bind_poses", "taxel_bind_poses"]) +@pytest.mark.parametrize("component", range(7)) +@pytest.mark.parametrize("value", [np.nan, np.inf, -np.inf]) +def test_rejects_nonfinite_bind_poses(field, component, value): + arrays = lbs_arrays() + arrays[field][-1, component] = value + geometry_mapping = LBS(**arrays) + with pytest.raises(ValueError, match="finite"): + TactileMetadata( + geometry_mapping, HandSkeletonRef([HandJoint.WRIST, HandJoint.INDEX_TIP]) + ) + + +@pytest.mark.parametrize( + "field", + [ + "segment_bind_poses", + "taxel_bind_poses", + "taxel_segment_indices", + "taxel_segment_weights", + ], +) +def test_lbs_rejects_wrong_array_width(field): + arrays = lbs_arrays() + arrays[field] = arrays[field][:, :-1] + with pytest.raises(ValueError, match=field): + LBS(**arrays) + + +@pytest.mark.parametrize( + "joint_ids", [[HandJoint.WRIST, HandJoint.WRIST], [HandJoint.NUM_JOINTS]] +) +def test_rejects_invalid_joint_mapping(joint_ids): + segment_source = HandSkeletonRef(joint_ids) + geometry_mapping = LBS(**lbs_arrays(num_joints=len(joint_ids))) + with pytest.raises(ValueError, match="joint_ids"): + TactileMetadata(geometry_mapping, segment_source)