diff --git a/examples/haptic_feedback/python/README.md b/examples/haptic_feedback/python/README.md index f533e634e..b46ddfbab 100644 --- a/examples/haptic_feedback/python/README.md +++ b/examples/haptic_feedback/python/README.md @@ -11,15 +11,17 @@ flushes to an `IHapticDevice` adapter each frame. | Example | What it demonstrates | | --- | --- | -| `controller_haptic_example.py` | Pull a controller trigger to rumble that same controller — the minimal `HapticSink` → `ControllerHapticDevice` wiring. | +| `controller_haptic_example.py` | Pull a controller trigger to rumble that same controller — the minimal in-process `HapticSink` → `ControllerHapticDevice` wiring. | +| `hand_pinch_haptic_example.py` | Pinch a fingertip toward the thumb to vibrate a haptic glove — the cross-process path (`HapticSink` → `PushTensorHapticDevice`, pushing a `HapticCommand` to a glove plugin such as Manus). | ```bash -uv run controller_haptic_example.py +uv run controller_haptic_example.py # motion-controller rumble +uv run hand_pinch_haptic_example.py # haptic glove (needs a glove plugin running) ``` -The example connects through the CloudXR / OpenXR runtime, so start the runtime -first. The full architecture, run instructions, and how to add a new haptic -device are in the official documentation: +Both connect through the CloudXR / OpenXR runtime, so start the runtime first. +The full architecture, run instructions, and how to add a new haptic device are +in the official documentation: **Haptic Feedback** — (source: [`docs/source/device/haptic_feedback.rst`](../../../docs/source/device/haptic_feedback.rst)) diff --git a/examples/haptic_feedback/python/hand_pinch_haptic_example.py b/examples/haptic_feedback/python/hand_pinch_haptic_example.py new file mode 100644 index 000000000..90e84b56c --- /dev/null +++ b/examples/haptic_feedback/python/hand_pinch_haptic_example.py @@ -0,0 +1,223 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Minimal haptic-glove example: feel each finger pinch toward the thumb. + +Reads XR hand-tracking joints, turns each fingertip's distance to the thumb +tip into a per-finger vibration intensity, and sends those per-finger powers +to a haptic glove running in a separate plugin process. It is the glove +counterpart to ``controller_haptic_example.py`` and the cross-process +reference for the Isaac Teleop device-output path: + +:: + + HandsSource (input) + | hand joints + v + PinchProximityToTactile -> TactileVectorToFingerPower -> HapticSink (IDeviceIOSink) + | + (after the graph) v + TeleopSession flushes the sink to the device + -> PushTensorHapticDevice.flush + -> HapticCommand pushed over XR_NVX1_push_tensor + -> glove plugin process (e.g. Manus) + +Key points for integrators: + +* The glove is a *cross-process* device. ``haptic_glove_device(...)`` returns a + :class:`~isaacteleop.haptic_devices.push_tensor.PushTensorHapticDevice` that + serialises each frame's per-finger powers into a vendor-neutral + ``HapticCommand`` and pushes it on the ``collection_id`` below. A glove + plugin (here the Manus plugin) reads the same collection and drives the + hardware. To target a different glove, change ``COLLECTION_ID`` and run that + vendor's plugin -- nothing else here changes. +* The device is a *sink*: register it with ``TeleopSessionConfig(sinks=[...])`` + and the session flushes it to the device each frame after the main pipeline. +* The mapping is split like the controller example: a thin + ``PinchProximityToTactile`` adapter emits a vendor-neutral ``TactileVector``, + and the library retargeter + :class:`~isaacteleop.retargeters.tactile_retargeters.TactileVectorToFingerPower` + shapes it (gain / deadband / saturation) into the ``FingerPowerVector`` every + glove accepts. Swap the adapter for an Isaac Lab ``ContactSensor`` fetch (or + use ``TactileHeatmapToFingerPower``) to drive the glove from sim contact. +""" + +from __future__ import annotations + +import time + +import numpy as np + +from isaacteleop.haptic_devices.glove import haptic_glove_device +from isaacteleop.retargeters.tactile_retargeters import TactileVectorToFingerPower +from isaacteleop.retargeting_engine.deviceio_source_nodes import HandsSource, HapticSink +from isaacteleop.retargeting_engine.interface import BaseRetargeter, OutputCombiner +from isaacteleop.retargeting_engine.interface.retargeter_core_types import ( + ComputeContext, + RetargeterIO, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.tensor_group_type import OptionalType +from isaacteleop.retargeting_engine.tensor_types import ( + FingerIndex, + HandInput, + HandInputIndex, + HandJointIndex, + NUM_HAPTIC_FINGERS, + TactileVector, +) +from isaacteleop.teleop_session_manager import TeleopSession, TeleopSessionConfig + + +APP_NAME = "HandPinchHapticExample" +FPS = 60.0 # demo loop rate; the retargeting pipeline runs once per frame + +# Haptic-glove plugin's tensor-collection id. The Manus plugin uses this exact +# string (src/plugins/manus/core/inc/manus/manus_glove_collection.hpp); for a +# different vendor, point this at that plugin's collection_id. +COLLECTION_ID = "manus_glove_haptic" + +# Pinch ramp: vibration starts as a fingertip comes within MAX_DISTANCE_M of +# the thumb tip and reaches full power at MIN_DISTANCE_M. +MAX_DISTANCE_M = 0.10 +MIN_DISTANCE_M = 0.005 + +# FingerPowerVector channel -> fingertip joint, for the four non-thumb fingers. +# The thumb channel has no pinch-to-thumb distance, so it stays at zero. +_FINGER_TIP_JOINTS = { + FingerIndex.INDEX: HandJointIndex.INDEX_TIP, + FingerIndex.MIDDLE: HandJointIndex.MIDDLE_TIP, + FingerIndex.RING: HandJointIndex.RING_TIP, + FingerIndex.PINKY: HandJointIndex.LITTLE_TIP, +} + +_FINGER_LABELS = ["Th", "Ix", "Md", "Rg", "Pk"] + + +class PinchProximityToTactile(BaseRetargeter): + """Per hand: ``distance(thumb_tip, finger_tip)`` -> raw per-finger proximity. + + Emits a ``TactileVector(5)`` (order Thumb..Pinky) where each non-thumb + channel is ``(MAX_DISTANCE_M - distance) / span`` -- ~1 when the fingertip + touches the thumb, ~0 at ``MAX_DISTANCE_M``, and out of range otherwise. + The downstream ``TactileVectorToFingerPower`` clamps and shapes it; this + adapter just reads the sensor, like ``TriggerToTactile`` in the controller + example. When the hand is not tracked the output is all zeros. + """ + + INPUT_HAND = "hand" + OUTPUT_TACTILE = "tactile" + + def input_spec(self) -> RetargeterIOType: + return {self.INPUT_HAND: OptionalType(HandInput())} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_TACTILE: TactileVector(NUM_HAPTIC_FINGERS)} + + def _compute_fn( + self, inputs: RetargeterIO, outputs: RetargeterIO, context: ComputeContext + ) -> None: + proximity = np.zeros(NUM_HAPTIC_FINGERS, dtype=np.float32) + hand = inputs[self.INPUT_HAND] + + if not hand.is_none: + joint_positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) + joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID]) + if bool(joint_valid[HandJointIndex.THUMB_TIP]): + thumb_tip = joint_positions[HandJointIndex.THUMB_TIP] + span = MAX_DISTANCE_M - MIN_DISTANCE_M + for finger, tip_joint in _FINGER_TIP_JOINTS.items(): + if bool(joint_valid[tip_joint]): + distance = float( + np.linalg.norm(joint_positions[tip_joint] - thumb_tip) + ) + proximity[finger] = (MAX_DISTANCE_M - distance) / span + + outputs[self.OUTPUT_TACTILE][0] = proximity + + +def _bar(value: float, width: int = 4) -> str: + """Fixed-width ASCII bar for a value in [0, 1].""" + filled = round(max(0.0, min(1.0, value)) * width) + return "█" * filled + "░" * (width - filled) + + +def _powers(result: dict, key: str) -> np.ndarray: + """The 5 finger powers of a ``FingerPowerVector`` output (zeros if absent).""" + group = result.get(key) + if group is None or group.is_none: + return np.zeros(NUM_HAPTIC_FINGERS, dtype=np.float32) + return np.asarray(group[0], dtype=np.float32).ravel() + + +def _row(label: str, powers: np.ndarray) -> str: + """One-hand summary: a per-finger bar for each of the 5 channels.""" + cols = " ".join( + f"{_FINGER_LABELS[i]} {_bar(powers[i])}" for i in range(NUM_HAPTIC_FINGERS) + ) + return f"{label} {cols}" + + +def main() -> None: + # 1. Input source. + hands = HandsSource("hands") + + # 2. Cross-process glove device + sink. The device pushes per-finger powers + # to the glove plugin listening on COLLECTION_ID. + device = haptic_glove_device(COLLECTION_ID) + sink = HapticSink("haptic_sink", device) + + # 3. Per hand: hand joints -> TactileVector (thin adapter) -> FingerPowerVector + # (library mapper). Each hand drives its own glove (left -> "left", etc). + sink_inputs = {} + monitoring = {} + for side, hand_output in ( + ("left", hands.output(HandsSource.LEFT)), + ("right", hands.output(HandsSource.RIGHT)), + ): + proximity = PinchProximityToTactile(f"{side}_pinch").connect( + {PinchProximityToTactile.INPUT_HAND: hand_output} + ) + powers = TactileVectorToFingerPower( + f"{side}_powers", + num_taxels=NUM_HAPTIC_FINGERS, + num_fingers=NUM_HAPTIC_FINGERS, + ).connect( + { + TactileVectorToFingerPower.INPUT_TACTILE: proximity.output( + PinchProximityToTactile.OUTPUT_TACTILE + ) + } + ) + sink_inputs[side] = powers.output(TactileVectorToFingerPower.OUTPUT_POWERS) + monitoring[f"powers_{side}"] = sink_inputs[side] + + # 4. The main pipeline only carries the values we print; the sink is + # registered separately and flushed to the glove by the session. + config = TeleopSessionConfig( + app_name=APP_NAME, + pipeline=OutputCombiner(monitoring), + sinks=[sink.connect(sink_inputs)], + ) + + print( + "Haptic glove pinch demo -- bring a fingertip toward the thumb to vibrate it." + ) + print( + f"Pushing HapticCommands on collection '{COLLECTION_ID}'. Press Ctrl+C to exit.\n" + ) + + frame_period_s = 1.0 / FPS + with TeleopSession(config) as session: + while True: + result = session.step() + line = f"{_row('L', _powers(result, 'powers_left'))} | {_row('R', _powers(result, 'powers_right'))}" + print(f"\r{line:<104}", end="", flush=True) + time.sleep(frame_period_s) + + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print("\nExiting.") diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/haptic_command_reader_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/haptic_command_reader_tracker_base.hpp new file mode 100644 index 000000000..d0eceeac3 --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/haptic_command_reader_tracker_base.hpp @@ -0,0 +1,20 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +namespace core +{ + +struct HapticCommandTrackedT; + +// Abstract base interface for HapticCommandReaderTracker implementations. +class IHapticCommandReaderTrackerImpl : public ITrackerImpl +{ +public: + virtual const HapticCommandTrackedT& get_data() const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/tensor_push_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/tensor_push_tracker_base.hpp new file mode 100644 index 000000000..044f2b894 --- /dev/null +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/tensor_push_tracker_base.hpp @@ -0,0 +1,24 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "tracker.hpp" + +#include +#include + +namespace core +{ + +// Abstract base interface for TensorPushTracker implementations. +class ITensorPushTrackerImpl : public ITrackerImpl +{ +public: + // `payload` is an opaque serialised buffer (the caller's schema); the + // impl pads to the configured per-sample size and attaches timestamps + // before pushing it as a tensor sample. + virtual void push(const std::vector& payload) const = 0; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/CMakeLists.txt b/src/core/deviceio_trackers/cpp/CMakeLists.txt index 48b460d71..630bfa38a 100644 --- a/src/core/deviceio_trackers/cpp/CMakeLists.txt +++ b/src/core/deviceio_trackers/cpp/CMakeLists.txt @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 cmake_minimum_required(VERSION 3.20) @@ -10,6 +10,8 @@ add_library(deviceio_trackers STATIC controller_tracker.cpp message_channel_tracker.cpp generic_3axis_pedal_tracker.cpp + tensor_push_tracker.cpp + haptic_command_reader_tracker.cpp frame_metadata_tracker_oak.cpp full_body_tracker_pico.cpp inc/deviceio_trackers/head_tracker.hpp @@ -18,6 +20,8 @@ add_library(deviceio_trackers STATIC inc/deviceio_trackers/message_channel_tracker.hpp inc/deviceio_trackers/full_body_tracker_pico.hpp inc/deviceio_trackers/generic_3axis_pedal_tracker.hpp + inc/deviceio_trackers/tensor_push_tracker.hpp + inc/deviceio_trackers/haptic_command_reader_tracker.hpp inc/deviceio_trackers/frame_metadata_tracker_oak.hpp ) diff --git a/src/core/deviceio_trackers/cpp/haptic_command_reader_tracker.cpp b/src/core/deviceio_trackers/cpp/haptic_command_reader_tracker.cpp new file mode 100644 index 000000000..c96573055 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/haptic_command_reader_tracker.cpp @@ -0,0 +1,29 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/haptic_command_reader_tracker.hpp" + +#include + +namespace core +{ + +HapticCommandReaderTracker::HapticCommandReaderTracker(const std::string& collection_id, std::size_t max_payload_size) + : collection_id_(collection_id), max_payload_size_(max_payload_size) +{ + if (collection_id_.empty()) + { + throw std::invalid_argument("HapticCommandReaderTracker: collection_id must be non-empty"); + } + if (max_payload_size_ == 0) + { + throw std::invalid_argument("HapticCommandReaderTracker: max_payload_size must be > 0"); + } +} + +const HapticCommandTrackedT& HapticCommandReaderTracker::get_data(const ITrackerSession& session) const +{ + return static_cast(session.get_tracker_impl(*this)).get_data(); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/haptic_command_reader_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/haptic_command_reader_tracker.hpp new file mode 100644 index 000000000..81fde88c4 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/haptic_command_reader_tracker.hpp @@ -0,0 +1,55 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include +#include +#include + +namespace core +{ + +// Consumer ITracker (plugin side): reads the most-recent HapticCommand +// FlatBuffer pushed by a TensorPushTracker on the same `collection_id` +// (the producer encodes a HapticCommand and pushes it under the canonical +// "haptic_command" tensor identifier). A vendor plugin reuses this directly +// instead of writing its own SchemaTracker boilerplate. +class HapticCommandReaderTracker : public ITracker +{ +public: + static constexpr std::size_t DEFAULT_MAX_PAYLOAD_SIZE = 256; + + explicit HapticCommandReaderTracker(const std::string& collection_id, + std::size_t max_payload_size = DEFAULT_MAX_PAYLOAD_SIZE); + + std::string_view get_name() const override + { + return TRACKER_NAME; + } + + // `tracked.data` is null until the first sample arrives or after the + // producer collection disappears from the tensor list. + const HapticCommandTrackedT& get_data(const ITrackerSession& session) const; + + const std::string& collection_id() const + { + return collection_id_; + } + + std::size_t max_payload_size() const + { + return max_payload_size_; + } + +private: + static constexpr const char* TRACKER_NAME = "HapticCommandReaderTracker"; + + std::string collection_id_; + std::size_t max_payload_size_{ DEFAULT_MAX_PAYLOAD_SIZE }; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/tensor_push_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/tensor_push_tracker.hpp new file mode 100644 index 000000000..b0993c36b --- /dev/null +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/tensor_push_tracker.hpp @@ -0,0 +1,67 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include +#include +#include + +namespace core +{ + +// Generic producer ITracker: pushes opaque serialised payloads as tensor +// samples over XR_NVX1_push_tensor. The payload's schema is the caller's +// concern -- this tracker is schema-agnostic, so any cross-process output +// device (haptic glove, exoskeleton, ...) reuses it without writing a new +// C++ tracker. Pairs with a consumer (e.g. a SchemaTracker / a +// HapticCommandReaderTracker) on the same `collection_id` + `tensor_identifier`. +class TensorPushTracker : public ITracker +{ +public: + static constexpr std::size_t DEFAULT_MAX_PAYLOAD_SIZE = 256; + + // `collection_id` pairs producer and consumer across processes; + // `tensor_identifier` names the tensor within the collection (must match + // the consumer); `max_payload_size` is the fixed per-sample buffer size. + TensorPushTracker(std::string collection_id, + std::string tensor_identifier, + std::size_t max_payload_size = DEFAULT_MAX_PAYLOAD_SIZE); + + std::string_view get_name() const override + { + return TRACKER_NAME; + } + + // `payload.size()` must be <= max_payload_size(); the impl pads to the + // per-sample size declared at collection-create time. + void push(const ITrackerSession& session, const std::vector& payload) const; + + const std::string& collection_id() const + { + return collection_id_; + } + + const std::string& tensor_identifier() const + { + return tensor_identifier_; + } + + std::size_t max_payload_size() const + { + return max_payload_size_; + } + +private: + static constexpr const char* TRACKER_NAME = "TensorPushTracker"; + + std::string collection_id_; + std::string tensor_identifier_; + std::size_t max_payload_size_{ DEFAULT_MAX_PAYLOAD_SIZE }; +}; + +} // namespace core diff --git a/src/core/deviceio_trackers/cpp/tensor_push_tracker.cpp b/src/core/deviceio_trackers/cpp/tensor_push_tracker.cpp new file mode 100644 index 000000000..d1ce35265 --- /dev/null +++ b/src/core/deviceio_trackers/cpp/tensor_push_tracker.cpp @@ -0,0 +1,36 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "inc/deviceio_trackers/tensor_push_tracker.hpp" + +#include +#include + +namespace core +{ + +TensorPushTracker::TensorPushTracker(std::string collection_id, std::string tensor_identifier, std::size_t max_payload_size) + : collection_id_(std::move(collection_id)), + tensor_identifier_(std::move(tensor_identifier)), + max_payload_size_(max_payload_size) +{ + if (collection_id_.empty()) + { + throw std::invalid_argument("TensorPushTracker: collection_id must be non-empty"); + } + if (tensor_identifier_.empty()) + { + throw std::invalid_argument("TensorPushTracker: tensor_identifier must be non-empty"); + } + if (max_payload_size_ == 0) + { + throw std::invalid_argument("TensorPushTracker: max_payload_size must be > 0"); + } +} + +void TensorPushTracker::push(const ITrackerSession& session, const std::vector& payload) const +{ + static_cast(session.get_tracker_impl(*this)).push(payload); +} + +} // namespace core diff --git a/src/core/deviceio_trackers/python/deviceio_trackers_init.py b/src/core/deviceio_trackers/python/deviceio_trackers_init.py index f867e8f54..a231b0a33 100644 --- a/src/core/deviceio_trackers/python/deviceio_trackers_init.py +++ b/src/core/deviceio_trackers/python/deviceio_trackers_init.py @@ -12,6 +12,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + TensorPushTracker, FullBodyTrackerPico, ITrackerSession, NUM_JOINTS, @@ -28,6 +29,7 @@ "FrameMetadataTrackerOak", "FullBodyTrackerPico", "Generic3AxisPedalTracker", + "TensorPushTracker", "HandTracker", "HeadTracker", "ITracker", diff --git a/src/core/deviceio_trackers/python/tracker_bindings.cpp b/src/core/deviceio_trackers/python/tracker_bindings.cpp index 601c7db06..df234bb51 100644 --- a/src/core/deviceio_trackers/python/tracker_bindings.cpp +++ b/src/core/deviceio_trackers/python/tracker_bindings.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -150,6 +151,26 @@ PYBIND11_MODULE(_deviceio_trackers, m) { return self.get_data(session); }, py::arg("session"), "Get the current foot pedal tracked state (data is None when no data available)"); + py::class_> tensor_push_tracker( + m, "TensorPushTracker"); + tensor_push_tracker.attr("DEFAULT_MAX_PAYLOAD_SIZE") = + static_cast(core::TensorPushTracker::DEFAULT_MAX_PAYLOAD_SIZE); + tensor_push_tracker + .def(py::init(), py::arg("collection_id"), py::arg("tensor_identifier"), + py::arg("max_payload_size") = core::TensorPushTracker::DEFAULT_MAX_PAYLOAD_SIZE, + "Generic producer ITracker: pushes opaque serialized payloads as tensor samples over " + "XR_NVX1_push_tensor. Pairs with a consumer on the same collection_id + tensor_identifier.") + .def( + "push", + [](const core::TensorPushTracker& self, const core::ITrackerSession& session, py::bytes payload) + { + const std::string bytes = payload; + const std::vector buffer(bytes.begin(), bytes.end()); + self.push(session, buffer); + }, + py::arg("session"), py::arg("payload"), + "Push one serialized payload (bytes, length <= max_payload_size) to the paired consumer."); + py::class_>( m, "FullBodyTrackerPico") .def(py::init<>()) diff --git a/src/core/live_trackers/cpp/CMakeLists.txt b/src/core/live_trackers/cpp/CMakeLists.txt index 23d105b7d..9f15a88eb 100644 --- a/src/core/live_trackers/cpp/CMakeLists.txt +++ b/src/core/live_trackers/cpp/CMakeLists.txt @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 cmake_minimum_required(VERSION 3.20) @@ -12,6 +12,8 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.cpp live_full_body_tracker_pico_impl.cpp live_generic_3axis_pedal_tracker_impl.cpp + live_tensor_push_tracker_impl.cpp + live_haptic_command_reader_tracker_impl.cpp live_frame_metadata_tracker_oak_impl.cpp inc/live_trackers/schema_tracker_base.hpp inc/live_trackers/schema_tracker.hpp @@ -22,6 +24,8 @@ add_library(live_trackers STATIC live_message_channel_tracker_impl.hpp live_full_body_tracker_pico_impl.hpp live_generic_3axis_pedal_tracker_impl.hpp + live_tensor_push_tracker_impl.hpp + live_haptic_command_reader_tracker_impl.hpp live_frame_metadata_tracker_oak_impl.hpp ) @@ -36,6 +40,7 @@ target_link_libraries(live_trackers deviceio::deviceio_trackers mcap::mcap_core oxr::oxr_utils + pusherio::pusherio Teleop::openxr_extensions ) diff --git a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp index 7d6b5c4f9..d120d8620 100644 --- a/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp +++ b/src/core/live_trackers/cpp/inc/live_trackers/live_deviceio_factory.hpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #pragma once @@ -30,6 +30,10 @@ class FullBodyTrackerPico; class IFullBodyTrackerPicoImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; +class TensorPushTracker; +class ITensorPushTrackerImpl; +class HapticCommandReaderTracker; +class IHapticCommandReaderTrackerImpl; class HandTracker; class IHandTrackerImpl; class HeadTracker; @@ -62,6 +66,9 @@ class LiveDeviceIOFactory std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); + std::unique_ptr create_tensor_push_tracker_impl(const TensorPushTracker* tracker); + std::unique_ptr create_haptic_command_reader_tracker_impl( + const HapticCommandReaderTracker* tracker); std::unique_ptr create_frame_metadata_tracker_oak_impl( const FrameMetadataTrackerOak* tracker); diff --git a/src/core/live_trackers/cpp/live_deviceio_factory.cpp b/src/core/live_trackers/cpp/live_deviceio_factory.cpp index 2c304480c..004f92a6e 100644 --- a/src/core/live_trackers/cpp/live_deviceio_factory.cpp +++ b/src/core/live_trackers/cpp/live_deviceio_factory.cpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 #include "inc/live_trackers/live_deviceio_factory.hpp" @@ -8,16 +8,20 @@ #include "live_full_body_tracker_pico_impl.hpp" #include "live_generic_3axis_pedal_tracker_impl.hpp" #include "live_hand_tracker_impl.hpp" +#include "live_haptic_command_reader_tracker_impl.hpp" #include "live_head_tracker_impl.hpp" #include "live_message_channel_tracker_impl.hpp" +#include "live_tensor_push_tracker_impl.hpp" #include #include #include #include #include +#include #include #include +#include #include #include @@ -79,6 +83,18 @@ std::unique_ptr try_create_generic_pedal_impl(LiveDeviceIOFactory& return typed ? factory.create_generic_3axis_pedal_tracker_impl(typed) : nullptr; } +std::unique_ptr try_create_tensor_push_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_tensor_push_tracker_impl(typed) : nullptr; +} + +std::unique_ptr try_create_haptic_command_reader_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_haptic_command_reader_tracker_impl(typed) : nullptr; +} + std::unique_ptr try_create_oak_impl(LiveDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -102,6 +118,9 @@ inline const TrackerDispatchEntry k_tracker_dispatch[] = { { &try_add_extensions, &try_create_message_channel_impl }, { &try_add_extensions, &try_create_full_body_pico_impl }, { &try_add_extensions, &try_create_generic_pedal_impl }, + { &try_add_extensions, &try_create_tensor_push_impl }, + { &try_add_extensions, + &try_create_haptic_command_reader_impl }, { &try_add_extensions, &try_create_oak_impl }, }; @@ -244,6 +263,17 @@ std::unique_ptr LiveDeviceIOFactory::create_gener return std::make_unique(handles_, tracker, std::move(channels)); } +std::unique_ptr LiveDeviceIOFactory::create_tensor_push_tracker_impl(const TensorPushTracker* tracker) +{ + return std::make_unique(handles_, tracker); +} + +std::unique_ptr LiveDeviceIOFactory::create_haptic_command_reader_tracker_impl( + const HapticCommandReaderTracker* tracker) +{ + return std::make_unique(handles_, tracker); +} + std::unique_ptr LiveDeviceIOFactory::create_frame_metadata_tracker_oak_impl( const FrameMetadataTrackerOak* tracker) { diff --git a/src/core/live_trackers/cpp/live_haptic_command_reader_tracker_impl.cpp b/src/core/live_trackers/cpp/live_haptic_command_reader_tracker_impl.cpp new file mode 100644 index 000000000..a4dd492cd --- /dev/null +++ b/src/core/live_trackers/cpp/live_haptic_command_reader_tracker_impl.cpp @@ -0,0 +1,48 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_haptic_command_reader_tracker_impl.hpp" + +namespace core +{ + +namespace +{ + +// Canonical identifiers for the vendor-neutral HapticCommand payload. The +// producer (TensorPushTracker created by PushTensorHapticDevice) pushes under +// the same "haptic_command" tensor identifier; SchemaTrackerBase filters by +// collection_id, these strings just keep the runtime's per-tensor diagnostics +// aligned. +constexpr const char* kHapticCommandTensorIdentifier = "haptic_command"; +constexpr const char* kHapticCommandLocalizedName = "HapticCommand"; + +SchemaTrackerConfig make_haptic_command_reader_config(const HapticCommandReaderTracker* tracker) +{ + SchemaTrackerConfig cfg; + cfg.collection_id = tracker->collection_id(); + cfg.max_flatbuffer_size = tracker->max_payload_size(); + cfg.tensor_identifier = kHapticCommandTensorIdentifier; + cfg.localized_name = kHapticCommandLocalizedName; + return cfg; +} + +} // namespace + +LiveHapticCommandReaderTrackerImpl::LiveHapticCommandReaderTrackerImpl(const OpenXRSessionHandles& handles, + const HapticCommandReaderTracker* tracker) + : schema_reader_(handles, make_haptic_command_reader_config(tracker), /*mcap_channels=*/nullptr) +{ +} + +void LiveHapticCommandReaderTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ + schema_reader_.update(tracked_.data); +} + +const HapticCommandTrackedT& LiveHapticCommandReaderTrackerImpl::get_data() const +{ + return tracked_; +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_haptic_command_reader_tracker_impl.hpp b/src/core/live_trackers/cpp/live_haptic_command_reader_tracker_impl.hpp new file mode 100644 index 000000000..b3b8e522a --- /dev/null +++ b/src/core/live_trackers/cpp/live_haptic_command_reader_tracker_impl.hpp @@ -0,0 +1,47 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include "inc/live_trackers/schema_tracker.hpp" + +#include +#include +#include +#include + +#include +#include +#include + +namespace core +{ + +// MCAP recording disabled for v1; the Record template arg is required by +// SchemaTracker but the impl passes mcap_channels=nullptr. +using HapticCommandSchemaTracker = SchemaTracker; + +class LiveHapticCommandReaderTrackerImpl : public IHapticCommandReaderTrackerImpl +{ +public: + static std::vector required_extensions() + { + return SchemaTrackerBase::get_required_extensions(); + } + + LiveHapticCommandReaderTrackerImpl(const OpenXRSessionHandles& handles, const HapticCommandReaderTracker* tracker); + + LiveHapticCommandReaderTrackerImpl(const LiveHapticCommandReaderTrackerImpl&) = delete; + LiveHapticCommandReaderTrackerImpl& operator=(const LiveHapticCommandReaderTrackerImpl&) = delete; + LiveHapticCommandReaderTrackerImpl(LiveHapticCommandReaderTrackerImpl&&) = delete; + LiveHapticCommandReaderTrackerImpl& operator=(LiveHapticCommandReaderTrackerImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + const HapticCommandTrackedT& get_data() const override; + +private: + HapticCommandSchemaTracker schema_reader_; + HapticCommandTrackedT tracked_; +}; + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_tensor_push_tracker_impl.cpp b/src/core/live_trackers/cpp/live_tensor_push_tracker_impl.cpp new file mode 100644 index 000000000..642454881 --- /dev/null +++ b/src/core/live_trackers/cpp/live_tensor_push_tracker_impl.cpp @@ -0,0 +1,46 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "live_tensor_push_tracker_impl.hpp" + +#include + +namespace core +{ + +namespace +{ + +SchemaPusherConfig make_tensor_push_config(const TensorPushTracker* tracker) +{ + SchemaPusherConfig cfg; + cfg.collection_id = tracker->collection_id(); + cfg.max_flatbuffer_size = tracker->max_payload_size(); + cfg.tensor_identifier = tracker->tensor_identifier(); + cfg.localized_name = tracker->tensor_identifier(); + return cfg; +} + +} // namespace + +LiveTensorPushTrackerImpl::LiveTensorPushTrackerImpl(const OpenXRSessionHandles& handles, const TensorPushTracker* tracker) + : pusher_(handles, make_tensor_push_config(tracker)) +{ +} + +void LiveTensorPushTrackerImpl::update(int64_t monotonic_time_ns) +{ + last_update_time_ns_ = monotonic_time_ns; +} + +void LiveTensorPushTrackerImpl::push(const std::vector& payload) const +{ + // Prefer the most-recent session tick so pushes share the session's + // monotonic-clock domain; fall back to "now" for pushes that beat the + // first update(). Synthesised commands have no raw-device clock, so both + // timestamps get the same value. + const int64_t now_ns = last_update_time_ns_ > 0 ? last_update_time_ns_ : core::os_monotonic_now_ns(); + pusher_.push_buffer(payload.data(), payload.size(), now_ns, now_ns); +} + +} // namespace core diff --git a/src/core/live_trackers/cpp/live_tensor_push_tracker_impl.hpp b/src/core/live_trackers/cpp/live_tensor_push_tracker_impl.hpp new file mode 100644 index 000000000..368678d30 --- /dev/null +++ b/src/core/live_trackers/cpp/live_tensor_push_tracker_impl.hpp @@ -0,0 +1,43 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include + +#include +#include + +namespace core +{ + +// Wraps core::SchemaPusher; owns the XR_NVX1_push_tensor handle. +class LiveTensorPushTrackerImpl : public ITensorPushTrackerImpl +{ +public: + static std::vector required_extensions() + { + return SchemaPusher::get_required_extensions(); + } + + LiveTensorPushTrackerImpl(const OpenXRSessionHandles& handles, const TensorPushTracker* tracker); + + LiveTensorPushTrackerImpl(const LiveTensorPushTrackerImpl&) = delete; + LiveTensorPushTrackerImpl& operator=(const LiveTensorPushTrackerImpl&) = delete; + LiveTensorPushTrackerImpl(LiveTensorPushTrackerImpl&&) = delete; + LiveTensorPushTrackerImpl& operator=(LiveTensorPushTrackerImpl&&) = delete; + + void update(int64_t monotonic_time_ns) override; + void push(const std::vector& payload) const override; + +private: + // `mutable` keeps push() `const` (mirrors MessageChannelTracker::send_message); + // SchemaPusher::push_buffer is non-const but each call is just a runtime side effect. + mutable SchemaPusher pusher_; + int64_t last_update_time_ns_{ 0 }; +}; + +} // namespace core diff --git a/src/core/python/deviceio_init.py b/src/core/python/deviceio_init.py index ea4a5aafe..48cd52261 100644 --- a/src/core/python/deviceio_init.py +++ b/src/core/python/deviceio_init.py @@ -17,6 +17,7 @@ MessageChannelTracker, FrameMetadataTrackerOak, Generic3AxisPedalTracker, + TensorPushTracker, FullBodyTrackerPico, NUM_JOINTS, JOINT_PALM, @@ -60,6 +61,7 @@ "MessageChannelTracker", "FrameMetadataTrackerOak", "Generic3AxisPedalTracker", + "TensorPushTracker", "FullBodyTrackerPico", "OpenXRSessionHandles", "DeviceIOSession", diff --git a/src/core/replay_trackers/cpp/CMakeLists.txt b/src/core/replay_trackers/cpp/CMakeLists.txt index 3647af299..06676fd25 100644 --- a/src/core/replay_trackers/cpp/CMakeLists.txt +++ b/src/core/replay_trackers/cpp/CMakeLists.txt @@ -11,6 +11,8 @@ add_library(replay_trackers STATIC replay_full_body_tracker_pico_impl.cpp replay_generic_3axis_pedal_tracker_impl.cpp replay_message_channel_tracker_impl.cpp + replay_tensor_push_tracker_impl.cpp + replay_haptic_command_reader_tracker_impl.cpp inc/replay_trackers/replay_deviceio_factory.hpp replay_hand_tracker_impl.hpp replay_head_tracker_impl.hpp @@ -18,6 +20,8 @@ add_library(replay_trackers STATIC replay_full_body_tracker_pico_impl.hpp replay_generic_3axis_pedal_tracker_impl.hpp replay_message_channel_tracker_impl.hpp + replay_tensor_push_tracker_impl.hpp + replay_haptic_command_reader_tracker_impl.hpp ) target_include_directories(replay_trackers diff --git a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp index c8272babb..031d3c2c2 100644 --- a/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp +++ b/src/core/replay_trackers/cpp/inc/replay_trackers/replay_deviceio_factory.hpp @@ -21,6 +21,10 @@ class FullBodyTrackerPico; class IFullBodyTrackerPicoImpl; class Generic3AxisPedalTracker; class IGeneric3AxisPedalTrackerImpl; +class TensorPushTracker; +class ITensorPushTrackerImpl; +class HapticCommandReaderTracker; +class IHapticCommandReaderTrackerImpl; class HandTracker; class IHandTrackerImpl; class HeadTracker; @@ -50,6 +54,9 @@ class ReplayDeviceIOFactory std::unique_ptr create_full_body_tracker_pico_impl(const FullBodyTrackerPico* tracker); std::unique_ptr create_generic_3axis_pedal_tracker_impl( const Generic3AxisPedalTracker* tracker); + std::unique_ptr create_tensor_push_tracker_impl(const TensorPushTracker* tracker); + std::unique_ptr create_haptic_command_reader_tracker_impl( + const HapticCommandReaderTracker* tracker); std::unique_ptr create_message_channel_tracker_impl(const MessageChannelTracker* tracker); private: diff --git a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp index a3d6c3b6a..e30220eca 100644 --- a/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp +++ b/src/core/replay_trackers/cpp/replay_deviceio_factory.cpp @@ -7,15 +7,19 @@ #include "replay_full_body_tracker_pico_impl.hpp" #include "replay_generic_3axis_pedal_tracker_impl.hpp" #include "replay_hand_tracker_impl.hpp" +#include "replay_haptic_command_reader_tracker_impl.hpp" #include "replay_head_tracker_impl.hpp" #include "replay_message_channel_tracker_impl.hpp" +#include "replay_tensor_push_tracker_impl.hpp" #include #include #include #include +#include #include #include +#include #include #include @@ -71,6 +75,19 @@ std::unique_ptr try_create_generic_pedal_impl(ReplayDeviceIOFactor return typed ? factory.create_generic_3axis_pedal_tracker_impl(typed) : nullptr; } +std::unique_ptr try_create_tensor_push_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_tensor_push_tracker_impl(typed) : nullptr; +} + +std::unique_ptr try_create_haptic_command_reader_impl(ReplayDeviceIOFactory& factory, + const ITracker& tracker) +{ + auto* typed = dynamic_cast(&tracker); + return typed ? factory.create_haptic_command_reader_tracker_impl(typed) : nullptr; +} + std::unique_ptr try_create_message_channel_impl(ReplayDeviceIOFactory& factory, const ITracker& tracker) { auto* typed = dynamic_cast(&tracker); @@ -80,8 +97,14 @@ std::unique_ptr try_create_message_channel_impl(ReplayDeviceIOFact using TryCreateFn = std::unique_ptr (*)(ReplayDeviceIOFactory&, const ITracker&); inline const TryCreateFn k_tracker_dispatch[] = { - &try_create_head_impl, &try_create_hand_impl, &try_create_controller_impl, - &try_create_full_body_pico_impl, &try_create_generic_pedal_impl, &try_create_message_channel_impl, + &try_create_head_impl, + &try_create_hand_impl, + &try_create_controller_impl, + &try_create_full_body_pico_impl, + &try_create_generic_pedal_impl, + &try_create_tensor_push_impl, + &try_create_haptic_command_reader_impl, + &try_create_message_channel_impl, }; } // namespace @@ -148,6 +171,18 @@ std::unique_ptr ReplayDeviceIOFactory::create_gen return std::make_unique(open_reader(filename_), get_name(tracker)); } +std::unique_ptr ReplayDeviceIOFactory::create_tensor_push_tracker_impl( + const TensorPushTracker* /*tracker*/) +{ + return std::make_unique(); +} + +std::unique_ptr ReplayDeviceIOFactory::create_haptic_command_reader_tracker_impl( + const HapticCommandReaderTracker* /*tracker*/) +{ + return std::make_unique(); +} + std::unique_ptr ReplayDeviceIOFactory::create_message_channel_tracker_impl( const MessageChannelTracker* tracker) { diff --git a/src/core/replay_trackers/cpp/replay_haptic_command_reader_tracker_impl.cpp b/src/core/replay_trackers/cpp/replay_haptic_command_reader_tracker_impl.cpp new file mode 100644 index 000000000..59340638b --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_haptic_command_reader_tracker_impl.cpp @@ -0,0 +1,18 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "replay_haptic_command_reader_tracker_impl.hpp" + +namespace core +{ + +void ReplayHapticCommandReaderTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ +} + +const HapticCommandTrackedT& ReplayHapticCommandReaderTrackerImpl::get_data() const +{ + return tracked_; +} + +} // namespace core diff --git a/src/core/replay_trackers/cpp/replay_haptic_command_reader_tracker_impl.hpp b/src/core/replay_trackers/cpp/replay_haptic_command_reader_tracker_impl.hpp new file mode 100644 index 000000000..af8ad8fb4 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_haptic_command_reader_tracker_impl.hpp @@ -0,0 +1,28 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include + +#include + +namespace core +{ + +// Haptic commands are not recorded to MCAP, so replay always returns empty +// tracked data (data == nullptr). +class ReplayHapticCommandReaderTrackerImpl : public IHapticCommandReaderTrackerImpl +{ +public: + ReplayHapticCommandReaderTrackerImpl() = default; + + void update(int64_t monotonic_time_ns) override; + const HapticCommandTrackedT& get_data() const override; + +private: + HapticCommandTrackedT tracked_; +}; + +} // namespace core diff --git a/src/core/replay_trackers/cpp/replay_tensor_push_tracker_impl.cpp b/src/core/replay_trackers/cpp/replay_tensor_push_tracker_impl.cpp new file mode 100644 index 000000000..16d8ff705 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_tensor_push_tracker_impl.cpp @@ -0,0 +1,26 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#include "replay_tensor_push_tracker_impl.hpp" + +#include + +namespace core +{ + +void ReplayTensorPushTrackerImpl::update(int64_t /*monotonic_time_ns*/) +{ +} + +void ReplayTensorPushTrackerImpl::push(const std::vector& /*payload*/) const +{ + bool expected = false; + if (m_drop_logged.compare_exchange_strong(expected, true)) + { + std::cerr << "ReplayTensorPushTrackerImpl::push: no peer in replay mode; " + "pushes are dropped (silenced after this message)." + << std::endl; + } +} + +} // namespace core diff --git a/src/core/replay_trackers/cpp/replay_tensor_push_tracker_impl.hpp b/src/core/replay_trackers/cpp/replay_tensor_push_tracker_impl.hpp new file mode 100644 index 000000000..419064f78 --- /dev/null +++ b/src/core/replay_trackers/cpp/replay_tensor_push_tracker_impl.hpp @@ -0,0 +1,29 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include + +#include +#include +#include + +namespace core +{ + +// Replay has no peer to push to, so pushes are dropped. Push happens per +// frame, so the drop is logged only once to avoid flooding the console. +class ReplayTensorPushTrackerImpl : public ITensorPushTrackerImpl +{ +public: + ReplayTensorPushTrackerImpl() = default; + + void update(int64_t monotonic_time_ns) override; + void push(const std::vector& payload) const override; + +private: + mutable std::atomic m_drop_logged{ false }; +}; + +} // namespace core diff --git a/src/core/retargeting_engine/python/tensor_types/__init__.py b/src/core/retargeting_engine/python/tensor_types/__init__.py index da106fae6..70b5c21ff 100644 --- a/src/core/retargeting_engine/python/tensor_types/__init__.py +++ b/src/core/retargeting_engine/python/tensor_types/__init__.py @@ -19,8 +19,10 @@ from .tactile_types import ( TactileVector, TactileHeatmap, + FingerPowerVector, ControllerHapticPulse, EndEffectorForce, + NUM_HAPTIC_FINGERS, NUM_CONTROLLER_HAPTIC_FIELDS, NUM_END_EFFECTOR_FORCE_AXES, ) @@ -32,6 +34,7 @@ FullBodyInputIndex, HandJointIndex, BodyJointPicoIndex, + FingerIndex, ControllerHapticPulseField, EndEffectorForceAxis, ) @@ -56,8 +59,10 @@ # Tactile / haptic types "TactileVector", "TactileHeatmap", + "FingerPowerVector", "ControllerHapticPulse", "EndEffectorForce", + "NUM_HAPTIC_FINGERS", "NUM_CONTROLLER_HAPTIC_FIELDS", "NUM_END_EFFECTOR_FORCE_AXES", # Indices @@ -68,6 +73,7 @@ "FullBodyInputIndex", "HandJointIndex", "BodyJointPicoIndex", + "FingerIndex", "ControllerHapticPulseField", "EndEffectorForceAxis", ] diff --git a/src/core/retargeting_engine/python/tensor_types/indices.py b/src/core/retargeting_engine/python/tensor_types/indices.py index 2fec98b92..61ab7a375 100644 --- a/src/core/retargeting_engine/python/tensor_types/indices.py +++ b/src/core/retargeting_engine/python/tensor_types/indices.py @@ -108,6 +108,16 @@ class BodyJointPicoIndex(IntEnum): RIGHT_HAND = 23 +class FingerIndex(IntEnum): + """Channel indices into a :func:`FingerPowerVector`, standard glove order.""" + + THUMB = 0 + INDEX = 1 + MIDDLE = 2 + RING = 3 + PINKY = 4 + + class ControllerHapticPulseField(IntEnum): """Field indices into a :func:`ControllerHapticPulse` ``[amplitude, frequency_hz, duration_s]`` vector.""" diff --git a/src/core/retargeting_engine/python/tensor_types/tactile_types.py b/src/core/retargeting_engine/python/tensor_types/tactile_types.py index 7f9ec8b55..f9d58d7ad 100644 --- a/src/core/retargeting_engine/python/tensor_types/tactile_types.py +++ b/src/core/retargeting_engine/python/tensor_types/tactile_types.py @@ -4,11 +4,12 @@ """TensorGroupType definitions for tactile feedback and haptic output. Sim-side schemas (``TactileVector``, ``TactileHeatmap``) carry contact data -into the retargeting pipeline; device-side schemas (``ControllerHapticPulse``, -``EndEffectorForce``) describe what each ``IHapticDevice`` adapter accepts. -Retargeters in :mod:`isaacteleop.retargeters.tactile_retargeters` map -sim-side to device-side; ``HapticSink`` uses ``accepted_type()`` for -connect-time type checking. +into the retargeting pipeline; device-side schemas (``FingerPowerVector``, +``ControllerHapticPulse``, ``EndEffectorForce``) describe what each +``IHapticDevice`` adapter accepts. Retargeters in +:mod:`isaacteleop.retargeters.tactile_retargeters` map sim-side to +device-side; ``HapticSink`` uses ``accepted_type()`` for connect-time type +checking. """ from ..interface.tensor_group_type import TensorGroupType @@ -16,6 +17,13 @@ # Constants +NUM_HAPTIC_FINGERS = 5 +"""Channels in a :func:`FingerPowerVector`. + +Standard glove convention, in order: Thumb, Index, Middle, Ring, Pinky +(see :class:`FingerIndex`). +""" + NUM_CONTROLLER_HAPTIC_FIELDS = 3 """Fields in a :func:`ControllerHapticPulse`: ``[amplitude, frequency_hz, duration_s]``.""" @@ -88,6 +96,37 @@ def TactileHeatmap(rows: int, cols: int, num_pads: int = 1) -> TensorGroupType: # ============================================================================ +def FingerPowerVector(num_fingers: int = NUM_HAPTIC_FINGERS) -> TensorGroupType: + """Per-finger vibration intensities [unitless, 0..1]. + + Device-side schema for vibration-glove output. Standard glove order: + ``[Thumb, Index, Middle, Ring, Pinky]`` (see :class:`FingerIndex`). + + Consumed by a :class:`~isaacteleop.haptic_devices.push_tensor.PushTensorHapticDevice` + (see :func:`~isaacteleop.haptic_devices.glove.haptic_glove_device`), which + re-encodes the values into a vendor-neutral ``HapticCommand`` and pushes + them to the glove plugin process; the plugin applies them via its vendor SDK. + + Args: + num_fingers: Number of finger channels. Defaults to 5 (standard + five-finger glove). + + Returns: + TensorGroupType with one ``(num_fingers,) float32`` tensor. + """ + return TensorGroupType( + f"finger_power_vector_{num_fingers}", + [ + NDArrayType( + "finger_power", + shape=(num_fingers,), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + ], + ) + + def ControllerHapticPulse() -> TensorGroupType: """One-frame motion-controller vibration pulse ``[amplitude, frequency_hz, duration_s]``. @@ -121,8 +160,8 @@ def EndEffectorForce() -> TensorGroupType: the :class:`HapticSink` rotate sim-frame forces into device frame via the optional ``world_T_haptic`` ValueInput leaf and :class:`Vector3FrameTransform`. - Shipped in v1 (no v1 device consumes it) so the schema set is stable when - the Haply force-feedback adapter lands. + Shipped even though no current device consumes it, so the schema set is + stable when the Haply force-feedback adapter lands. """ return TensorGroupType( "end_effector_force", diff --git a/src/core/retargeting_engine_tests/python/test_haptic_devices.py b/src/core/retargeting_engine_tests/python/test_haptic_devices.py index 927cc2e48..dce1a946d 100644 --- a/src/core/retargeting_engine_tests/python/test_haptic_devices.py +++ b/src/core/retargeting_engine_tests/python/test_haptic_devices.py @@ -2,20 +2,22 @@ # SPDX-License-Identifier: Apache-2.0 """ -Tests for ``isaacteleop.haptic_devices.controller``. +Tests for ``isaacteleop.haptic_devices`` adapters. -``ControllerHapticDevice`` is the in-process device archetype: it stores -per-endpoint pulses in ``apply()`` (called inside the retargeting graph, no -session in scope) and writes them out in ``flush(session)`` (called by -``TeleopSession`` after the graph). We lock down: +``ControllerHapticDevice`` is the in-process device archetype and +``PushTensorHapticDevice`` (with the ``haptic_glove_device`` factory) is the +cross-process archetype. Both store per-endpoint values in ``apply()`` (called +inside the retargeting graph, no session in scope) and write them out in +``flush(session)`` (called by ``TeleopSession`` after the graph). We lock down: * Store/emit semantics (latest-wins coalescing per endpoint, flush clears). * Shape validation on ``apply()``. -* ``flush`` forwards each stored pulse to - ``ControllerTracker``'s per-side ``apply_left_haptic_feedback`` / - ``apply_right_haptic_feedback`` with the right argument shape. +* ``flush`` forwards each stored value to the device's tracker — the controller + device calls the per-side ``apply_left_haptic_feedback`` / + ``apply_right_haptic_feedback``; the push device encodes a ``HapticCommand`` + and ``push``-es it. * ``flush`` swallows tracker exceptions and only logs once per endpoint. -* ``get_tracker`` / ``endpoints`` reflect construction. +* ``get_tracker`` / ``endpoints`` / ``accepted_type`` reflect construction. """ from typing import List, Tuple @@ -24,7 +26,10 @@ import pytest from isaacteleop.haptic_devices.controller import ControllerHapticDevice -from isaacteleop.retargeting_engine.tensor_types import ControllerHapticPulse +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerHapticPulse, + FingerPowerVector, +) _PulseCall = Tuple[object, str, float, float, float] @@ -161,3 +166,118 @@ def test_flush_logs_failure_at_most_once_per_endpoint(self, caplog) -> None: "expected a single once-per-endpoint warning, " f"got {[r.getMessage() for r in warnings]}" ) + + +class _RecordingTensorPushTracker: + """Test double for ``TensorPushTracker``. + + Records ``push(session, payload)`` calls; ``fail=True`` makes every push + raise so we can exercise ``PushTensorHapticDevice``'s once-per-endpoint + error gate. The endpoint is encoded inside ``payload`` (a HapticCommand + FlatBuffer), so tests assert on the raw bytes. + """ + + def __init__(self, fail: bool = False) -> None: + self.pushes: List[Tuple[object, bytes]] = [] + self._fail = fail + + def get_name(self) -> str: + return "TensorPushTracker" + + def push(self, session, payload) -> None: + if self._fail: + raise RuntimeError("simulated push failure") + self.pushes.append((session, bytes(payload))) + + +class TestPushTensorHapticDevice: + """Cross-process device: ``apply`` stores, ``flush`` encodes one + ``HapticCommand`` per endpoint and pushes it through a ``TensorPushTracker``. + The real tracker is swapped for a recording double so ``flush`` can run + without a live DeviceIO session.""" + + def _device(self, **kwargs): + from isaacteleop.haptic_devices.push_tensor import PushTensorHapticDevice + + return PushTensorHapticDevice("test_collection", FingerPowerVector(5), **kwargs) + + def test_accepted_type_reflects_constructor(self) -> None: + device = self._device() + assert device.accepted_type().name == FingerPowerVector(5).name + + def test_endpoints_reflect_constructor(self) -> None: + device = self._device(endpoints=("device",)) + assert device.endpoints() == ("device",) + + def test_glove_factory_builds_finger_power_device(self) -> None: + from isaacteleop.haptic_devices.glove import haptic_glove_device + + device = haptic_glove_device("manus_glove_haptic") + assert device.accepted_type().name == FingerPowerVector(5).name + assert device.endpoints() == ("left", "right") + + def test_apply_then_flush_pushes_encoded_command_per_endpoint(self) -> None: + device = self._device() + recorder = _RecordingTensorPushTracker() + device._tracker = recorder # swap in the double; flush() needs no session + + device.apply("left", np.array([0.1, 0.2, 0.3, 0.4, 0.5], dtype=np.float32)) + device.apply("right", np.zeros(5, dtype=np.float32)) + + sentinel_session = object() + device.flush(sentinel_session) + + assert len(recorder.pushes) == 2 + for session, payload in recorder.pushes: + assert session is sentinel_session + assert isinstance(payload, bytes) and len(payload) > 0 + # The endpoint name travels inside the serialized HapticCommand. + all_bytes = b"".join(payload for _s, payload in recorder.pushes) + assert b"left" in all_bytes + assert b"right" in all_bytes + + def test_apply_coalesces_to_latest_per_endpoint(self) -> None: + device = self._device() + recorder = _RecordingTensorPushTracker() + device._tracker = recorder + + device.apply("left", np.full(5, 0.1, dtype=np.float32)) + device.apply("left", np.full(5, 0.9, dtype=np.float32)) + device.flush(object()) + + assert len(recorder.pushes) == 1 + + def test_flush_clears_pending(self) -> None: + device = self._device() + recorder = _RecordingTensorPushTracker() + device._tracker = recorder + + device.apply("left", np.zeros(5, dtype=np.float32)) + device.flush(object()) + device.flush(object()) + + assert len(recorder.pushes) == 1 + + def test_flush_swallows_exceptions_and_logs_once_per_endpoint(self, caplog) -> None: + device = self._device(endpoints=("left",)) + device._tracker = _RecordingTensorPushTracker(fail=True) + + for _ in range(3): + device.apply("left", np.zeros(5, dtype=np.float32)) + with caplog.at_level("WARNING"): + device.flush(object()) # must not raise + + warnings = [ + r for r in caplog.records if "PushTensorHapticDevice" in r.getMessage() + ] + assert len(warnings) == 1 + + +class TestPackHapticCommand: + def test_pack_returns_nonempty_bytes_with_endpoint(self) -> None: + from isaacteleop.schema import pack_haptic_command + + payload = pack_haptic_command("left", [0.1, 0.2, 0.3, 0.4, 0.5]) + assert isinstance(payload, bytes) and len(payload) > 0 + # FlatBuffers stores the endpoint string inline. + assert b"left" in payload diff --git a/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py b/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py index afc57df91..1543a94de 100644 --- a/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py +++ b/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py @@ -7,9 +7,10 @@ Covers the composable spatial primitives (``Vector3FrameTransform``, ``WorldForceAccumulator``, ``MagnitudeReducer``) and the per-device mappers that turn sim-side ``TactileVector`` / ``TactileHeatmap`` flows into the -``ControllerHapticPulse`` device schema. The shared gain/deadband/saturation -curve (``_apply_gain_curve``) is exercised indirectly through the deadband -tests on ``TactileVectorToControllerPulse``. +``FingerPowerVector`` and ``ControllerHapticPulse`` device schemas. The shared +gain/deadband/saturation curve (``_apply_gain_curve``) and the EMA smoother +(``_smooth_ema``) are exercised indirectly through the deadband, saturation, +and smoothing tests on the per-device mappers. """ import numpy as np @@ -17,9 +18,13 @@ import pytest from isaacteleop.retargeters.tactile_retargeters import ( + FingerPowerToControllerPulse, MagnitudeReducer, TactileHeatmapToControllerPulse, + TactileHeatmapToFingerPower, + TactileHeatmapToWristPulse, TactileVectorToControllerPulse, + TactileVectorToFingerPower, Vector3FrameTransform, WorldForceAccumulator, ) @@ -218,3 +223,101 @@ def test_heatmap_to_controller_pulse(self) -> None: pulse = np.asarray(outputs["pulse"][0]) assert pulse[ControllerHapticPulseField.AMPLITUDE] == pytest.approx(0.4) assert pulse[ControllerHapticPulseField.FREQUENCY_HZ] == pytest.approx(200.0) + + +# --------------------------------------------------------------------------- +# Per-device mappers -- target schema: FingerPowerVector +# --------------------------------------------------------------------------- + + +class TestTactileVectorToFingerPower: + def test_identity_mapping_per_finger(self) -> None: + """num_taxels == num_fingers and no finger_groups maps taxel i -> finger i.""" + node = TactileVectorToFingerPower("vec_to_fp", num_taxels=5, num_fingers=5) + outputs = _run(node, {"tactile": [0.1, 0.2, 0.3, 0.4, 0.5]}) + npt.assert_array_almost_equal( + np.asarray(outputs["powers"][0]), + np.array([0.1, 0.2, 0.3, 0.4, 0.5], dtype=np.float32), + ) + + def test_finger_groups_reduce_with_max(self) -> None: + """Two taxels per finger, reduced with 'max'.""" + groups = [[0, 1], [2, 3], [4, 5], [6, 7], [8, 9]] + node = TactileVectorToFingerPower( + "vec_to_fp", num_taxels=10, finger_groups=groups, reduction="max" + ) + outputs = _run( + node, {"tactile": [0.0, 0.9, 0.2, 0.1, 0.5, 0.5, 0.7, 0.0, 0.0, 0.3]} + ) + npt.assert_array_almost_equal( + np.asarray(outputs["powers"][0]), + np.array([0.9, 0.2, 0.5, 0.7, 0.3], dtype=np.float32), + ) + + def test_requires_finger_groups_when_taxels_ne_fingers(self) -> None: + with pytest.raises(ValueError, match="finger_groups is required"): + TactileVectorToFingerPower("vec_to_fp", num_taxels=7) + + def test_saturation_clamps_each_finger(self) -> None: + node = TactileVectorToFingerPower( + "vec_to_fp", num_taxels=5, num_fingers=5, gain=10.0, saturation=0.5 + ) + outputs = _run(node, {"tactile": [0.1, 0.2, 0.3, 0.4, 1.0]}) + assert float(np.asarray(outputs["powers"][0]).max()) == pytest.approx(0.5) + + def test_smoothing_blends_consecutive_frames(self) -> None: + """smoothing=0.5: first frame passes through, second is the EMA blend.""" + node = TactileVectorToFingerPower( + "vec_to_fp", num_taxels=1, num_fingers=1, smoothing=0.5 + ) + first = _run(node, {"tactile": [1.0]}) + assert float(np.asarray(first["powers"][0])[0]) == pytest.approx(1.0) + second = _run(node, {"tactile": [0.0]}) + assert float(np.asarray(second["powers"][0])[0]) == pytest.approx(0.5) + + +class TestTactileHeatmapToFingerPower: + def test_per_pad_max_reduction(self) -> None: + node = TactileHeatmapToFingerPower("heat_fp", rows=2, cols=2, num_pads=5) + heatmap = np.zeros((5, 2, 2), dtype=np.float32) + heatmap[0] = [[0.1, 0.7], [0.0, 0.2]] + heatmap[1] = [[0.3, 0.0], [0.0, 0.0]] + heatmap[2] = 0.5 + outputs = _run(node, {"heatmap": heatmap}) + powers = np.asarray(outputs["powers"][0]) + assert powers[0] == pytest.approx(0.7) + assert powers[1] == pytest.approx(0.3) + assert powers[2] == pytest.approx(0.5) + + def test_rejects_nonpositive_dimensions(self) -> None: + with pytest.raises(ValueError, match="rows/cols/num_pads"): + TactileHeatmapToFingerPower("heat_fp", rows=0, cols=2, num_pads=5) + + +class TestTactileHeatmapToWristPulse: + def test_collapses_to_single_channel(self) -> None: + node = TactileHeatmapToWristPulse("wrist", rows=2, cols=2, num_pads=1) + heatmap = np.array([[[0.0, 0.8], [0.1, 0.2]]], dtype=np.float32) + outputs = _run(node, {"heatmap": heatmap}) + power = np.asarray(outputs["power"][0]) + assert power.shape == (1,) + assert float(power[0]) == pytest.approx(0.8) + + def test_rejects_nonpositive_dimensions(self) -> None: + with pytest.raises(ValueError, match="rows/cols/num_pads"): + TactileHeatmapToWristPulse("wrist", rows=2, cols=0, num_pads=1) + + +class TestFingerPowerToControllerPulse: + def test_reduces_fingers_to_pulse_amplitude(self) -> None: + node = FingerPowerToControllerPulse( + "fp_to_pulse", num_fingers=5, frequency_hz=150.0 + ) + outputs = _run(node, {"powers": [0.1, 0.2, 0.9, 0.3, 0.0]}) + pulse = np.asarray(outputs["pulse"][0]) + assert pulse[ControllerHapticPulseField.AMPLITUDE] == pytest.approx(0.9) + assert pulse[ControllerHapticPulseField.FREQUENCY_HZ] == pytest.approx(150.0) + + def test_rejects_zero_fingers(self) -> None: + with pytest.raises(ValueError, match="num_fingers"): + FingerPowerToControllerPulse("fp_to_pulse", num_fingers=0) diff --git a/src/core/schema/fbs/haptic_command.fbs b/src/core/schema/fbs/haptic_command.fbs new file mode 100644 index 000000000..5aee4ecdf --- /dev/null +++ b/src/core/schema/fbs/haptic_command.fbs @@ -0,0 +1,34 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +include "timestamp.fbs"; + +namespace core; + +// Vendor-neutral wire payload for the cross-process device-output path. +// One haptic command for one named actuator endpoint. `endpoint` is the +// device-defined channel name (e.g. "left" / "right" for hand-mounted +// devices, "device" for a single grounded device); `values` is the +// device-side payload (e.g. per-finger powers in [0, 1], a force vector, +// ...). The schema does not lock the `values` length so different devices +// document their own ordering in their plugin README. +table HapticCommand { + endpoint: string (id: 0); + values: [float] (id: 1); +} + +// Tracked wrapper for the in-memory reader API; data is null until the +// first sample arrives or after the producer collection disappears. +table HapticCommandTracked { + data: HapticCommand (id: 0); +} + +// MCAP recording wrapper required by the SchemaTracker +// template. Recording is disabled here -- the live reader passes +// mcap_channels=nullptr. +table HapticCommandRecord { + data: HapticCommand (id: 0); + timestamp: DeviceDataTimestamp (id: 1); +} + +root_type HapticCommandRecord; diff --git a/src/core/schema/python/CMakeLists.txt b/src/core/schema/python/CMakeLists.txt index d948e1417..f5787cf70 100644 --- a/src/core/schema/python/CMakeLists.txt +++ b/src/core/schema/python/CMakeLists.txt @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 pybind11_add_module(schema_py @@ -6,6 +6,7 @@ pybind11_add_module(schema_py controller_bindings.h full_body_bindings.h hand_bindings.h + haptic_command_bindings.h head_bindings.h message_channel_bindings.h pedals_bindings.h diff --git a/src/core/schema/python/haptic_command_bindings.h b/src/core/schema/python/haptic_command_bindings.h new file mode 100644 index 000000000..3143b4335 --- /dev/null +++ b/src/core/schema/python/haptic_command_bindings.h @@ -0,0 +1,63 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Python bindings for the vendor-neutral HapticCommand FlatBuffer schema. +// Types: HapticCommand (table) + a pack helper that serialises it to the +// bytes a TensorPushTracker pushes to a peer-process device plugin. + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace core +{ + +inline void bind_haptic_command(py::module& m) +{ + py::class_>(m, "HapticCommand") + .def(py::init([]() { return std::make_shared(); })) + .def(py::init( + [](const std::string& endpoint, const std::vector& values) + { + auto obj = std::make_shared(); + obj->endpoint = endpoint; + obj->values = values; + return obj; + }), + py::arg("endpoint"), py::arg("values")) + .def_property( + "endpoint", [](const HapticCommandT& self) { return self.endpoint; }, + [](HapticCommandT& self, const std::string& v) { self.endpoint = v; }) + .def_property( + "values", [](const HapticCommandT& self) { return self.values; }, + [](HapticCommandT& self, const std::vector& v) { self.values = v; }); + + // Producer-side encode: serialise a HapticCommand (endpoint + values) to + // the FlatBuffer bytes that TensorPushTracker.push() carries to the + // consumer. Uses the generated Pack so the wire layout always matches the + // C++ SchemaTracker reader. + m.def( + "pack_haptic_command", + [](const std::string& endpoint, const std::vector& values) -> py::bytes + { + HapticCommandT cmd; + cmd.endpoint = endpoint; + cmd.values = values; + flatbuffers::FlatBufferBuilder fbb; + fbb.Finish(HapticCommand::Pack(fbb, &cmd)); + return py::bytes(reinterpret_cast(fbb.GetBufferPointer()), fbb.GetSize()); + }, + py::arg("endpoint"), py::arg("values"), + "Serialise a HapticCommand (endpoint, values) to FlatBuffer bytes for TensorPushTracker.push()."); +} + +} // namespace core diff --git a/src/core/schema/python/schema_init.py b/src/core/schema/python/schema_init.py index 3f3aeb108..916d02c42 100644 --- a/src/core/schema/python/schema_init.py +++ b/src/core/schema/python/schema_init.py @@ -39,6 +39,9 @@ MessageChannelMessages, MessageChannelMessagesTrackedT, MessageChannelMessagesRecord, + # Haptic command types (vendor-neutral cross-process device output). + HapticCommand, + pack_haptic_command, # Camera-related types. StreamType, FrameMetadataOak, @@ -86,6 +89,9 @@ "MessageChannelMessages", "MessageChannelMessagesTrackedT", "MessageChannelMessagesRecord", + # Haptic command types. + "HapticCommand", + "pack_haptic_command", # Camera types. "StreamType", "FrameMetadataOak", diff --git a/src/core/schema/python/schema_module.cpp b/src/core/schema/python/schema_module.cpp index e20dae586..63504f18c 100644 --- a/src/core/schema/python/schema_module.cpp +++ b/src/core/schema/python/schema_module.cpp @@ -1,4 +1,4 @@ -// SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. // SPDX-License-Identifier: Apache-2.0 // Python module entry point for Isaac Teleop schema bindings. @@ -9,6 +9,7 @@ #include "controller_bindings.h" #include "full_body_bindings.h" #include "hand_bindings.h" +#include "haptic_command_bindings.h" #include "head_bindings.h" #include "message_channel_bindings.h" #include "oak_bindings.h" @@ -43,6 +44,9 @@ PYBIND11_MODULE(_schema, m) // Bind message channel types (MessageChannelMessages table). core::bind_message_channel(m); + // Bind vendor-neutral HapticCommand table + pack_haptic_command() encoder. + core::bind_haptic_command(m); + // Bind OAK types (StreamType enum, FrameMetadataOak table). core::bind_oak(m); diff --git a/src/haptic_devices/glove.py b/src/haptic_devices/glove.py new file mode 100644 index 000000000..016806715 --- /dev/null +++ b/src/haptic_devices/glove.py @@ -0,0 +1,62 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Haptic-glove adapter — the cross-process device reference for gloves. + +A vibration glove is a cross-process device: the vendor SDK runs in its own +plugin process and reads per-finger powers off a push-tensor collection. On +the Isaac Teleop side the glove is therefore just a +:class:`~isaacteleop.haptic_devices.push_tensor.PushTensorHapticDevice` +configured to accept a +:func:`~isaacteleop.retargeting_engine.tensor_types.FingerPowerVector`. +:func:`haptic_glove_device` is the named constructor for that configuration, so +glove integrators get a discoverable entry point without needing to know the +generic push device exists. +""" + +from __future__ import annotations + +from typing import Iterable + +from isaacteleop.retargeting_engine.tensor_types import ( + FingerPowerVector, + NUM_HAPTIC_FINGERS, +) + +from .interface import Endpoint +from .push_tensor import PushTensorHapticDevice + + +def haptic_glove_device( + collection_id: str, + *, + num_fingers: int = NUM_HAPTIC_FINGERS, + endpoints: Iterable[Endpoint] = ("left", "right"), + tensor_identifier: str = "haptic_command", +) -> PushTensorHapticDevice: + """Construct a cross-process haptic-glove device. + + The returned device accepts a ``FingerPowerVector(num_fingers)`` per + endpoint (values in ``[0, 1]``, standard order Thumb..Pinky) and pushes + each frame's powers as a ``HapticCommand`` to the glove plugin process + listening on ``collection_id``. + + Args: + collection_id: Push-tensor collection that pairs Isaac Teleop with the + glove plugin process. Both must use the same string on the same + system. + num_fingers: Per-endpoint finger channels. Defaults to 5. + endpoints: Named gloves to drive. Defaults to ``("left", "right")``; + pass ``("left",)`` for a single glove. + tensor_identifier: Tensor name within the collection; must match the + plugin's reader. + + Returns: + A :class:`PushTensorHapticDevice` bound to ``FingerPowerVector(num_fingers)``. + """ + return PushTensorHapticDevice( + collection_id, + FingerPowerVector(num_fingers), + tensor_identifier=tensor_identifier, + endpoints=endpoints, + ) diff --git a/src/haptic_devices/push_tensor.py b/src/haptic_devices/push_tensor.py new file mode 100644 index 000000000..73f262d5e --- /dev/null +++ b/src/haptic_devices/push_tensor.py @@ -0,0 +1,119 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Cross-process haptic adapter — the out-of-process device reference. + +``PushTensorHapticDevice`` is an :class:`IHapticDevice` that forwards graph +output to a device plugin running in a *separate process*. Where +``ControllerHapticDevice`` calls a vendor SDK in-process, this adapter +serialises each endpoint's values into a vendor-neutral ``HapticCommand`` +FlatBuffer and pushes it over ``XR_NVX1_push_tensor`` to a peer-process consumer +(a plugin built on ``HapticCommandReaderTracker``) sharing the same +``collection_id`` + ``tensor_identifier``. + +It is the Python half of the "push device" layer: a partner ships a small +out-of-process plugin that owns their SDK and reads ``HapticCommand`` from the +shared collection, and reuses this adapter unchanged on the Isaac Teleop side. +The adapter is deliberately payload-agnostic -- ``accepted_type`` and +``endpoints`` are constructor arguments, so a haptic glove (per-finger powers), +a grounded force device (a force vector), or a multi-actuator exoskeleton all +reuse it without a new C++ tracker or a new wire schema. The meaning and +ordering of ``values`` is a contract between the upstream retargeter and the +plugin, documented in the plugin's README. +""" + +from __future__ import annotations + +import logging +from typing import TYPE_CHECKING, Any, Iterable + +import numpy as np + +from isaacteleop.deviceio_trackers import TensorPushTracker +from isaacteleop.retargeting_engine.interface.tensor_group_type import TensorGroupType +from isaacteleop.schema import pack_haptic_command + +from .interface import Endpoint, IHapticDevice + + +if TYPE_CHECKING: + from isaacteleop.deviceio import ITracker + + +logger = logging.getLogger(__name__) + + +class PushTensorHapticDevice(IHapticDevice): + """:class:`IHapticDevice` that pushes ``HapticCommand`` to a peer process. + + Each frame, :meth:`apply` stores the latest values per endpoint, and + :meth:`flush` serialises one ``HapticCommand`` per endpoint (via + ``pack_haptic_command``) and pushes it through a :class:`TensorPushTracker`. + The paired consumer process reads them back with a + ``HapticCommandReaderTracker`` on the same ``collection_id`` and + ``tensor_identifier`` and drives the real hardware there. + + Args: + collection_id: Push-tensor collection that pairs this producer with the + consumer plugin. Both processes must use the same string and run on + the same system. + accepted_type: Device-side ``TensorGroupType`` the upstream retargeter + must output (e.g. a per-finger power vector for a glove). Checked at + ``HapticSink.connect()`` time. + tensor_identifier: Name of the tensor within the collection; must match + the consumer. Defaults to ``"haptic_command"``. + endpoints: Named actuators this device drives. Defaults to the + hand-mounted convention ``("left", "right")``; a single grounded + device may pass ``("device",)``. + max_payload_size: Fixed per-sample buffer size in bytes; must be at + least the largest serialised ``HapticCommand`` (endpoint name + + ``values``). Defaults to the tracker's own default. + """ + + def __init__( + self, + collection_id: str, + accepted_type: TensorGroupType, + *, + tensor_identifier: str = "haptic_command", + endpoints: Iterable[Endpoint] = ("left", "right"), + max_payload_size: int = TensorPushTracker.DEFAULT_MAX_PAYLOAD_SIZE, + ) -> None: + self._accepted_type = accepted_type + self._endpoints: tuple[Endpoint, ...] = tuple(endpoints) + self._tracker = TensorPushTracker( + collection_id, tensor_identifier, max_payload_size + ) + # Latest-wins per endpoint within a frame; emitted and cleared by flush. + self._pending: dict[Endpoint, list[float]] = {} + self._error_logged: dict[Endpoint, bool] = { + endpoint: False for endpoint in self._endpoints + } + + def accepted_type(self) -> TensorGroupType: + return self._accepted_type + + def endpoints(self) -> tuple[Endpoint, ...]: + return self._endpoints + + def get_tracker(self) -> "ITracker": + return self._tracker + + def apply(self, endpoint: Endpoint, values: np.ndarray) -> None: + self._pending[endpoint] = np.asarray(values, dtype=np.float32).ravel().tolist() + + def flush(self, deviceio_session: Any) -> None: + pending, self._pending = self._pending, {} + for endpoint, values in pending.items(): + try: + payload = pack_haptic_command(endpoint, values) + self._tracker.push(deviceio_session, payload) + except Exception as exc: + if not self._error_logged.get(endpoint, False): + logger.warning( + "PushTensorHapticDevice.flush(%s) failed (further errors " + "for this endpoint will be silenced): %s", + endpoint, + exc, + ) + self._error_logged[endpoint] = True diff --git a/src/plugins/manus/core/inc/manus/manus_glove_collection.hpp b/src/plugins/manus/core/inc/manus/manus_glove_collection.hpp new file mode 100644 index 000000000..b12fa10c3 --- /dev/null +++ b/src/plugins/manus/core/inc/manus/manus_glove_collection.hpp @@ -0,0 +1,26 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +namespace plugins +{ +namespace manus +{ + +// Vendor binding for the Teleop -> Manus haptic-glove tensor collection. +// The Teleop-side producer is a generic +// isaacteleop.haptic_devices.push_tensor.PushTensorHapticDevice (see the +// isaacteleop.haptic_devices.glove.haptic_glove_device factory and the +// haptic_feedback example). Whatever consumer the app wires must pass this +// same collection_id string so the runtime pairs them by name. +// +// The per-sample buffer size is deliberately NOT configured here: the reader +// (HapticCommandReaderTracker) and the producer (TensorPushTracker) share the +// same DEFAULT_MAX_PAYLOAD_SIZE, so both sides agree without a Manus-specific +// constant that could drift below the producer's collection size (the reader +// rejects a collection whose sample size exceeds its buffer). +inline constexpr const char* MANUS_GLOVE_COLLECTION_ID = "manus_glove_haptic"; + +} // namespace manus +} // namespace plugins diff --git a/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp b/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp index 85cd95f43..aeb31b0df 100644 --- a/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp +++ b/src/plugins/manus/core/inc/manus/manus_hand_tracking_plugin.hpp @@ -6,6 +6,7 @@ #include #include #include +#include #include #include #include @@ -13,6 +14,8 @@ #include #include +#include +#include #include #include #include @@ -29,6 +32,11 @@ namespace plugins namespace manus { +// Manus haptic gloves expose exactly five finger motors; the SDK's +// CoreSdk_VibrateFingersForGlove takes a fixed powers[5]. A glove with a +// different actuator count would change this and the values it consumes. +inline constexpr std::size_t kManusFingerCount = 5; + class __attribute__((visibility("default"))) ManusTracker { public: @@ -41,6 +49,19 @@ class __attribute__((visibility("default"))) ManusTracker std::vector get_left_node_info() const; std::vector get_right_node_info() const; + /// Vibrate the five finger motors of one haptic glove. + /// + /// `powers` is in Manus order [Thumb, Index, Middle, Ring, Pinky], + /// values clamped to [0, 1]. Dispatched from `update()` once per frame + /// off the latest HapticCommand the plugin received. + /// + /// No-ops (and logs at most once per side) when the glove is not + /// connected, the glove reports no haptic support, or the SDK call + /// returns a non-success code. + /// + /// Thread-safe — `landscape_mutex` guards the per-side glove id. + void apply_haptic_command(bool is_left, const std::array& powers); + private: // Lifecycle explicit ManusTracker(const std::string& app_name) noexcept(false); @@ -78,11 +99,17 @@ class __attribute__((visibility("default"))) ManusTracker bool m_initialized = false; // ManusSDK State - std::mutex landscape_mutex; + mutable std::mutex landscape_mutex; std::optional left_glove_id; std::optional right_glove_id; bool is_connected = false; + // Haptic state — the per-side log-once flags use std::atomic to stay + // quiet when many frames in a row fail (e.g. the glove was disconnected + // mid-session). Only `apply_haptic_command` (non-const) writes here, so + // no `mutable` is needed; const callers do not touch these flags. + std::array, 2> m_haptic_error_logged{ { false, false } }; + // OpenXR State std::shared_ptr m_session; core::OpenXRSessionHandles m_handles; @@ -90,6 +117,9 @@ class __attribute__((visibility("default"))) ManusTracker std::unique_ptr m_right_injector; std::shared_ptr m_controller_tracker; std::shared_ptr m_hand_tracker; + // Inbound HapticCommand tensor; collection identity in + // inc/manus/manus_glove_collection.hpp. Read each frame in update(). + std::shared_ptr m_haptic_reader; std::unique_ptr m_deviceio_session; // XDev native hand trackers (Quest 3 hand tracking via XR_MNDX_xdev_space) diff --git a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp index 06212ab22..bff9b0512 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -3,15 +3,20 @@ #include "inc/manus/manus_hand_tracking_plugin.hpp" +#include "inc/manus/manus_glove_collection.hpp" + #include #include #include #include +#include #include #include #include +#include #include +#include #include #include #include @@ -79,6 +84,26 @@ void ManusTracker::update() // Update DeviceIOSession which handles time conversion and tracker updates internally m_deviceio_session->update(); + // Latest-wins: the hardware only retains the most recent vibration call, + // so dropping intermediate samples on a slow tick is fine. The generic + // HapticCommand carries an endpoint string ("left"/"right"); commands for + // other endpoints or with a non-5-finger values vector are ignored (this + // plugin only drives 5-finger gloves). + if (m_haptic_reader) + { + const auto& tracked = m_haptic_reader->get_data(*m_deviceio_session); + if (tracked.data && tracked.data->values.size() == kManusFingerCount && + (tracked.data->endpoint == "left" || tracked.data->endpoint == "right")) + { + std::array powers{}; + for (size_t i = 0; i < kManusFingerCount; ++i) + { + powers[i] = tracked.data->values[i]; + } + apply_haptic_command(tracked.data->endpoint == "left", powers); + } + } + inject_hand_data(); } @@ -106,6 +131,48 @@ std::vector ManusTracker::get_right_node_info() const return m_right_node_info; } +void ManusTracker::apply_haptic_command(bool is_left, const std::array& powers) +{ + uint32_t glove_id = 0; + { + std::lock_guard lock(landscape_mutex); + const auto& opt = is_left ? left_glove_id : right_glove_id; + if (!opt.has_value()) + { + // No glove connected on this side — silently no-op. Spamming the + // log every frame while the glove is disconnected drowns out real + // errors; the user already knows the glove is down because hand + // tracking is unavailable. + return; + } + glove_id = *opt; + } + + // Clamp to [0, 1] — the Manus SDK does the same internally but + // documenting the contract here lets retargeters with looser saturation + // bounds wire up safely. + std::array clamped{}; + for (size_t i = 0; i < clamped.size(); ++i) + { + // std::clamp passes NaN / ±Inf through unchanged, so sanitize first -- + // a non-finite power must never reach the SDK. + clamped[i] = std::isfinite(powers[i]) ? std::clamp(powers[i], 0.0f, 1.0f) : 0.0f; + } + + const SDKReturnCode rc = CoreSdk_VibrateFingersForGlove(glove_id, clamped.data()); + if (rc != SDKReturnCode::SDKReturnCode_Success) + { + const size_t slot = is_left ? 0 : 1; + bool expected = false; + if (m_haptic_error_logged[slot].compare_exchange_strong(expected, true)) + { + std::cerr << "[Manus] CoreSdk_VibrateFingersForGlove failed for " << (is_left ? "left" : "right") + << " glove (id=" << glove_id << ", code=" << static_cast(rc) + << "); further errors for this side will be silenced." << std::endl; + } + } +} + ManusTracker::ManusTracker(const std::string& app_name) noexcept(false) { initialize(app_name); @@ -181,6 +248,15 @@ void ManusTracker::initialize(const std::string& app_name) noexcept(false) << " is not supported by the current runtime; HandTracker will not be created." << std::endl; } + // Registering the reader pulls XR_NVX1_tensor_data into the + // OpenXRSession's required-extension set; the session will fail + // loudly on a runtime that doesn't advertise it. The reader's buffer + // must be >= the producer's collection sample size; we use the shared + // default (matching the producer's PushTensorHapticDevice) rather than + // a Manus-specific size that could drift below it. + m_haptic_reader = std::make_shared(MANUS_GLOVE_COLLECTION_ID); + trackers.push_back(m_haptic_reader); + // Get required extensions from trackers auto extensions = core::DeviceIOSession::get_required_extensions(trackers); extensions.push_back(XR_NVX1_DEVICE_INTERFACE_BASE_EXTENSION_NAME); diff --git a/src/retargeters/tactile_retargeters.py b/src/retargeters/tactile_retargeters.py index 60c080666..5af4d182f 100644 --- a/src/retargeters/tactile_retargeters.py +++ b/src/retargeters/tactile_retargeters.py @@ -8,8 +8,9 @@ * **Composable spatial primitives** -- :class:`Vector3FrameTransform`, :class:`WorldForceAccumulator`, :class:`MagnitudeReducer` -- operating on sim-side ``TactileVector`` flows. -* **Per-device mappers** -- ``Tactile{Vector,Heatmap}ToControllerPulse`` -- - named after the target device-side schema, not the vendor. +* **Per-device mappers** -- ``Tactile{Vector,Heatmap}To{FingerPower,ControllerPulse}`` + and :class:`FingerPowerToControllerPulse` -- named after the target + device-side schema, not the vendor. """ from __future__ import annotations @@ -31,7 +32,9 @@ from isaacteleop.retargeting_engine.tensor_types import ( ControllerHapticPulse, ControllerHapticPulseField, + FingerPowerVector, NUM_CONTROLLER_HAPTIC_FIELDS, + NUM_HAPTIC_FINGERS, TactileHeatmap, TactileVector, TransformMatrix, @@ -156,7 +159,8 @@ class MagnitudeReducer(BaseRetargeter): """Reduce a ``TactileVector(3)`` to a ``TactileVector(1)`` scalar. Bridges directional contact data into frame-invariant device schemas - such as ``ControllerHapticPulse``. Mode is fixed at construction: + (``FingerPowerVector``, ``ControllerHapticPulse``). Mode is fixed at + construction: * ``"norm"`` -- Euclidean length ``||vec||_2``. * ``"axis_x"`` / ``"axis_y"`` / ``"axis_z"`` -- absolute value of the @@ -218,6 +222,414 @@ def _apply_gain_curve( return np.clip(scaled, 0.0, saturation).astype(np.float32) +def _smooth_ema(prev: np.ndarray | None, new: np.ndarray, alpha: float) -> np.ndarray: + """Exponential moving average smoothing. ``alpha`` is the new-sample weight.""" + if prev is None or prev.shape != new.shape: + return new.copy() + return (alpha * new + (1.0 - alpha) * prev).astype(np.float32) + + +# ============================================================================ +# Per-device mappers -- target schema: FingerPowerVector +# ============================================================================ + + +class TactileVectorToFingerPower(BaseRetargeter): + """Map a per-taxel :func:`TactileVector` to a :func:`FingerPowerVector`. + + Inputs: + - ``"tactile"``: :func:`TactileVector(num_taxels) ` + -- typically each taxel is the contact-force magnitude on one finger + pad, but the mapping is configurable via ``finger_groups``. + + Outputs: + - ``"powers"``: :func:`FingerPowerVector(num_fingers) ` + in ``[0, 1]``. + + Per-finger reduction over the configured taxel indices is the configured + ``reduction`` mode (``"max"``, ``"mean"``, or ``"sum"``). The result is + passed through the standard gain / deadband / saturation curve and + optionally EMA-smoothed. + + Tunable parameters (all surface in the tuning UI): + - ``gain``: float, scales the post-deadband signal. + - ``deadband``: float, suppresses signals below this magnitude. + - ``saturation``: float, upper clamp (default 1.0, the haptic-glove + schema upper bound). + - ``smoothing``: float in [0, 1], EMA new-sample weight (1.0 = no smoothing). + """ + + INPUT_TACTILE = "tactile" + OUTPUT_POWERS = "powers" + + def __init__( + self, + name: str, + num_taxels: int, + finger_groups: list[list[int]] | None = None, + num_fingers: int = NUM_HAPTIC_FINGERS, + reduction: Literal["max", "mean", "sum"] = "max", + gain: float = 1.0, + deadband: float = 0.0, + saturation: float = 1.0, + smoothing: float = 1.0, + ) -> None: + if num_taxels < 1: + raise ValueError( + f"TactileVectorToFingerPower '{name}' requires num_taxels >= 1" + ) + if reduction not in ("max", "mean", "sum"): + raise ValueError( + f"TactileVectorToFingerPower '{name}': unknown reduction '{reduction}'" + ) + + self._num_taxels = num_taxels + self._num_fingers = num_fingers + self._reduction = reduction + + if finger_groups is None: + if num_taxels != num_fingers: + raise ValueError( + f"TactileVectorToFingerPower '{name}': finger_groups is required " + f"unless num_taxels ({num_taxels}) equals num_fingers ({num_fingers})." + ) + finger_groups = [[i] for i in range(num_fingers)] + if len(finger_groups) != num_fingers: + raise ValueError( + f"TactileVectorToFingerPower '{name}': finger_groups has " + f"{len(finger_groups)} entries, expected {num_fingers}." + ) + for fi, group in enumerate(finger_groups): + for idx in group: + if not (0 <= idx < num_taxels): + raise ValueError( + f"TactileVectorToFingerPower '{name}': finger_groups[{fi}] " + f"contains taxel index {idx} outside [0, {num_taxels})." + ) + self._finger_groups = [list(g) for g in finger_groups] + + # Synced from ParameterState before each compute by BaseRetargeter. + self._gain = gain + self._deadband = deadband + self._saturation = saturation + self._smoothing = smoothing + self._smoothed: np.ndarray | None = None + + param_state = ParameterState( + name, + [ + FloatParameter( + name="gain", + description="Scale factor applied after the deadband.", + default_value=gain, + min_value=0.0, + max_value=100.0, + sync_fn=lambda v: setattr(self, "_gain", float(v)), + ), + FloatParameter( + name="deadband", + description="Signal magnitude below which output is zero.", + default_value=deadband, + min_value=0.0, + max_value=10.0, + sync_fn=lambda v: setattr(self, "_deadband", float(v)), + ), + FloatParameter( + name="saturation", + description="Maximum per-finger power (clamped at 1.0).", + default_value=saturation, + min_value=0.0, + max_value=1.0, + sync_fn=lambda v: setattr(self, "_saturation", float(v)), + ), + FloatParameter( + name="smoothing", + description="EMA new-sample weight in [0,1]. 1.0 = no smoothing.", + default_value=smoothing, + min_value=0.0, + max_value=1.0, + sync_fn=lambda v: setattr(self, "_smoothing", float(v)), + ), + ], + ) + super().__init__(name=name, parameter_state=param_state) + + def input_spec(self) -> RetargeterIOType: + return {self.INPUT_TACTILE: TactileVector(self._num_taxels)} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_POWERS: FingerPowerVector(self._num_fingers)} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + if context.execution_events.reset: + self._smoothed = None + + raw = np.asarray(inputs[self.INPUT_TACTILE][0], dtype=np.float32).reshape( + self._num_taxels + ) + + per_finger = np.zeros(self._num_fingers, dtype=np.float32) + for fi, group in enumerate(self._finger_groups): + slice_ = raw[group] + if self._reduction == "max": + per_finger[fi] = float(np.max(slice_)) if slice_.size else 0.0 + elif self._reduction == "mean": + per_finger[fi] = float(np.mean(slice_)) if slice_.size else 0.0 + else: # sum + per_finger[fi] = float(np.sum(slice_)) + + shaped = _apply_gain_curve( + per_finger, self._gain, self._deadband, self._saturation + ) + self._smoothed = _smooth_ema(self._smoothed, shaped, self._smoothing) + outputs[self.OUTPUT_POWERS][0] = self._smoothed.copy() + + +class TactileHeatmapToFingerPower(BaseRetargeter): + """Reduce a :func:`TactileHeatmap` to a :func:`FingerPowerVector`, one finger per pad. + + Inputs: + - ``"heatmap"``: :func:`TactileHeatmap(rows, cols, num_pads) ` + with ``num_pads == num_fingers``. + + Outputs: + - ``"powers"``: :func:`FingerPowerVector(num_fingers) `. + + Each ``(rows, cols)`` pad is reduced to one scalar via the configured + ``reduction`` (``"max"``, ``"mean"``, or ``"sum"``), then run through the + standard gain / deadband / saturation curve and optionally EMA-smoothed. + + Tunable parameters: ``gain``, ``deadband``, ``saturation``, ``smoothing``. + """ + + INPUT_HEATMAP = "heatmap" + OUTPUT_POWERS = "powers" + + def __init__( + self, + name: str, + rows: int, + cols: int, + num_pads: int = NUM_HAPTIC_FINGERS, + reduction: Literal["max", "mean", "sum"] = "max", + gain: float = 1.0, + deadband: float = 0.0, + saturation: float = 1.0, + smoothing: float = 1.0, + ) -> None: + if rows < 1 or cols < 1 or num_pads < 1: + raise ValueError( + f"TactileHeatmapToFingerPower '{name}' requires rows/cols/num_pads >= 1, " + f"got rows={rows}, cols={cols}, num_pads={num_pads}" + ) + if reduction not in ("max", "mean", "sum"): + raise ValueError( + f"TactileHeatmapToFingerPower '{name}': unknown reduction '{reduction}'" + ) + self._rows = rows + self._cols = cols + self._num_pads = num_pads + self._reduction = reduction + + self._gain = gain + self._deadband = deadband + self._saturation = saturation + self._smoothing = smoothing + self._smoothed: np.ndarray | None = None + + param_state = ParameterState( + name, + [ + FloatParameter( + name="gain", + description="Scale factor applied after the deadband.", + default_value=gain, + min_value=0.0, + max_value=100.0, + sync_fn=lambda v: setattr(self, "_gain", float(v)), + ), + FloatParameter( + name="deadband", + description="Pad-reduced magnitude below which output is zero.", + default_value=deadband, + min_value=0.0, + max_value=10.0, + sync_fn=lambda v: setattr(self, "_deadband", float(v)), + ), + FloatParameter( + name="saturation", + description="Maximum per-finger power.", + default_value=saturation, + min_value=0.0, + max_value=1.0, + sync_fn=lambda v: setattr(self, "_saturation", float(v)), + ), + FloatParameter( + name="smoothing", + description="EMA new-sample weight in [0,1]. 1.0 = no smoothing.", + default_value=smoothing, + min_value=0.0, + max_value=1.0, + sync_fn=lambda v: setattr(self, "_smoothing", float(v)), + ), + ], + ) + super().__init__(name=name, parameter_state=param_state) + + def input_spec(self) -> RetargeterIOType: + return { + self.INPUT_HEATMAP: TactileHeatmap(self._rows, self._cols, self._num_pads) + } + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_POWERS: FingerPowerVector(self._num_pads)} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + if context.execution_events.reset: + self._smoothed = None + + heatmap = np.asarray(inputs[self.INPUT_HEATMAP][0], dtype=np.float32).reshape( + self._num_pads, self._rows, self._cols + ) + + if self._reduction == "max": + per_pad = heatmap.max(axis=(1, 2)) + elif self._reduction == "mean": + per_pad = heatmap.mean(axis=(1, 2)) + else: # sum + per_pad = heatmap.sum(axis=(1, 2)) + + shaped = _apply_gain_curve( + per_pad.astype(np.float32), + self._gain, + self._deadband, + self._saturation, + ) + self._smoothed = _smooth_ema(self._smoothed, shaped, self._smoothing) + outputs[self.OUTPUT_POWERS][0] = self._smoothed.copy() + + +class TactileHeatmapToWristPulse(BaseRetargeter): + """Collapse a full :func:`TactileHeatmap` to a single scalar. + + Inputs: + - ``"heatmap"``: :func:`TactileHeatmap(rows, cols, num_pads) `. + + Outputs: + - ``"power"``: :func:`FingerPowerVector(1) ` + (single-channel power; reused here for wrist-only devices to avoid + introducing a schema with no concrete consumer). + + Reduction is over the entire ``(num_pads, rows, cols)`` array via + ``"max"``, ``"mean"``, or ``"sum"``. Standard gain / deadband / saturation + curve and EMA smoothing follow. + """ + + INPUT_HEATMAP = "heatmap" + OUTPUT_POWER = "power" + + def __init__( + self, + name: str, + rows: int, + cols: int, + num_pads: int = 1, + reduction: Literal["max", "mean", "sum"] = "max", + gain: float = 1.0, + deadband: float = 0.0, + saturation: float = 1.0, + smoothing: float = 1.0, + ) -> None: + if rows < 1 or cols < 1 or num_pads < 1: + raise ValueError( + f"TactileHeatmapToWristPulse '{name}' requires rows/cols/num_pads >= 1, " + f"got rows={rows}, cols={cols}, num_pads={num_pads}" + ) + if reduction not in ("max", "mean", "sum"): + raise ValueError( + f"TactileHeatmapToWristPulse '{name}': unknown reduction '{reduction}'" + ) + self._rows = rows + self._cols = cols + self._num_pads = num_pads + self._reduction = reduction + + self._gain = gain + self._deadband = deadband + self._saturation = saturation + self._smoothing = smoothing + self._smoothed: np.ndarray | None = None + + param_state = ParameterState( + name, + [ + FloatParameter( + name="gain", + description="Scale factor applied after the deadband.", + default_value=gain, + min_value=0.0, + max_value=100.0, + sync_fn=lambda v: setattr(self, "_gain", float(v)), + ), + FloatParameter( + name="deadband", + description="Pulse magnitude below which output is zero.", + default_value=deadband, + min_value=0.0, + max_value=10.0, + sync_fn=lambda v: setattr(self, "_deadband", float(v)), + ), + FloatParameter( + name="saturation", + description="Maximum pulse magnitude.", + default_value=saturation, + min_value=0.0, + max_value=1.0, + sync_fn=lambda v: setattr(self, "_saturation", float(v)), + ), + FloatParameter( + name="smoothing", + description="EMA new-sample weight in [0,1]. 1.0 = no smoothing.", + default_value=smoothing, + min_value=0.0, + max_value=1.0, + sync_fn=lambda v: setattr(self, "_smoothing", float(v)), + ), + ], + ) + super().__init__(name=name, parameter_state=param_state) + + def input_spec(self) -> RetargeterIOType: + return { + self.INPUT_HEATMAP: TactileHeatmap(self._rows, self._cols, self._num_pads) + } + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_POWER: FingerPowerVector(1)} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + if context.execution_events.reset: + self._smoothed = None + + heatmap = np.asarray(inputs[self.INPUT_HEATMAP][0], dtype=np.float32) + + if self._reduction == "max": + scalar = float(heatmap.max()) if heatmap.size else 0.0 + elif self._reduction == "mean": + scalar = float(heatmap.mean()) if heatmap.size else 0.0 + else: # sum + scalar = float(heatmap.sum()) + + shaped = _apply_gain_curve( + np.array([scalar], dtype=np.float32), + self._gain, + self._deadband, + self._saturation, + ) + self._smoothed = _smooth_ema(self._smoothed, shaped, self._smoothing) + outputs[self.OUTPUT_POWER][0] = self._smoothed.copy() + + # ============================================================================ # Per-device mappers -- target schema: ControllerHapticPulse # ============================================================================ @@ -241,14 +653,14 @@ class TactileVectorToControllerPulse(BaseRetargeter): (``"max"``, ``"mean"``, ``"sum"``), passed through the gain / deadband / saturation curve to become ``amplitude`` in ``[0, saturation]``, then paired with constant ``frequency_hz`` / ``duration_s`` parameters - (defaults ``0.0`` map to ``XR_FREQUENCY_UNSPECIFIED`` / - ``XR_MIN_HAPTIC_DURATION`` on the runtime). + (defaults ``0.0`` select the backend's default frequency and shortest + supported pulse). Tunable parameters: ``gain``, ``deadband``, ``saturation``, - ``frequency_hz``, ``duration_s``. No ``smoothing`` parameter: - ``xrApplyHapticFeedback`` supersedes any in-flight pulse every frame, so - EMA-smoothing in Python only shifts latency. Add an upstream low-pass - retargeter on the ``TactileVector`` input if you need temporal shaping. + ``frequency_hz``, ``duration_s``. No ``smoothing`` parameter: the backend + supersedes any in-flight pulse every frame, so EMA-smoothing in Python only + shifts latency. Add an upstream low-pass retargeter on the + ``TactileVector`` input if you need temporal shaping. """ INPUT_TACTILE = "tactile" @@ -307,7 +719,7 @@ def __init__( ), FloatParameter( name="frequency_hz", - description="OpenXR pulse frequency [Hz]. 0 = XR_FREQUENCY_UNSPECIFIED.", + description="Pulse frequency [Hz]. 0 selects the backend's default frequency.", default_value=frequency_hz, min_value=0.0, max_value=1000.0, @@ -315,7 +727,7 @@ def __init__( ), FloatParameter( name="duration_s", - description="OpenXR pulse duration [s]. 0 = XR_MIN_HAPTIC_DURATION.", + description="Pulse duration [s]. 0 selects the shortest pulse the backend supports.", default_value=duration_s, min_value=0.0, max_value=10.0, @@ -431,7 +843,7 @@ def __init__( ), FloatParameter( name="frequency_hz", - description="OpenXR pulse frequency [Hz]. 0 = XR_FREQUENCY_UNSPECIFIED.", + description="Pulse frequency [Hz]. 0 selects the backend's default frequency.", default_value=frequency_hz, min_value=0.0, max_value=1000.0, @@ -439,7 +851,7 @@ def __init__( ), FloatParameter( name="duration_s", - description="OpenXR pulse duration [s]. 0 = XR_MIN_HAPTIC_DURATION.", + description="Pulse duration [s]. 0 selects the shortest pulse the backend supports.", default_value=duration_s, min_value=0.0, max_value=10.0, @@ -480,3 +892,134 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N pulse[ControllerHapticPulseField.FREQUENCY_HZ] = self._frequency_hz pulse[ControllerHapticPulseField.DURATION_S] = self._duration_s outputs[self.OUTPUT_PULSE][0] = pulse + + +class FingerPowerToControllerPulse(BaseRetargeter): + """Reduce a :func:`FingerPowerVector` to one :func:`ControllerHapticPulse`. + + Bridges per-finger glove output to single-channel controller rumble. + Collapses the channels to a single amplitude via ``reduction``, applies + the same gain / deadband / saturation curve as + :class:`TactileVectorToControllerPulse` (so the rumble can be tuned + independently of the upstream signal), and pairs the result with constant + ``frequency_hz`` / ``duration_s`` parameters. + + Inputs: ``"powers"`` -- ``FingerPowerVector(num_fingers)``. + Outputs: ``"pulse"`` -- ``[amplitude, frequency_hz, duration_s]``. + + Tunable: ``gain``, ``deadband``, ``saturation``, ``frequency_hz``, + ``duration_s``. No ``smoothing`` for the same reason as + :class:`TactileVectorToControllerPulse`. + """ + + INPUT_POWERS = "powers" + OUTPUT_PULSE = "pulse" + + def __init__( + self, + name: str, + num_fingers: int = NUM_HAPTIC_FINGERS, + reduction: Literal["max", "mean", "sum"] = "max", + gain: float = 1.0, + deadband: float = 0.0, + saturation: float = 1.0, + frequency_hz: float = 0.0, + duration_s: float = 0.0, + ) -> None: + if num_fingers < 1: + raise ValueError( + f"FingerPowerToControllerPulse '{name}' requires num_fingers >= 1, got {num_fingers}" + ) + if reduction not in ("max", "mean", "sum"): + raise ValueError( + f"FingerPowerToControllerPulse '{name}': unknown reduction '{reduction}'" + ) + + self._num_fingers = num_fingers + self._reduction = reduction + + self._gain = gain + self._deadband = deadband + self._saturation = saturation + self._frequency_hz = frequency_hz + self._duration_s = duration_s + + param_state = ParameterState( + name, + [ + FloatParameter( + name="gain", + description="Scale factor applied after the deadband.", + default_value=gain, + min_value=0.0, + max_value=100.0, + sync_fn=lambda v: setattr(self, "_gain", float(v)), + ), + FloatParameter( + name="deadband", + description="Amplitude below which the pulse is suppressed.", + default_value=deadband, + min_value=0.0, + max_value=10.0, + sync_fn=lambda v: setattr(self, "_deadband", float(v)), + ), + FloatParameter( + name="saturation", + description="Maximum pulse amplitude in [0, 1].", + default_value=saturation, + min_value=0.0, + max_value=1.0, + sync_fn=lambda v: setattr(self, "_saturation", float(v)), + ), + FloatParameter( + name="frequency_hz", + description="Pulse frequency [Hz]. 0 selects the backend's default frequency.", + default_value=frequency_hz, + min_value=0.0, + max_value=1000.0, + sync_fn=lambda v: setattr(self, "_frequency_hz", float(v)), + ), + FloatParameter( + name="duration_s", + description="Pulse duration [s]. 0 selects the shortest pulse the backend supports.", + default_value=duration_s, + min_value=0.0, + max_value=10.0, + sync_fn=lambda v: setattr(self, "_duration_s", float(v)), + ), + ], + ) + super().__init__(name=name, parameter_state=param_state) + + def input_spec(self) -> RetargeterIOType: + return {self.INPUT_POWERS: FingerPowerVector(self._num_fingers)} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_PULSE: ControllerHapticPulse()} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + powers = np.asarray(inputs[self.INPUT_POWERS][0], dtype=np.float32).reshape( + self._num_fingers + ) + + if self._reduction == "max": + scalar = float(powers.max()) if powers.size else 0.0 + elif self._reduction == "mean": + scalar = float(powers.mean()) if powers.size else 0.0 + else: # sum + scalar = float(powers.sum()) + + amplitude = float( + _apply_gain_curve( + np.array([scalar], dtype=np.float32), + self._gain, + self._deadband, + self._saturation, + )[0] + ) + + pulse = np.zeros(NUM_CONTROLLER_HAPTIC_FIELDS, dtype=np.float32) + pulse[ControllerHapticPulseField.AMPLITUDE] = amplitude + pulse[ControllerHapticPulseField.FREQUENCY_HZ] = self._frequency_hz + pulse[ControllerHapticPulseField.DURATION_S] = self._duration_s + outputs[self.OUTPUT_PULSE][0] = pulse