From 88b37f94c7d3ecfbed33e68e79b7c8e3022fa616 Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Thu, 21 May 2026 12:51:49 -0400 Subject: [PATCH 1/5] Initial haptics and tactile feedback implementation --- CMakeLists.txt | 10 + examples/haptic_feedback/CMakeLists.txt | 7 + examples/haptic_feedback/python/README.md | 173 ++++ .../python/hand_pinch_haptic_example.py | 652 ++++++++++++ .../openxr_controller_haptic_example.py | 516 ++++++++++ .../haptic_feedback/python/pyproject.toml | 12 + src/core/AGENTS.md | 1 + .../deviceio_base/controller_tracker_base.hpp | 20 +- .../cpp/controller_tracker.cpp | 12 +- .../deviceio_trackers/controller_tracker.hpp | 21 +- .../python/tracker_bindings.cpp | 29 +- .../cpp/live_controller_tracker_impl.cpp | 83 ++ .../cpp/live_controller_tracker_impl.hpp | 15 + .../oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp | 11 + src/core/python/pyproject.toml.in | 2 + .../cpp/replay_controller_tracker_impl.hpp | 7 + .../python/deviceio_source_nodes/__init__.py | 2 + .../deviceio_source_nodes/haptic_sink.py | 120 +++ .../python/tensor_types/__init__.py | 25 + .../python/tensor_types/indices.py | 26 + .../python/tensor_types/tactile_types.py | 187 ++++ .../python/test_haptic_devices.py | 227 +++++ .../python/test_haptic_sink.py | 211 ++++ .../python/test_tactile_retargeters.py | 360 +++++++ src/haptic_devices/CMakeLists.txt | 15 + src/haptic_devices/__init__.py | 26 + src/haptic_devices/interface.py | 100 ++ src/haptic_devices/manus.py | 118 +++ src/haptic_devices/openxr_controller.py | 262 +++++ src/plugins/manus/CMakeLists.txt | 4 + .../manus/core/manus_hand_tracking_plugin.cpp | 62 ++ .../inc/core/manus_hand_tracking_plugin.hpp | 34 +- src/plugins/manus/python/CMakeLists.txt | 52 + .../manus/python/manus_haptic_bindings.cpp | 91 ++ src/retargeters/tactile_retargeters.py | 942 ++++++++++++++++++ 35 files changed, 4430 insertions(+), 5 deletions(-) create mode 100644 examples/haptic_feedback/CMakeLists.txt create mode 100644 examples/haptic_feedback/python/README.md create mode 100644 examples/haptic_feedback/python/hand_pinch_haptic_example.py create mode 100644 examples/haptic_feedback/python/openxr_controller_haptic_example.py create mode 100644 examples/haptic_feedback/python/pyproject.toml create mode 100644 src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py create mode 100644 src/core/retargeting_engine/python/tensor_types/tactile_types.py create mode 100644 src/core/retargeting_engine_tests/python/test_haptic_devices.py create mode 100644 src/core/retargeting_engine_tests/python/test_haptic_sink.py create mode 100644 src/core/retargeting_engine_tests/python/test_tactile_retargeters.py create mode 100644 src/haptic_devices/CMakeLists.txt create mode 100644 src/haptic_devices/__init__.py create mode 100644 src/haptic_devices/interface.py create mode 100644 src/haptic_devices/manus.py create mode 100644 src/haptic_devices/openxr_controller.py create mode 100644 src/plugins/manus/python/CMakeLists.txt create mode 100644 src/plugins/manus/python/manus_haptic_bindings.cpp create mode 100644 src/retargeters/tactile_retargeters.py diff --git a/CMakeLists.txt b/CMakeLists.txt index adec5c24a..6039cbfb9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -124,6 +124,15 @@ add_subdirectory(src/core) # Build retargeters (pure Python, sibling of src/core) add_subdirectory(src/retargeters) +# Build haptic device adapters (pure Python, sibling of src/core). +# Vendor-specific pybind extensions (e.g. src/plugins/manus/python) write +# their .so files into the wheel-staging tree at +# python_package//isaacteleop/haptic_devices/ alongside the Python adapter +# modules so adapters can lazy-import them via `from . import __haptic`. +# (cmake-install of those extensions goes to ${CMAKE_INSTALL_LIBDIR} per the +# vendor plugin's own install rules; verify wheel layout matches.) +add_subdirectory(src/haptic_devices) + # Build Televiz visualization module (sibling of src/core). # src/viz/CMakeLists.txt orchestrates its own sub-modules and tests. if(BUILD_VIZ) @@ -139,6 +148,7 @@ if(BUILD_EXAMPLES) add_subdirectory(examples/schemaio) add_subdirectory(examples/native_openxr) add_subdirectory(examples/mcap_record_replay) + add_subdirectory(examples/haptic_feedback) if(BUILD_VIZ) add_subdirectory(examples/camera_viz/tests) endif() diff --git a/examples/haptic_feedback/CMakeLists.txt b/examples/haptic_feedback/CMakeLists.txt new file mode 100644 index 000000000..0bc23be68 --- /dev/null +++ b/examples/haptic_feedback/CMakeLists.txt @@ -0,0 +1,7 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.20) + +include(${CMAKE_SOURCE_DIR}/cmake/InstallPythonExample.cmake) +install_python_example(DESTINATION examples/haptic_feedback/python) diff --git a/examples/haptic_feedback/python/README.md b/examples/haptic_feedback/python/README.md new file mode 100644 index 000000000..12869831b --- /dev/null +++ b/examples/haptic_feedback/python/README.md @@ -0,0 +1,173 @@ + + +# Haptic Feedback Examples + +End-to-end CLI demos for the tactile / haptic-feedback flow introduced in Isaac +Teleop. Each demo builds a `TeleopSession` whose pipeline reaches **backwards** +through the retargeting graph -- from a tactile-shaped signal source, through +the per-device retargeter and `HapticSink`, into the matching +`IHapticDevice` adapter and out to hardware. + +| Example | Input | Device | What it demonstrates | +| --- | --- | --- | --- | +| `openxr_controller_haptic_example.py` | Controller trigger value (or a synthetic sine) | OpenXR motion controller | The full session-aware sink + source pattern (`HapticSink` + `OpenXRControllerHapticSource`) -- one `ControllerHapticPulse` per hand per frame. | +| `hand_pinch_haptic_example.py` | Hand-tracking joint poses (XR_EXT_hand_tracking) | Manus glove (default) or OpenXR controller | Per-finger vibration driven by the distance from each finger tip to the thumb tip. The closer the finger to the thumb, the stronger the buzz on *both* pads. | + +## Pinch-proximity haptics (`hand_pinch_haptic_example.py`) + +Reads `HandsSource` joint poses, computes the Euclidean distance from each +finger tip to the thumb tip per hand, and ramps that distance into per-finger +vibration amplitudes. As the finger approaches the thumb the vibration grows; +when you actually touch them together both pads buzz at the saturation +amplitude. Works on either hand; the two hands are independent. + +### Pipeline (`--device manus`, default) + +``` +HandsSource (left + right) + | + v +PinchProximityToFingerPower (HandInput -> FingerPowerVector(5)) + | + v +HapticSink(ManusHapticDevice()) +``` + +### Pipeline (`--device openxr_controller`) + +Collapses per-finger powers to a single controller pulse so the demo works on +a Quest/Index without Manus hardware -- useful as a hand-tracking smoke test. +Reuses the `ControllerTracker` from `ControllersSource` to avoid spawning two +competing `LiveControllerTrackerImpl` instances. + +``` +HandsSource (left + right) ControllersSource (tracker handle only) + | | + v | +PinchProximityToFingerPower | + | | + v | +FingerPowerToControllerPulse (max across fingers) | + | | + v v +HapticSink(OpenXRControllerHapticDevice()) -> OpenXRControllerHapticSource + poll_tracker(session) +``` + +### Usage + +```bash +# Default: Manus per-finger haptics. Pinch any finger toward the thumb. +uv run hand_pinch_haptic_example.py + +# Loosen the ramp so vibration kicks in at 15 cm and saturates at 1 cm. +uv run hand_pinch_haptic_example.py --max-distance-m 0.15 --min-distance-m 0.01 + +# Steeper falloff -- only register a strong pinch when very close. +uv run hand_pinch_haptic_example.py --falloff-exponent 2.5 + +# Same demo on OpenXR controllers (no Manus needed). Both controllers rumble +# with the strongest finger-to-thumb proximity on their respective hand. +uv run hand_pinch_haptic_example.py --device openxr_controller +``` + +### Notes + +- Requires an XR runtime that exposes `XR_EXT_hand_tracking`. Quest 3 (via + CloudXR or native), Manus gloves with optical fallback, and Vive Index with + hand-tracking add-ons all satisfy this. Without hand tracking the + retargeter sees `is_none` and quietly outputs zero amplitude. +- `--device manus` requires the Manus plugin to have already been started so + the singleton is alive (see `src/plugins/manus/README.md`). When the plugin + is not built or no glove is connected, the per-side adapter logs once and + silently no-ops -- the rest of the session keeps running. +- The retargeter's tunable parameters (`--max-distance-m`, `--saturation`, + `--falloff-exponent`) are constructor-time only in this example. The + production tuning-UI flow lives on the + `isaacteleop.retargeters.tactile_retargeters` mappers, which carry a + `ParameterState` for live adjustment from the existing + `MultiRetargeterTuningUIImGui` panel. + +## OpenXR motion-controller haptics (`openxr_controller_haptic_example.py`) + +Drives the haptic actuator on each OpenXR motion controller through the full +stack. Two demo modes: + +| Mode (default `trigger`) | Behaviour | +| --- | --- | +| `--mode trigger` (default; `--same-hand` to flip) | Pulling the **left** trigger rumbles the **right** controller and vice versa (cross-hand by default). | +| `--mode sine` | Both controllers rumble on a smooth half-rectified sine envelope. No controller input required -- useful as a hardware smoke test. | + +### Pipeline + +``` +ControllersSource (input) OpenXRControllerHapticDevice + | ^ ^ + v | | +TriggerToTactile -> TactileVectorToControllerPulse -> HapticSink + | + v + OpenXRControllerHapticSource + poll_tracker(session) +``` + +The same `ControllerTracker` instance is reused by both `ControllersSource` and +`OpenXRControllerHapticSource`, so `DeviceIOSession` only creates one +`LiveControllerTrackerImpl` and there is no contention on the underlying +OpenXR action set. + +### Usage + +```bash +# Default: cross-hand trigger -> rumble (most fun on a single user). +uv run openxr_controller_haptic_example.py + +# Same-hand instead of cross-hand. +uv run openxr_controller_haptic_example.py --same-hand + +# Open-loop sine wave smoke test (no input needed). +uv run openxr_controller_haptic_example.py --mode sine --sine-period 1.5 + +# Override OpenXR pulse parameters (defaults select runtime-picked values). +uv run openxr_controller_haptic_example.py --frequency-hz 320 --duration-s 0.05 +``` + +### Notes + +- Requires an OpenXR runtime that exposes haptic output on the standard + `/user/hand/{left,right}/output/haptic` paths. Verified runtimes: Quest 2/3 + via CloudXR, Vive Index, Pico 4. Runtimes that omit `xrApplyHapticFeedback` + silently no-op without tearing the session down. +- `--frequency-hz 0` selects `XR_FREQUENCY_UNSPECIFIED` and `--duration-s 0` + selects `XR_MIN_HAPTIC_DURATION`. Both defaults work on every conformant + runtime, so leave them at zero unless you need a specific waveform. +- The OpenXR controller haptic adapter is the canonical example of an + `IHapticDevice` that needs a session reference at write-time. See the + design doc (`IsaacLab/docs/tactile_haptic_feedback_design.md`, §4.7 and + §5.6) for the architectural rationale. + +## Quick start + +```bash +# Build & install Isaac Teleop (from the repo root). Set CMAKE_INSTALL_PREFIX +# to a writable path so the demo and its install tree end up somewhere you +# can run them from -- the default `/usr/local` requires root and is harder +# to clean up. +cd IsaacTeleop +cmake -B build -DCMAKE_INSTALL_PREFIX="$PWD/install" +cmake --build build --target install -j16 + +# Run the demo from the install tree. +cd install/examples/haptic_feedback/python +uv run openxr_controller_haptic_example.py +``` + +## See also + +- **Design doc:** `IsaacLab/docs/tactile_haptic_feedback_design.md` +- **Haptic adapters:** `isaacteleop.haptic_devices` +- **Per-device mappers:** `isaacteleop.retargeters.tactile_retargeters` +- **Sink:** `isaacteleop.retargeting_engine.deviceio_source_nodes.HapticSink` 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..523044c36 --- /dev/null +++ b/examples/haptic_feedback/python/hand_pinch_haptic_example.py @@ -0,0 +1,652 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Pinch-proximity haptic feedback: feel each finger approach the thumb. + +Reads XR hand-tracking joint poses, computes the Euclidean distance between +each finger tip and the thumb tip for both hands, and ramps that distance +into per-finger vibration amplitudes. The closer the finger tip is to the +thumb tip, the stronger both the finger *and* the thumb vibrate -- so when +you actually touch them together you get a maximum-intensity pulse on both. + +Pipeline (default ``--device manus``): + +:: + + HandsSource (left + right) + | + v + PinchProximityToFingerPower (per side; HandInput -> FingerPowerVector(5)) + | + v + HapticSink(ManusHapticDevice()) + +For users without Manus gloves, ``--device openxr_controller`` collapses each +per-side ``FingerPowerVector(5)`` to a single ``ControllerHapticPulse`` (max +across fingers) and routes it through the OpenXR controller haptic stack. +This is useful as a hardware smoke test or when running on a Quest with +hand-tracking enabled but no Manus available. + +The retargeter is intentionally vendor-neutral: it produces +``FingerPowerVector(num_fingers=5)`` per the +``isaacteleop.retargeting_engine.tensor_types.tactile_types`` schema, and +any ``IHapticDevice`` whose ``accepted_type()`` is that schema can be wired +into the same sink without changing the per-side mapper. +""" + +from __future__ import annotations + +import argparse +import sys +import time +from typing import Any + +import numpy as np + +from isaacteleop.haptic_devices.manus import ManusHapticDevice +from isaacteleop.haptic_devices.openxr_controller import ( + OpenXRControllerHapticDevice, + OpenXRControllerHapticSource, +) +from isaacteleop.retargeting_engine.deviceio_source_nodes import ( + ControllersSource, + 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 ( + ControllerHapticPulse, + ControllerHapticPulseField, + FingerIndex, + FingerPowerVector, + HandInput, + HandInputIndex, + HandJointIndex, + NUM_CONTROLLER_HAPTIC_FIELDS, + NUM_HAPTIC_FINGERS, +) +from isaacteleop.teleop_session_manager import TeleopSession, TeleopSessionConfig + + +# Maps each non-thumb finger in FingerPowerVector to its OpenXR fingertip joint. +# `FingerIndex.THUMB` is intentionally absent: the thumb pad's vibration mirrors +# the strongest non-thumb finger's pinch (filled in `_compute_fn` after this +# loop), so iterating only the four other fingers here keeps the logic linear. +_FINGER_TIP_JOINTS: dict[FingerIndex, HandJointIndex] = { + FingerIndex.INDEX: HandJointIndex.INDEX_TIP, + FingerIndex.MIDDLE: HandJointIndex.MIDDLE_TIP, + FingerIndex.RING: HandJointIndex.RING_TIP, + FingerIndex.PINKY: HandJointIndex.LITTLE_TIP, +} + + +# ============================================================================ +# Pinch retargeter -- HandInput -> FingerPowerVector +# ============================================================================ + + +class PinchProximityToFingerPower(BaseRetargeter): + """Per-hand: distance(thumb_tip, finger_tip) -> per-finger vibration intensity. + + Inputs: + - ``"hand"``: optional :func:`HandInput ` + -- when absent (hand not tracked), the output is all zeros. + + Outputs: + - ``"powers"``: :func:`FingerPowerVector(5) ` + in ``[0, saturation]`` (Manus-compatible order: + ``[Thumb, Index, Middle, Ring, Pinky]``). + + Math: + For each non-thumb finger ``f``:: + + d_f = ||joint_pos[f_tip] - joint_pos[THUMB_TIP]|| + x = (max_distance_m - d_f) / (max_distance_m - min_distance_m) + powers[f] = clip(x ** falloff_exponent, 0.0, saturation) + + The thumb's slot is filled with the max across the four other fingers, + so any pinch event the user feels on a finger is also felt on the + thumb -- the intuitive "both sides of the pinch buzz" behaviour. + + Tunables are exposed as constructor arguments (not ``ParameterState``) + because this is an example retargeter; the production tuning UI integration + lives on the :mod:`~isaacteleop.retargeters.tactile_retargeters` mappers. + """ + + INPUT_HAND = "hand" + OUTPUT_POWERS = "powers" + + def __init__( + self, + name: str, + max_distance_m: float = 0.10, + min_distance_m: float = 0.005, + saturation: float = 1.0, + falloff_exponent: float = 1.0, + ) -> None: + if max_distance_m <= min_distance_m: + raise ValueError( + f"PinchProximityToFingerPower '{name}': " + f"max_distance_m ({max_distance_m}) must be > min_distance_m ({min_distance_m})" + ) + if not 0.0 <= saturation <= 1.0: + raise ValueError( + f"PinchProximityToFingerPower '{name}': saturation must be in [0, 1], got {saturation}" + ) + if falloff_exponent <= 0.0: + raise ValueError( + f"PinchProximityToFingerPower '{name}': falloff_exponent must be > 0, got {falloff_exponent}" + ) + + self._max_distance_m = float(max_distance_m) + self._min_distance_m = float(min_distance_m) + self._saturation = float(saturation) + self._falloff_exponent = float(falloff_exponent) + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return {self.INPUT_HAND: OptionalType(HandInput())} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_POWERS: FingerPowerVector(NUM_HAPTIC_FINGERS)} + + def _compute_fn( + self, inputs: RetargeterIO, outputs: RetargeterIO, context: ComputeContext + ) -> None: + powers = np.zeros(NUM_HAPTIC_FINGERS, dtype=np.float32) + + hand = inputs[self.INPUT_HAND] + if hand.is_none: + outputs[self.OUTPUT_POWERS][0] = powers + return + + joint_positions = np.asarray(hand[HandInputIndex.JOINT_POSITIONS]) + joint_valid = np.asarray(hand[HandInputIndex.JOINT_VALID]) + + # Bail cleanly if the thumb tip itself is not valid -- without it we + # have nothing meaningful to measure against. + if not bool(joint_valid[HandJointIndex.THUMB_TIP]): + outputs[self.OUTPUT_POWERS][0] = powers + return + + thumb_tip = joint_positions[HandJointIndex.THUMB_TIP] + denom = self._max_distance_m - self._min_distance_m + + max_finger_power = 0.0 + for finger_idx, tip_joint in _FINGER_TIP_JOINTS.items(): + if not bool(joint_valid[tip_joint]): + # Skip invalid joints; the corresponding power stays at zero. + continue + distance = float(np.linalg.norm(joint_positions[tip_joint] - thumb_tip)) + # Map distance to [0, 1]: 0 at >= max_distance, 1 at <= min_distance. + ratio = (self._max_distance_m - distance) / denom + ratio = max(0.0, min(1.0, ratio)) + # Apply falloff (exponent < 1 makes the response feel stronger near + # the threshold; > 1 makes only very-close pinches feel anything). + shaped = ratio**self._falloff_exponent + power = min(self._saturation, max(0.0, shaped)) + powers[finger_idx] = power + if power > max_finger_power: + max_finger_power = power + + # Both sides of the pinch buzz: thumb mirrors the strongest finger + # signal so the user feels the pinch on both pads. + powers[FingerIndex.THUMB] = max_finger_power + outputs[self.OUTPUT_POWERS][0] = powers + + +class FingerPowerToControllerPulse(BaseRetargeter): + """Collapse a :func:`FingerPowerVector` to one :func:`ControllerHapticPulse`. + + Single-channel adapter for users routing pinch feedback to an OpenXR + motion controller (one motor per hand). Takes the max across fingers -- + matches "the most-pinched finger drives the rumble" intuition. + + Pure shim, no tunables -- frequency / duration come from the constructor + so the controller-side pulse parameters are still configurable but the + amplitude is unmodified from the FingerPowerVector input. + """ + + INPUT_POWERS = "powers" + OUTPUT_PULSE = "pulse" + + def __init__( + self, + name: str, + frequency_hz: float = 0.0, + duration_s: float = 0.0, + num_fingers: int = NUM_HAPTIC_FINGERS, + ) -> None: + self._frequency_hz = float(frequency_hz) + self._duration_s = float(duration_s) + self._num_fingers = int(num_fingers) + super().__init__(name=name) + + 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: ComputeContext + ) -> None: + powers = np.asarray(inputs[self.INPUT_POWERS][0], dtype=np.float32) + amplitude = float(powers.max()) if powers.size else 0.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 + + +# ============================================================================ +# Pipeline builders +# ============================================================================ + + +def _build_manus_pipeline( + hands: HandsSource, + *, + max_distance_m: float, + min_distance_m: float, + saturation: float, + falloff_exponent: float, +): + """Pinch-proximity -> per-finger powers -> Manus glove vibration. + + Returns: + Tuple of (OutputCombiner, monitoring) where monitoring is a dict of + ``OutputSelector``s keyed ``"powers_"`` (FingerPowerVector) so + the main loop can read per-finger amplitudes for display. + """ + sink_inputs: dict[str, Any] = {} + monitoring: dict[str, Any] = {} + + for side, hands_output in ( + ("left", HandsSource.LEFT), + ("right", HandsSource.RIGHT), + ): + mapper = PinchProximityToFingerPower( + f"{side}_pinch_to_finger_power", + max_distance_m=max_distance_m, + min_distance_m=min_distance_m, + saturation=saturation, + falloff_exponent=falloff_exponent, + ) + mapper_graph = mapper.connect( + {PinchProximityToFingerPower.INPUT_HAND: hands.output(hands_output)} + ) + powers_sel = mapper_graph.output(PinchProximityToFingerPower.OUTPUT_POWERS) + sink_inputs[HapticSink.LEFT if side == "left" else HapticSink.RIGHT] = ( + powers_sel + ) + monitoring[f"powers_{side}"] = powers_sel + + device = ManusHapticDevice() + sink = HapticSink("haptic_sink_manus", device) + sink_graph = sink.connect(sink_inputs) + + pipeline = OutputCombiner( + {HapticSink.HEARTBEAT: sink_graph.output(HapticSink.HEARTBEAT), **monitoring} + ) + return pipeline, monitoring + + +def _build_openxr_controller_pipeline( + hands: HandsSource, + controllers: ControllersSource, + *, + max_distance_m: float, + min_distance_m: float, + saturation: float, + falloff_exponent: float, + frequency_hz: float, + duration_s: float, +): + """Pinch-proximity -> max-across-fingers -> OpenXR controller rumble. + + Reuses ``controllers``' :class:`~isaacteleop.deviceio_trackers.ControllerTracker` + instance for the haptic source so :class:`~isaacteleop.deviceio.DeviceIOSession` + creates only one ``LiveControllerTrackerImpl``. + + Returns: + Tuple of (OutputCombiner, monitoring) where monitoring exposes + ``"powers_"`` (FingerPowerVector) and ``"haptic_"`` + (ControllerHapticPulse) per side for display. + """ + sink_inputs: dict[str, Any] = {} + monitoring: dict[str, Any] = {} + + for side, hands_output in ( + ("left", HandsSource.LEFT), + ("right", HandsSource.RIGHT), + ): + mapper = PinchProximityToFingerPower( + f"{side}_pinch_to_finger_power", + max_distance_m=max_distance_m, + min_distance_m=min_distance_m, + saturation=saturation, + falloff_exponent=falloff_exponent, + ) + mapper_graph = mapper.connect( + {PinchProximityToFingerPower.INPUT_HAND: hands.output(hands_output)} + ) + monitoring[f"powers_{side}"] = mapper_graph.output( + PinchProximityToFingerPower.OUTPUT_POWERS + ) + + collapser = FingerPowerToControllerPulse( + f"{side}_finger_power_to_pulse", + frequency_hz=frequency_hz, + duration_s=duration_s, + ) + collapser_graph = collapser.connect( + { + FingerPowerToControllerPulse.INPUT_POWERS: mapper_graph.output( + PinchProximityToFingerPower.OUTPUT_POWERS + ), + } + ) + pulse_sel = collapser_graph.output(FingerPowerToControllerPulse.OUTPUT_PULSE) + sink_inputs[HapticSink.LEFT if side == "left" else HapticSink.RIGHT] = pulse_sel + monitoring[f"haptic_{side}"] = pulse_sel + + device = OpenXRControllerHapticDevice(sides=("left", "right")) + sink = HapticSink("haptic_sink_openxr", device) + sink_graph = sink.connect(sink_inputs) + + haptic_source = OpenXRControllerHapticSource.for_controllers_source( + "_openxr_haptic_source", device, controllers + ) + + pipeline = OutputCombiner( + { + HapticSink.HEARTBEAT: sink_graph.output(HapticSink.HEARTBEAT), + OpenXRControllerHapticSource.HEARTBEAT: haptic_source.output( + OpenXRControllerHapticSource.HEARTBEAT + ), + **monitoring, + } + ) + return pipeline, monitoring + + +# ============================================================================ +# Display helpers +# ============================================================================ + +_BAR_WIDTH = 6 +_BAR_FILL = "█" +_BAR_EMPTY = "░" + +# Short names for each finger column (Thumb first, Manus order). +_FINGER_LABELS = ["Th", "Ix", "Md", "Rg", "Pk"] + + +def _bar(value: float, width: int = _BAR_WIDTH) -> str: + """Fixed-width ASCII progress bar for a value in [0, 1].""" + filled = round(max(0.0, min(1.0, value)) * width) + return _BAR_FILL * filled + _BAR_EMPTY * (width - filled) + + +def _read_powers(result: dict, key: str) -> list[float]: + """Extract all 5 finger powers from a FingerPowerVector group.""" + group = result.get(key) + if group is None or group.is_none: + return [0.0] * NUM_HAPTIC_FINGERS + arr = np.asarray(group[0], dtype=np.float32).ravel() + return [float(arr[i]) if i < len(arr) else 0.0 for i in range(NUM_HAPTIC_FINGERS)] + + +def _read_haptic_amplitude(result: dict, key: str) -> float: + """Extract the amplitude from a ControllerHapticPulse group.""" + group = result.get(key) + if group is None or group.is_none: + return 0.0 + return float(np.asarray(group[0])[ControllerHapticPulseField.AMPLITUDE]) + + +def _render_finger_row(label: str, powers: list[float]) -> str: + """Render one hand's per-finger bars as a compact inline string. + + Example: ``L Th ████░░ 0.65 Ix ░░░░░░ 0.00 Md ░░░░░░ 0.00 …`` + """ + cols = " ".join( + f"{_FINGER_LABELS[i]} {_bar(powers[i])} {powers[i]:.2f}" + for i in range(NUM_HAPTIC_FINGERS) + ) + return f"{label} {cols}" + + +# ============================================================================ +# CLI +# ============================================================================ + + +def _positive_float(value: str) -> float: + try: + n = float(value) + except ValueError: + raise argparse.ArgumentTypeError(f"expected a positive float, got {value!r}") + if n <= 0.0: + raise argparse.ArgumentTypeError(f"must be > 0, got {n}") + return n + + +def _non_negative_float(value: str) -> float: + try: + n = float(value) + except ValueError: + raise argparse.ArgumentTypeError(f"expected a number, got {value!r}") + if n < 0.0: + raise argparse.ArgumentTypeError(f"must be >= 0, got {n}") + return n + + +def _unit_float(value: str) -> float: + n = _non_negative_float(value) + if n > 1.0: + raise argparse.ArgumentTypeError(f"must be in [0, 1], got {n}") + return n + + +def main() -> int: + parser = argparse.ArgumentParser( + description="Hand-tracking pinch-proximity haptic feedback demo.", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument( + "--device", + choices=("manus", "openxr_controller"), + default="manus", + help=( + "manus: per-finger Manus glove vibration (canonical use case). " + "openxr_controller: collapse to single-channel motor rumble per hand " + "(useful as a hardware smoke test without Manus)." + ), + ) + parser.add_argument( + "--max-distance-m", + type=_positive_float, + default=0.10, + help="Finger->thumb distance at which vibration starts (zero amplitude). Meters.", + ) + parser.add_argument( + "--min-distance-m", + type=_non_negative_float, + default=0.005, + help="Finger->thumb distance at which vibration reaches the saturation amplitude. Meters.", + ) + parser.add_argument( + "--saturation", + type=_unit_float, + default=1.0, + help="Maximum per-finger vibration amplitude in [0, 1].", + ) + parser.add_argument( + "--falloff-exponent", + type=_positive_float, + default=1.0, + help=( + "Shape of the distance->amplitude ramp. 1.0 = linear. <1 makes the response " + "feel stronger as soon as you cross --max-distance-m; >1 makes only very " + "close finger/thumb proximity register." + ), + ) + parser.add_argument( + "--frequency-hz", + type=_non_negative_float, + default=0.0, + help=( + "OpenXR-only: pulse frequency. 0 -> XR_FREQUENCY_UNSPECIFIED. " + "Ignored when --device manus (Manus has no frequency knob)." + ), + ) + parser.add_argument( + "--duration-s", + type=_non_negative_float, + default=0.0, + help=( + "OpenXR-only: pulse duration per frame. 0 -> XR_MIN_HAPTIC_DURATION. " + "Ignored when --device manus." + ), + ) + parser.add_argument( + "--app-name", + default="HandPinchHapticExample", + help="App name for the TeleopSession (used for OpenXR action-set naming).", + ) + parser.add_argument( + "--fps", + type=_positive_float, + default=60.0, + help="Demo loop target frame rate. The pipeline runs once per frame.", + ) + parser.add_argument( + "--quiet", + action="store_true", + help=( + "Suppress per-second status output. Useful on terminals that do " + "not interpret ANSI cursor-move escapes (e.g. legacy cmd.exe; " + "modern Windows Terminal handles them correctly)." + ), + ) + args = parser.parse_args() + + if args.min_distance_m >= args.max_distance_m: + parser.error( + f"--min-distance-m ({args.min_distance_m}) must be < --max-distance-m ({args.max_distance_m})" + ) + + hands = HandsSource("hands") + + if args.device == "manus": + pipeline, monitoring = _build_manus_pipeline( + hands, + max_distance_m=args.max_distance_m, + min_distance_m=args.min_distance_m, + saturation=args.saturation, + falloff_exponent=args.falloff_exponent, + ) + else: + # OpenXR mode reuses ControllersSource's tracker for the haptic source + # (see _build_openxr_controller_pipeline for the rationale). + controllers = ControllersSource("controllers") + pipeline, monitoring = _build_openxr_controller_pipeline( + hands, + controllers, + max_distance_m=args.max_distance_m, + min_distance_m=args.min_distance_m, + saturation=args.saturation, + falloff_exponent=args.falloff_exponent, + frequency_hz=args.frequency_hz, + duration_s=args.duration_s, + ) + + config = TeleopSessionConfig(app_name=args.app_name, pipeline=pipeline) + + print("=" * 80) + print("Hand-pinch Haptic Feedback Demo") + print("=" * 80) + print(f"Device : {args.device}") + print( + f"Distance ramp : {args.max_distance_m * 100:.1f} cm (start) -> " + f"{args.min_distance_m * 100:.2f} cm (full) " + f"[exponent {args.falloff_exponent:.2f}, max amp {args.saturation:.2f}]" + ) + if args.device == "openxr_controller": + print( + f"OpenXR pulse : freq={args.frequency_hz:.1f} Hz " + f"(0=runtime default), duration={args.duration_s:.3f} s (0=min)" + ) + print() + print("Columns: Th=Thumb Ix=Index Md=Middle Rg=Ring Pk=Pinky") + print("Pinch any finger toward the thumb -- bars show vibration per finger.") + if args.device == "openxr_controller": + print("OXR: per-side rumble amplitude = max across all five fingers.") + print("Press Ctrl+C to exit.") + print() + + # Reserve two lines for the live per-hand display (one per hand) so the + # header above stays visible while values scroll in-place. + print() # L hand placeholder + print() # R hand placeholder (or OXR haptic line) + + frame_period_s = 1.0 / args.fps + + with TeleopSession(config) as session: + while True: + result = session.step() + + if not args.quiet: + powers_l = _read_powers(result, "powers_left") + powers_r = _read_powers(result, "powers_right") + + row_l = _render_finger_row("L", powers_l) + row_r = _render_finger_row("R", powers_r) + + if args.device == "openxr_controller": + amp_l = _read_haptic_amplitude(result, "haptic_left") + amp_r = _read_haptic_amplitude(result, "haptic_right") + oxr_line = ( + f" OXR L rumble {_bar(amp_l, 8)} {amp_l:.2f} " + f"R rumble {_bar(amp_r, 8)} {amp_r:.2f} " + f"[frame {session.frame_count}]" + ) + # Move up 3 lines, overwrite, move back down. + print( + f"\x1b[3A\r{row_l:<90}\n\r{row_r:<90}\n\r{oxr_line:<90}", + end="", + flush=True, + ) + else: + frame_tag = f" [frame {session.frame_count}]" + # Move up 2 lines and overwrite. + print( + f"\x1b[2A\r{row_l:<90}\n\r{row_r + frame_tag:<90}", + end="", + flush=True, + ) + + time.sleep(frame_period_s) + + return 0 + + +if __name__ == "__main__": + try: + sys.exit(main()) + except KeyboardInterrupt: + # Newline after the in-place overwrite lines so the prompt is clean. + print("\n\nExiting.") + sys.exit(0) diff --git a/examples/haptic_feedback/python/openxr_controller_haptic_example.py b/examples/haptic_feedback/python/openxr_controller_haptic_example.py new file mode 100644 index 000000000..42133a083 --- /dev/null +++ b/examples/haptic_feedback/python/openxr_controller_haptic_example.py @@ -0,0 +1,516 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""End-to-end haptic feedback demo for OpenXR motion controllers. + +Builds a :class:`~isaacteleop.teleop_session_manager.TeleopSession` that drives +the haptic actuator on each controller through the full retargeting stack +introduced for tactile feedback in Isaac Teleop: + +:: + + ControllersSource (input) OpenXRControllerHapticDevice + | ^ ^ + v | | + TriggerToTactile -> TactileVectorToControllerPulse -> HapticSink + | + v + OpenXRControllerHapticSource + (poll_tracker(session)) + +Default behaviour ("trigger" mode) closes a loop on the controllers +themselves: pulling the *left* trigger rumbles the *right* controller, and +vice versa. ``--mode sine`` ignores controller input and applies a smooth +0->1->0 sine envelope to both controllers so the haptic path can be +verified before any tactile data exists on the rig. + +This example deliberately uses the **same** :class:`~isaacteleop.deviceio_trackers.ControllerTracker` +instance for both the input side +(:class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.ControllersSource`) +and the haptic output side +(:class:`~isaacteleop.haptic_devices.openxr_controller.OpenXRControllerHapticSource`) +so the session only creates one ``LiveControllerTrackerImpl`` and there is +no contention on the underlying OpenXR action set. +""" + +from __future__ import annotations + +import argparse +import math +import sys +import time + +import numpy as np + +from isaacteleop.haptic_devices.openxr_controller import ( + OpenXRControllerHapticDevice, + OpenXRControllerHapticSource, +) +from isaacteleop.retargeters.tactile_retargeters import TactileVectorToControllerPulse +from isaacteleop.retargeting_engine.deviceio_source_nodes import ( + ControllersSource, + 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 ( + ControllerHapticPulseField, + ControllerInput, + ControllerInputIndex, + TactileVector, +) +from isaacteleop.teleop_session_manager import TeleopSession, TeleopSessionConfig + + +# ============================================================================ +# Demo retargeters +# ============================================================================ + + +class TriggerToTactile(BaseRetargeter): + """Demo retargeter: stream the controller trigger value as a TactileVector(1). + + Mirrors the shape of an :class:`isaaclab.sensors.ContactSensor`-driven + fetch function (one scalar in [0, 1]) so the rest of the pipeline does + not know whether the signal came from a sim contact or a real human + finger on a controller trigger. + """ + + INPUT_CONTROLLER = "controller" + OUTPUT_TACTILE = "tactile" + + def input_spec(self) -> RetargeterIOType: + return {self.INPUT_CONTROLLER: OptionalType(ControllerInput())} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_TACTILE: TactileVector(1)} + + def _compute_fn( + self, inputs: RetargeterIO, outputs: RetargeterIO, context: ComputeContext + ) -> None: + controller = inputs[self.INPUT_CONTROLLER] + if controller.is_none: + outputs[self.OUTPUT_TACTILE][0] = np.zeros(1, dtype=np.float32) + return + trigger = float(controller[ControllerInputIndex.TRIGGER_VALUE]) + outputs[self.OUTPUT_TACTILE][0] = np.array([trigger], dtype=np.float32) + + +class SineWaveTactile(BaseRetargeter): + """Demo retargeter: emit a TactileVector(1) following a half-rectified sine. + + Useful as a hardware smoke test -- it pulses haptics without requiring + any human input on the controllers. ``period_s`` and ``peak`` are both + construction-time scalars (no ParameterState needed; this is a demo + knob, not a tuning knob). + """ + + OUTPUT_TACTILE = "tactile" + + def __init__(self, name: str, period_s: float = 2.0, peak: float = 1.0) -> None: + if period_s <= 0.0: + raise ValueError("SineWaveTactile period_s must be > 0") + self._period_s = float(period_s) + self._peak = float(peak) + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return {} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_TACTILE: TactileVector(1)} + + def _compute_fn( + self, inputs: RetargeterIO, outputs: RetargeterIO, context: ComputeContext + ) -> None: + # Half-rectified sine: never negative (a negative haptic amplitude + # would be silently clamped downstream but it is clearer to do it here). + t = context.graph_time.real_time_ns / 1.0e9 + phase = (2.0 * math.pi * t) / self._period_s + amplitude = self._peak * max(0.0, math.sin(phase)) + outputs[self.OUTPUT_TACTILE][0] = np.array([amplitude], dtype=np.float32) + + +# ============================================================================ +# Pipeline builders for each demo mode +# ============================================================================ + + +def _build_trigger_pipeline( + controllers: ControllersSource, + sink: HapticSink, + cross_hand: bool, + *, + saturation: float, + frequency_hz: float, + duration_s: float, +): + """Wire the cross-hand (or same-hand) trigger -> rumble loop. + + Each side gets its own :class:`TriggerToTactile` + per-pulse mapper; the + sink consumes one ``ControllerHapticPulse`` per side. + + Returns: + Tuple of (sink_graph, monitoring) where monitoring is a dict of + ``OutputSelector``s keyed ``"trigger_"`` and ``"haptic_"`` + so the main ``OutputCombiner`` can expose them for printing. + """ + sides = ("left", "right") + trigger_selectors: dict[str, object] = {} + pulse_selectors: dict[str, object] = {} + + for side in sides: + trigger = TriggerToTactile(f"{side}_trigger") + trigger_graph = trigger.connect( + { + TriggerToTactile.INPUT_CONTROLLER: controllers.output( + ControllersSource.LEFT + if side == "left" + else ControllersSource.RIGHT + ), + } + ) + trigger_selectors[side] = trigger_graph.output(TriggerToTactile.OUTPUT_TACTILE) + + mapper = TactileVectorToControllerPulse( + f"{side}_trigger_to_pulse", + num_taxels=1, + saturation=saturation, + frequency_hz=frequency_hz, + duration_s=duration_s, + ) + mapper_graph = mapper.connect( + { + TactileVectorToControllerPulse.INPUT_TACTILE: trigger_graph.output( + TriggerToTactile.OUTPUT_TACTILE + ), + } + ) + pulse_selectors[side] = mapper_graph.output( + TactileVectorToControllerPulse.OUTPUT_PULSE + ) + + if cross_hand: + # Left trigger -> right controller rumble (and vice versa). Closes a + # nice "feel what your other hand is doing" loop on a single user. + sink_inputs = { + HapticSink.LEFT: pulse_selectors["right"], + HapticSink.RIGHT: pulse_selectors["left"], + } + else: + sink_inputs = { + HapticSink.LEFT: pulse_selectors["left"], + HapticSink.RIGHT: pulse_selectors["right"], + } + + monitoring = { + "trigger_left": trigger_selectors["left"], + "trigger_right": trigger_selectors["right"], + "haptic_left": pulse_selectors["left"], + "haptic_right": pulse_selectors["right"], + } + return sink.connect(sink_inputs), monitoring + + +def _build_sine_pipeline( + sink: HapticSink, + *, + period_s: float, + saturation: float, + frequency_hz: float, + duration_s: float, +): + """Wire a synchronized sine envelope to both sides (no controller input). + + Returns: + Tuple of (sink_graph, monitoring) where monitoring exposes the raw + sine amplitude under ``"sine_amplitude"`` for printing. + """ + tactile = SineWaveTactile("sine_tactile", period_s=period_s, peak=1.0) + mapper = TactileVectorToControllerPulse( + "sine_to_pulse", + num_taxels=1, + saturation=saturation, + frequency_hz=frequency_hz, + duration_s=duration_s, + ) + tactile_selector = tactile.output(SineWaveTactile.OUTPUT_TACTILE) + mapper_graph = mapper.connect( + {TactileVectorToControllerPulse.INPUT_TACTILE: tactile_selector} + ) + pulse_selector = mapper_graph.output(TactileVectorToControllerPulse.OUTPUT_PULSE) + monitoring = { + "sine_amplitude": tactile_selector, + "haptic_left": pulse_selector, + "haptic_right": pulse_selector, + } + return sink.connect( + {HapticSink.LEFT: pulse_selector, HapticSink.RIGHT: pulse_selector} + ), monitoring + + +# ============================================================================ +# Display helpers +# ============================================================================ + +_BAR_WIDTH = 10 +_BAR_FILL = "█" +_BAR_EMPTY = "░" + + +def _bar(value: float, width: int = _BAR_WIDTH) -> str: + """Render a fixed-width ASCII progress bar for a value in [0, 1].""" + filled = round(max(0.0, min(1.0, value)) * width) + return _BAR_FILL * filled + _BAR_EMPTY * (width - filled) + + +def _read_tactile(result: dict, key: str) -> float: + """Extract the first scalar from a TactileVector(1) group in the result dict.""" + group = result.get(key) + if group is None or group.is_none: + return 0.0 + return float(np.asarray(group[0]).ravel()[0]) + + +def _read_haptic_amplitude(result: dict, key: str) -> float: + """Extract the amplitude from a ControllerHapticPulse group in the result dict.""" + group = result.get(key) + if group is None or group.is_none: + return 0.0 + return float(np.asarray(group[0])[ControllerHapticPulseField.AMPLITUDE]) + + +# ============================================================================ +# CLI +# ============================================================================ + + +def _positive_float(value: str) -> float: + try: + n = float(value) + except ValueError: + raise argparse.ArgumentTypeError(f"expected a positive float, got {value!r}") + if n <= 0.0: + raise argparse.ArgumentTypeError(f"must be > 0, got {n}") + return n + + +def _non_negative_float(value: str) -> float: + try: + n = float(value) + except ValueError: + raise argparse.ArgumentTypeError(f"expected a number, got {value!r}") + if n < 0.0: + raise argparse.ArgumentTypeError(f"must be >= 0, got {n}") + return n + + +def _unit_float(value: str) -> float: + n = _non_negative_float(value) + if n > 1.0: + raise argparse.ArgumentTypeError(f"must be in [0, 1], got {n}") + return n + + +def main() -> int: + parser = argparse.ArgumentParser( + description="OpenXR controller haptic feedback demo.", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument( + "--mode", + choices=("trigger", "sine"), + default="trigger", + help=( + "trigger: pull a trigger to rumble a controller (closed-loop on the user). " + "sine: emit a smooth sine envelope on both controllers (no input needed)." + ), + ) + parser.add_argument( + "--same-hand", + action="store_true", + help="trigger mode only: rumble the same-side controller instead of the cross-hand controller.", + ) + parser.add_argument( + "--sine-period", + type=_positive_float, + default=2.0, + help="sine mode only: full-cycle period of the rumble envelope, in seconds.", + ) + parser.add_argument( + "--saturation", + type=_unit_float, + default=1.0, + help="Upper clamp on the haptic pulse amplitude in [0, 1].", + ) + parser.add_argument( + "--frequency-hz", + type=_non_negative_float, + default=0.0, + help="OpenXR pulse frequency. 0 -> XR_FREQUENCY_UNSPECIFIED (runtime picks).", + ) + parser.add_argument( + "--duration-s", + type=_non_negative_float, + default=0.0, + help="OpenXR pulse duration per frame. 0 -> XR_MIN_HAPTIC_DURATION (shortest the runtime supports).", + ) + parser.add_argument( + "--app-name", + default="OpenXRHapticFeedbackExample", + help="App name for the TeleopSession (used for OpenXR action-set naming).", + ) + parser.add_argument( + "--fps", + type=_positive_float, + default=60.0, + help="Demo loop target frame rate. The pipeline runs once per frame.", + ) + args = parser.parse_args() + + # -------------------------------------------------------------- + # Sources & device adapter + # -------------------------------------------------------------- + controllers = ControllersSource("controllers") + device = OpenXRControllerHapticDevice(sides=("left", "right")) + sink = HapticSink("haptic_sink", device) + + # Reuse the controller tracker from ControllersSource so DeviceIOSession + # only creates one LiveControllerTrackerImpl (the impl owns the OpenXR + # action set, and two impls would race on the same actions). The + # `for_controllers_source` helper makes the sharing explicit so the two + # sources cannot accidentally diverge on the tracker handle. + haptic_source = OpenXRControllerHapticSource.for_controllers_source( + "_openxr_haptic_source", device, controllers + ) + + # -------------------------------------------------------------- + # Per-mode wiring + # -------------------------------------------------------------- + if args.mode == "trigger": + sink_graph, monitoring = _build_trigger_pipeline( + controllers, + sink, + cross_hand=not args.same_hand, + saturation=args.saturation, + frequency_hz=args.frequency_hz, + duration_s=args.duration_s, + ) + else: + sink_graph, monitoring = _build_sine_pipeline( + sink, + period_s=args.sine_period, + saturation=args.saturation, + frequency_hz=args.frequency_hz, + duration_s=args.duration_s, + ) + + # OutputCombiner only walks back from declared outputs; the heartbeats on + # the sink and the haptic source make both reachable so the session + # auto-discovers them as leaves and drains the queue every frame. + # Monitoring outputs are also included so the main loop can read them back + # from session.step() without any extra retargeters. + pipeline = OutputCombiner( + { + HapticSink.HEARTBEAT: sink_graph.output(HapticSink.HEARTBEAT), + OpenXRControllerHapticSource.HEARTBEAT: haptic_source.output( + OpenXRControllerHapticSource.HEARTBEAT + ), + **monitoring, + } + ) + + # -------------------------------------------------------------- + # Run + # -------------------------------------------------------------- + config = TeleopSessionConfig(app_name=args.app_name, pipeline=pipeline) + + print("=" * 80) + print("OpenXR Controller Haptic Feedback Demo") + print("=" * 80) + if args.mode == "trigger": + if args.same_hand: + print( + "Mode: trigger (same-hand) -- each trigger rumbles its own controller." + ) + else: + print( + "Mode: trigger (cross-hand) -- LEFT trigger rumbles RIGHT controller, " + "and vice versa." + ) + else: + print( + f"Mode: sine -- both controllers rumble on a {args.sine_period:.2f}s sine envelope." + ) + print( + f"Saturation={args.saturation:.2f}, frequency_hz={args.frequency_hz:.2f} " + f"(0=runtime default), duration_s={args.duration_s:.3f} (0=min)" + ) + print("Press Ctrl+C to exit.") + print() + + frame_period_s = 1.0 / args.fps + cross_hand = args.mode == "trigger" and not args.same_hand + + with TeleopSession(config) as session: + while True: + result = session.step() + + # Print every frame so values scroll in real time. The terminal + # cursor overwrites the previous line using \r so the output stays + # in place rather than scrolling. Use --fps to control how fast + # values update. + if args.mode == "trigger": + trig_l = _read_tactile(result, "trigger_left") + trig_r = _read_tactile(result, "trigger_right") + # In cross-hand mode the left trigger drives the right haptic + # and vice versa; label accordingly so the arrow makes sense. + hap_l = _read_haptic_amplitude(result, "haptic_left") + hap_r = _read_haptic_amplitude(result, "haptic_right") + if cross_hand: + line = ( + f"L trig {_bar(trig_l)} {trig_l:.2f} -> R haptic {_bar(hap_r)} {hap_r:.2f} | " + f"R trig {_bar(trig_r)} {trig_r:.2f} -> L haptic {_bar(hap_l)} {hap_l:.2f} " + f"[frame {session.frame_count}]" + ) + else: + line = ( + f"L trig {_bar(trig_l)} {trig_l:.2f} -> L haptic {_bar(hap_l)} {hap_l:.2f} | " + f"R trig {_bar(trig_r)} {trig_r:.2f} -> R haptic {_bar(hap_r)} {hap_r:.2f} " + f"[frame {session.frame_count}]" + ) + else: + amp = _read_tactile(result, "sine_amplitude") + hap_l = _read_haptic_amplitude(result, "haptic_left") + hap_r = _read_haptic_amplitude(result, "haptic_right") + line = ( + f"sine {_bar(amp)} {amp:.2f} -> " + f"L haptic {_bar(hap_l)} {hap_l:.2f} " + f"R haptic {_bar(hap_r)} {hap_r:.2f} " + f"[frame {session.frame_count}]" + ) + + # Pad to terminal width so partial overwrites don't leave stale chars. + print(f"\r{line:<100}", end="", flush=True) + + time.sleep(frame_period_s) + + return 0 + + +if __name__ == "__main__": + try: + sys.exit(main()) + except KeyboardInterrupt: + # Newline after the \r-overwritten status line so the shell prompt + # appears on a clean line. + print("\nExiting.") + sys.exit(0) diff --git a/examples/haptic_feedback/python/pyproject.toml b/examples/haptic_feedback/python/pyproject.toml new file mode 100644 index 000000000..8a8289dd6 --- /dev/null +++ b/examples/haptic_feedback/python/pyproject.toml @@ -0,0 +1,12 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +[project] +name = "isaacteleop-haptic-feedback-examples" +version = "0.0.0" # Internal examples - not versioned +description = "Isaac Teleop haptic feedback CLI examples (OpenXR controller)." +requires-python = ">=3.10,<3.14" +dependencies = [ + "isaacteleop", + "numpy>=1.23.0", +] diff --git a/src/core/AGENTS.md b/src/core/AGENTS.md index 5a07ff6e6..525e02edb 100644 --- a/src/core/AGENTS.md +++ b/src/core/AGENTS.md @@ -18,3 +18,4 @@ If work under **`src/core/`** went wrong—**user** correction, **pre-commit/CI* - When changing `TeleopSession` retargeting execution defaults, update config, docs, and default-behavior tests together so opt-in vs. default semantics stay aligned. - Preserve existing `TeleopSession` lifecycle flag semantics unless changing the public/context-manager contract intentionally; use tests to lock down cleanup details before altering them. - After Python test or session-manager edits, let `ruff format`/pre-commit own wrapping and rerun the hook when it modifies files. +- **Sink/source `IDeviceIOSource` leaves are only discovered when reachable from a declared `OutputCombiner` output.** `TeleopSession._discover_sources` calls `pipeline.get_leaf_nodes()`, which walks back from the combiner's outputs. A sink/source whose only purpose is a side effect (haptic output, message-channel send) must therefore expose at least one output (a heartbeat boolean is the established pattern) **and** the user's combiner must include it. Every new sink/source of this shape needs both an explicit heartbeat output and a docstring warning that custom combiners must wire it up — silent no-discovery is the recurring footgun. diff --git a/src/core/deviceio_base/cpp/inc/deviceio_base/controller_tracker_base.hpp b/src/core/deviceio_base/cpp/inc/deviceio_base/controller_tracker_base.hpp index 44d2fbf59..008469aaf 100644 --- a/src/core/deviceio_base/cpp/inc/deviceio_base/controller_tracker_base.hpp +++ b/src/core/deviceio_base/cpp/inc/deviceio_base/controller_tracker_base.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 @@ -16,6 +16,24 @@ class IControllerTrackerImpl : public ITrackerImpl public: virtual const ControllerSnapshotTrackedT& get_left_controller() const = 0; virtual const ControllerSnapshotTrackedT& get_right_controller() const = 0; + + /// Apply one frame of haptic vibration to the controller on the given side. + /// + /// `amplitude` in [0, 1]. `amplitude == 0` requests an explicit "stop" + /// rather than a zero-amplitude pulse, so a rumble can be aborted cleanly + /// when the upstream tactile signal drops below the deadband. + /// + /// `frequency_hz == 0` selects the runtime's default frequency (e.g. + /// OpenXR's `XR_FREQUENCY_UNSPECIFIED`); `duration_s == 0` selects the + /// runtime's shortest supported pulse (e.g. OpenXR's + /// `XR_MIN_HAPTIC_DURATION`). Implementations must be non-throwing on + /// transient hardware failures -- a missing haptic component must not + /// tear down the tracker. + /// + /// Marked `const` to match the rest of the impl API (the impl object is + /// treated as immutable from the public interface; the side effect lives + /// in the runtime / hardware). + virtual void apply_haptic_feedback(bool is_left, float amplitude, float frequency_hz, float duration_s) const = 0; }; } // namespace core diff --git a/src/core/deviceio_trackers/cpp/controller_tracker.cpp b/src/core/deviceio_trackers/cpp/controller_tracker.cpp index 3976fbf4b..d3fc21a3f 100644 --- a/src/core/deviceio_trackers/cpp/controller_tracker.cpp +++ b/src/core/deviceio_trackers/cpp/controller_tracker.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/deviceio_trackers/controller_tracker.hpp" @@ -20,4 +20,14 @@ const ControllerSnapshotTrackedT& ControllerTracker::get_right_controller(const return static_cast(session.get_tracker_impl(*this)).get_right_controller(); } +void ControllerTracker::apply_haptic_feedback(const ITrackerSession& session, + bool is_left, + float amplitude, + float frequency_hz, + float duration_s) const +{ + static_cast(session.get_tracker_impl(*this)) + .apply_haptic_feedback(is_left, amplitude, frequency_hz, duration_s); +} + } // namespace core diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp index 263de3d61..8bc6fab67 100644 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.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 @@ -26,6 +26,25 @@ class ControllerTracker : public ITracker const ControllerSnapshotTrackedT& get_left_controller(const ITrackerSession& session) const; const ControllerSnapshotTrackedT& get_right_controller(const ITrackerSession& session) const; + /// Drive the controller's haptic actuator for one frame. + /// + /// Bridges Isaac Teleop's haptic feedback flow (see + /// isaacteleop.haptic_devices.OpenXRControllerHapticDevice) to the + /// underlying runtime impl. Vendor neutral at this layer; concrete impls + /// (e.g. the live OpenXR impl) translate to runtime-specific calls. + /// + /// `amplitude` is in [0, 1]; `amplitude == 0` requests an explicit stop + /// instead of a zero-amplitude pulse. `frequency_hz == 0` selects the + /// runtime's default frequency; `duration_s == 0` selects the runtime's + /// shortest supported pulse. See the base interface + /// (:class:`IControllerTrackerImpl::apply_haptic_feedback`) for the full + /// contract. + void apply_haptic_feedback(const ITrackerSession& session, + bool is_left, + float amplitude, + float frequency_hz, + float duration_s) const; + private: static constexpr const char* TRACKER_NAME = "ControllerTracker"; }; diff --git a/src/core/deviceio_trackers/python/tracker_bindings.cpp b/src/core/deviceio_trackers/python/tracker_bindings.cpp index 7b339d57f..332af54ea 100644 --- a/src/core/deviceio_trackers/python/tracker_bindings.cpp +++ b/src/core/deviceio_trackers/python/tracker_bindings.cpp @@ -62,7 +62,34 @@ PYBIND11_MODULE(_deviceio_trackers, m) "get_right_controller", [](const core::ControllerTracker& self, const core::ITrackerSession& session) -> core::ControllerSnapshotTrackedT { return self.get_right_controller(session); }, - py::arg("session"), "Get the right controller tracked state (data is None if inactive)"); + py::arg("session"), "Get the right controller tracked state (data is None if inactive)") + .def( + "apply_haptic_feedback", + [](const core::ControllerTracker& self, const core::ITrackerSession& session, const std::string& side, + float amplitude, float frequency_hz, float duration_s) + { + bool is_left; + if (side == "left") + { + is_left = true; + } + else if (side == "right") + { + is_left = false; + } + else + { + throw py::value_error("side must be \"left\" or \"right\", got \"" + side + "\""); + } + self.apply_haptic_feedback(session, is_left, amplitude, frequency_hz, duration_s); + }, + py::arg("session"), py::arg("side"), py::arg("amplitude"), py::arg("frequency_hz") = 0.0f, + py::arg("duration_s") = 0.0f, + "Apply one frame of OpenXR haptic vibration to the controller on the given side.\n" + "side: 'left' or 'right'.\n" + "amplitude: [0, 1]; 0 issues xrStopHapticFeedback instead of a zero-amplitude pulse.\n" + "frequency_hz: 0 = XR_FREQUENCY_UNSPECIFIED (runtime default).\n" + "duration_s: 0 = XR_MIN_HAPTIC_DURATION (shortest pulse the runtime supports)."); py::enum_(m, "MessageChannelStatus") .value("CONNECTING", core::MessageChannelStatus::CONNECTING) diff --git a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp index 9ef2d2e0c..8ef6d1694 100644 --- a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -254,6 +255,13 @@ LiveControllerTrackerImpl::LiveControllerTrackerImpl(const OpenXRSessionHandles& "trigger_value", "Trigger Value", XR_ACTION_TYPE_FLOAT_INPUT)), + haptic_action_(create_action(core_funcs_, + action_set_.get(), + left_hand_path_, + right_hand_path_, + "haptic_output", + "Haptic Output", + XR_ACTION_TYPE_VIBRATION_OUTPUT)), left_grip_space_(create_space(core_funcs_, session_, grip_pose_action_, left_hand_path_)), right_grip_space_(create_space(core_funcs_, session_, grip_pose_action_, right_hand_path_)), left_aim_space_(create_space(core_funcs_, session_, aim_pose_action_, left_hand_path_)), @@ -285,6 +293,10 @@ LiveControllerTrackerImpl::LiveControllerTrackerImpl(const OpenXRSessionHandles& // Oculus Touch exposes menu only on the left controller; right hand has no such path, // so we bind left only. Right-hand menu_click will report false (action inactive). add_binding(menu_click_action_, "/user/hand/left/input/menu/click"); + // Haptic output bindings — one per side onto the standard haptic component path + // every conformant motion-controller interaction profile exposes. + add_binding(haptic_action_, "/user/hand/left/output/haptic"); + add_binding(haptic_action_, "/user/hand/right/output/haptic"); XrInstanceActionContextInfoNV binding_ctx_info{ XR_TYPE_INSTANCE_ACTION_CONTEXT_INFO_NV }; binding_ctx_info.instanceActionContext = instance_action_context_.get(); @@ -448,4 +460,75 @@ const ControllerSnapshotTrackedT& LiveControllerTrackerImpl::get_right_controlle return right_tracked_; } +void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, + float amplitude, + float frequency_hz, + float duration_s) const +{ + // The haptic action was created against both subaction paths during init, + // so targeting one side is a matter of choosing the matching path here. + const XrPath subaction_path = is_left ? left_hand_path_ : right_hand_path_; + const size_t slot = is_left ? 0 : 1; + const char* const side_name = is_left ? "left" : "right"; + + XrHapticActionInfo info{ XR_TYPE_HAPTIC_ACTION_INFO }; + info.action = haptic_action_; + info.subactionPath = subaction_path; + + // amplitude==0 maps to an explicit stop instead of a zero-amplitude pulse, + // so dropping below the upstream deadband cleanly aborts an in-flight rumble. + if (amplitude <= 0.0f) + { + if (core_funcs_.xrStopHapticFeedback == nullptr) + { + return; + } + const XrResult stop_result = core_funcs_.xrStopHapticFeedback(session_, &info); + if (XR_FAILED(stop_result)) + { + // Don't tear the session down on transient failures, but log the + // first occurrence per side so chronic problems are not invisible. + bool expected = false; + if (stop_haptic_error_logged_[slot].compare_exchange_strong(expected, true)) + { + std::cerr << "[ControllerTracker] xrStopHapticFeedback(" << side_name << ") failed: " + << static_cast(stop_result) + << "; further errors for this side will be silenced." << std::endl; + } + } + return; + } + + if (core_funcs_.xrApplyHapticFeedback == nullptr) + { + // Runtime does not advertise the haptic entry point — silently no-op. + // This keeps haptic feedback a nice-to-have rather than a session-fatal + // dependency for runtimes that omit it (e.g. some headless test rigs). + return; + } + + XrHapticVibration vibration{ XR_TYPE_HAPTIC_VIBRATION }; + vibration.amplitude = std::clamp(amplitude, 0.0f, 1.0f); + // duration_s == 0 -> shortest pulse the runtime supports. Use double for + // the seconds->nanoseconds conversion: float has ~7 decimal digits and + // can lose tens of microseconds for multi-second pulses. + vibration.duration = + (duration_s <= 0.0f) ? XR_MIN_HAPTIC_DURATION : static_cast(static_cast(duration_s) * 1.0e9); + // frequency_hz == 0 -> let the runtime pick. + vibration.frequency = (frequency_hz <= 0.0f) ? XR_FREQUENCY_UNSPECIFIED : frequency_hz; + + const XrResult apply_result = core_funcs_.xrApplyHapticFeedback( + session_, &info, reinterpret_cast(&vibration)); + if (XR_FAILED(apply_result)) + { + bool expected = false; + if (apply_haptic_error_logged_[slot].compare_exchange_strong(expected, true)) + { + std::cerr << "[ControllerTracker] xrApplyHapticFeedback(" << side_name << ") failed: " + << static_cast(apply_result) + << "; further errors for this side will be silenced." << std::endl; + } + } +} + } // namespace core diff --git a/src/core/live_trackers/cpp/live_controller_tracker_impl.hpp b/src/core/live_trackers/cpp/live_controller_tracker_impl.hpp index ee79e4346..3fc6a4435 100644 --- a/src/core/live_trackers/cpp/live_controller_tracker_impl.hpp +++ b/src/core/live_trackers/cpp/live_controller_tracker_impl.hpp @@ -10,6 +10,8 @@ #include #include +#include +#include #include #include #include @@ -42,6 +44,7 @@ class LiveControllerTrackerImpl : public IControllerTrackerImpl void update(int64_t monotonic_time_ns) override; const ControllerSnapshotTrackedT& get_left_controller() const override; const ControllerSnapshotTrackedT& get_right_controller() const override; + void apply_haptic_feedback(bool is_left, float amplitude, float frequency_hz, float duration_s) const override; private: const OpenXRCoreFunctions core_funcs_; @@ -67,6 +70,10 @@ class LiveControllerTrackerImpl : public IControllerTrackerImpl XrAction menu_click_action_; XrAction squeeze_value_action_; XrAction trigger_value_action_; + // Output action that drives controller rumble. One action with two + // subaction paths (left + right) bound to /user/hand/{left,right}/output/haptic + // matches how the input actions above subaction onto the same paths. + XrAction haptic_action_; XrSpacePtr left_grip_space_; XrSpacePtr right_grip_space_; @@ -77,6 +84,14 @@ class LiveControllerTrackerImpl : public IControllerTrackerImpl ControllerSnapshotTrackedT right_tracked_; int64_t last_update_time_ = 0; + // Once-per-side log gates for OpenXR haptic call failures. Indexed by + // side: [0]=left, [1]=right. `mutable` is required because + // `apply_haptic_feedback` is `const` (the impl object is treated as + // immutable from the public interface; the side effect lives in the + // runtime), but we still want to log the first failure per side. + mutable std::array, 2> apply_haptic_error_logged_{ { false, false } }; + mutable std::array, 2> stop_haptic_error_logged_{ { false, false } }; + std::unique_ptr mcap_channels_; }; diff --git a/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp b/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp index 53940cede..655de6d03 100644 --- a/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp +++ b/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp @@ -58,6 +58,10 @@ struct OpenXRCoreFunctions PFN_xrGetActionStateVector2f xrGetActionStateVector2f; PFN_xrGetActionStatePose xrGetActionStatePose; + // Haptic output (optional, used by controller-tracker haptic feedback). + PFN_xrApplyHapticFeedback xrApplyHapticFeedback; + PFN_xrStopHapticFeedback xrStopHapticFeedback; + // Load all core functions from an instance using the provided xrGetInstanceProcAddr static OpenXRCoreFunctions load(XrInstance instance, PFN_xrGetInstanceProcAddr getProcAddr) { @@ -103,6 +107,13 @@ struct OpenXRCoreFunctions getProcAddr( instance, "xrGetActionStatePose", reinterpret_cast(&results.xrGetActionStatePose)); + // Haptic output (optional — tracker haptic methods check for null pointers + // and silently no-op when the runtime does not advertise them). + getProcAddr(instance, "xrApplyHapticFeedback", + reinterpret_cast(&results.xrApplyHapticFeedback)); + getProcAddr(instance, "xrStopHapticFeedback", + reinterpret_cast(&results.xrStopHapticFeedback)); + return results; } }; diff --git a/src/core/python/pyproject.toml.in b/src/core/python/pyproject.toml.in index b0fbe9ee0..f87db8f17 100644 --- a/src/core/python/pyproject.toml.in +++ b/src/core/python/pyproject.toml.in @@ -53,6 +53,7 @@ packages = [ "isaacteleop.retargeting_engine_ui", "isaacteleop.teleop_session_manager", "isaacteleop.cloudxr", + "isaacteleop.haptic_devices", @VIZ_PACKAGES_BLOCK@@ROBOTIC_GROUNDING_PACKAGES_BLOCK@] include-package-data = false @@ -66,6 +67,7 @@ isaacteleop = ["*.so", "*.pyd", "*.pyi", "py.typed"] "isaacteleop.plugin_manager" = ["*.so", "*.pyd", "*.pyi"] "isaacteleop.schema" = ["*.so", "*.pyd", "*.pyi"] "isaacteleop.cloudxr" = ["native/*.so", "native/*.so.*", "native/openxr_cloudxr.json"] +"isaacteleop.haptic_devices" = ["*.so", "*.pyd", "*.pyi"] @VIZ_PACKAGE_DATA_BLOCK@@ROBOTIC_GROUNDING_PACKAGE_DATA_BLOCK@ [tool.uv] diff --git a/src/core/replay_trackers/cpp/replay_controller_tracker_impl.hpp b/src/core/replay_trackers/cpp/replay_controller_tracker_impl.hpp index d088e56a9..eeb857b04 100644 --- a/src/core/replay_trackers/cpp/replay_controller_tracker_impl.hpp +++ b/src/core/replay_trackers/cpp/replay_controller_tracker_impl.hpp @@ -29,6 +29,13 @@ class ReplayControllerTrackerImpl : public IControllerTrackerImpl void update(int64_t monotonic_time_ns) override; const ControllerSnapshotTrackedT& get_left_controller() const override; const ControllerSnapshotTrackedT& get_right_controller() const override; + // Replay sessions do not drive hardware — haptic feedback is a no-op here. + void apply_haptic_feedback(bool /*is_left*/, + float /*amplitude*/, + float /*frequency_hz*/, + float /*duration_s*/) const override + { + } private: ControllerSnapshotTrackedT left_tracked_; diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py index 2fac2f0c1..b1d2d571f 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/__init__.py @@ -16,6 +16,7 @@ message_channel_config, messageChannelConfig, ) +from .haptic_sink import HapticSink from .deviceio_tensor_types import ( HeadPoseTrackedType, HandPoseTrackedType, @@ -47,6 +48,7 @@ "MessageChannelConfig", "message_channel_config", "messageChannelConfig", + "HapticSink", "HeadPoseTrackedType", "HandPoseTrackedType", "ControllerSnapshotTrackedType", diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py b/src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py new file mode 100644 index 000000000..51ff81746 --- /dev/null +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py @@ -0,0 +1,120 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Vendor-agnostic haptic sink retargeter. + +Hands one frame of device-side values per side to whatever +:class:`~isaacteleop.haptic_devices.IHapticDevice` adapter is plugged in. +``HapticSink`` itself contains no vendor logic; the adapter handles all +I/O. Type compatibility between the upstream retargeter's output and the +device's ``accepted_type()`` is checked at ``connect()`` time so wiring +mistakes fail before the first hardware call. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +import numpy as np + +from ..interface.base_retargeter import BaseRetargeter +from ..interface.retargeter_core_types import RetargeterIO, RetargeterIOType +from ..interface.tensor_group_type import OptionalType, TensorGroupType +from ..tensor_types.scalar_types import BoolType + + +if TYPE_CHECKING: + from isaacteleop.haptic_devices import IHapticDevice + + +class HapticSink(BaseRetargeter): + """Per-frame sink for haptic feedback through any :class:`IHapticDevice` adapter. + + Inputs: + - ``"left"`` / ``"right"``: optional ``device.accepted_type()`` payloads + (one frame of device-side values per side). Optional so a one-handed + rig can wire only the side it actually drives. + + Outputs: + - ``"_haptic_heartbeat"``: a single bool, always ``True``. Exposed only + so :class:`~isaacteleop.retargeting_engine.interface.output_combiner.OutputCombiner` + can include the sink in its graph traversal -- the combiner only + executes nodes that are reachable from a declared output. The + heartbeat is never consumed by downstream code and the + ``_`` prefix in its name signals "internal plumbing"; the helpers in + :mod:`isaaclab_teleop.tactile_helpers` wire it up automatically. + + .. warning:: + If you build a custom :class:`OutputCombiner` instead of going through + the ``isaaclab_teleop.tactile_helpers`` helpers, you **must** include + :attr:`HapticSink.HEARTBEAT` (or any other output reachable from this + node) as one of the combiner's outputs. A reachable output is the + only way ``OutputCombiner.get_leaf_nodes`` will discover the sink and + run its ``_compute_fn``; without it, ``device.apply()`` is never + called and haptics silently do not fire. + + The sink calls :meth:`IHapticDevice.apply` for each side whose input is + present *and* :meth:`IHapticDevice.supports` returns ``True`` -- this lets + single-handed devices (e.g. Haply Inverse3) cleanly no-op the unused side + without the upstream pipeline knowing. + + Modeled after :class:`MessageChannelSink`. Whether the adapter writes + to hardware synchronously (Manus, via the plugin singleton) or queues + for a paired :class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.interface.IDeviceIOSource` + to flush against the active session (OpenXR controllers) is the + adapter's concern -- ``HapticSink`` is identical in both cases. + """ + + LEFT = "left" + RIGHT = "right" + HEARTBEAT = "_haptic_heartbeat" + + def __init__(self, name: str, device: "IHapticDevice") -> None: + """Construct a haptic sink bound to ``device``. + + Args: + name: Unique pipeline node name. + device: Vendor-specific :class:`IHapticDevice` adapter. The + accepted type is captured from ``device.accepted_type()`` + at construction time and used in :meth:`input_spec`. + """ + self._device = device + super().__init__(name) + + @property + def device(self) -> "IHapticDevice": + """Return the adapter this sink writes to.""" + return self._device + + def input_spec(self) -> RetargeterIOType: + accepted = self._device.accepted_type() + return { + self.LEFT: OptionalType(accepted), + self.RIGHT: OptionalType(accepted), + } + + def output_spec(self) -> RetargeterIOType: + return {self.HEARTBEAT: TensorGroupType("_haptic_heartbeat", [BoolType("ok")])} + + def _compute_fn( + self, + inputs: RetargeterIO, + outputs: RetargeterIO, + context: Any, + ) -> None: + for side in (self.LEFT, self.RIGHT): + group = inputs[side] + if group.is_none: + continue + if not self._device.supports(side): + continue + # Pull tensor 0 (the only inner tensor of every device-side schema + # in v1: FingerPowerVector, ControllerHapticPulse, EndEffectorForce + # are all single-NDArray groups). Materialise as a CPU NumPy array + # since adapters write to host-side hardware APIs that do not + # speak DLPack directly. + values = np.asarray(group[0]) + self._device.apply(side, values) + # Heartbeat output exists purely so OutputCombiner can include this + # sink in its graph traversal; the value is never consumed. + outputs[self.HEARTBEAT][0] = True diff --git a/src/core/retargeting_engine/python/tensor_types/__init__.py b/src/core/retargeting_engine/python/tensor_types/__init__.py index c3eff03d2..70b5c21ff 100644 --- a/src/core/retargeting_engine/python/tensor_types/__init__.py +++ b/src/core/retargeting_engine/python/tensor_types/__init__.py @@ -16,6 +16,16 @@ NUM_BODY_JOINTS_PICO, RobotHandJoints, ) +from .tactile_types import ( + TactileVector, + TactileHeatmap, + FingerPowerVector, + ControllerHapticPulse, + EndEffectorForce, + NUM_HAPTIC_FINGERS, + NUM_CONTROLLER_HAPTIC_FIELDS, + NUM_END_EFFECTOR_FORCE_AXES, +) from .indices import ( HandInputIndex, HeadPoseIndex, @@ -24,6 +34,9 @@ FullBodyInputIndex, HandJointIndex, BodyJointPicoIndex, + FingerIndex, + ControllerHapticPulseField, + EndEffectorForceAxis, ) __all__ = [ @@ -43,6 +56,15 @@ "NUM_HAND_JOINTS", "NUM_BODY_JOINTS_PICO", "RobotHandJoints", + # Tactile / haptic types + "TactileVector", + "TactileHeatmap", + "FingerPowerVector", + "ControllerHapticPulse", + "EndEffectorForce", + "NUM_HAPTIC_FINGERS", + "NUM_CONTROLLER_HAPTIC_FIELDS", + "NUM_END_EFFECTOR_FORCE_AXES", # Indices "HandInputIndex", "HeadPoseIndex", @@ -51,4 +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 63c6afb7d..827262620 100644 --- a/src/core/retargeting_engine/python/tensor_types/indices.py +++ b/src/core/retargeting_engine/python/tensor_types/indices.py @@ -106,3 +106,29 @@ class BodyJointPicoIndex(IntEnum): RIGHT_WRIST = 21 LEFT_HAND = 22 RIGHT_HAND = 23 + + +class FingerIndex(IntEnum): + """Indices into a :func:`FingerPowerVector` (Manus / glove convention).""" + + 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.""" + + AMPLITUDE = 0 + FREQUENCY_HZ = 1 + DURATION_S = 2 + + +class EndEffectorForceAxis(IntEnum): + """Component indices into an :func:`EndEffectorForce` ``[fx, fy, fz]`` vector.""" + + X = 0 + Y = 1 + Z = 2 diff --git a/src/core/retargeting_engine/python/tensor_types/tactile_types.py b/src/core/retargeting_engine/python/tensor_types/tactile_types.py new file mode 100644 index 000000000..d16123b38 --- /dev/null +++ b/src/core/retargeting_engine/python/tensor_types/tactile_types.py @@ -0,0 +1,187 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +TensorGroupType definitions for tactile feedback and haptic output. + +Sim-side schemas (TactileVector, TactileHeatmap) carry contact / pressure data +from simulators (e.g. Isaac Lab's ContactSensor) into the retargeting pipeline. +Device-side schemas (FingerPowerVector, ControllerHapticPulse, EndEffectorForce) +describe what a haptic device adapter will accept. + +Retargeters map sim-side -> device-side schemas; vendor adapters implementing +``isaacteleop.haptic_devices.IHapticDevice`` declare which device-side schema +they accept via ``accepted_type()`` and the ``HapticSink`` retargeter uses that +for connect-time type checking. + +The existing :func:`TransformMatrix` factory in ``standard_types`` is reused +unchanged for the optional ``world_T_haptic`` frame leaf -- no new transform +schema is introduced here. +""" + +from ..interface.tensor_group_type import TensorGroupType +from .ndarray_types import NDArrayType, DLDataType + + +# Constants +NUM_HAPTIC_FINGERS = 5 +"""Number of fingers in a :func:`FingerPowerVector`. + +Manus / OpenXR-glove convention: Thumb, Index, Middle, Ring, Pinky. +""" + +NUM_CONTROLLER_HAPTIC_FIELDS = 3 +"""Fields in a :func:`ControllerHapticPulse`: ``[amplitude, frequency_hz, duration_s]``.""" + +NUM_END_EFFECTOR_FORCE_AXES = 3 +"""Components in an :func:`EndEffectorForce`: ``[fx, fy, fz]``.""" + + +# ============================================================================ +# Sim-side types +# ============================================================================ + + +def TactileVector(num_taxels: int) -> TensorGroupType: + """Per-taxel scalar magnitudes (or N-element vector) [N, depending on use]. + + Generic sim-side schema for tactile / contact data. The same type covers: + + * a single contact force magnitude (``num_taxels == 1``), + * a row of taxels on a finger pad (``num_taxels == N``), + * a 3-vector force or 3-vector position (``num_taxels == 3``) used by the + composable spatial primitives (e.g. :class:`Vector3FrameTransform`). + + The semantic meaning of each entry is set by the retargeter consuming + this group; this schema only fixes shape and dtype. + + Args: + num_taxels: Number of scalar entries. + + Returns: + TensorGroupType with one ``(num_taxels,) float32`` tensor. + """ + return TensorGroupType( + f"tactile_vector_{num_taxels}", + [ + NDArrayType( + "tactile_values", + shape=(num_taxels,), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + ], + ) + + +def TactileHeatmap(rows: int, cols: int, num_pads: int = 1) -> TensorGroupType: + """2D pressure grid per pad [Pa or unitless, depending on consumer]. + + Sim-side schema for heatmap-style tactile sensors (e.g. Sharpa TacMap). + Shape is ``(num_pads, rows, cols)`` so a single pad is still a 3D array + with leading dimension 1. + + Args: + rows: Rows per pad. + cols: Columns per pad. + num_pads: Number of independent pads, e.g. 5 for one pad per finger. + + Returns: + TensorGroupType with one ``(num_pads, rows, cols) float32`` tensor. + """ + return TensorGroupType( + f"tactile_heatmap_{num_pads}x{rows}x{cols}", + [ + NDArrayType( + "tactile_pressure", + shape=(num_pads, rows, cols), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + ], + ) + + +# ============================================================================ +# Device-side types +# ============================================================================ + + +def FingerPowerVector(num_fingers: int = NUM_HAPTIC_FINGERS) -> TensorGroupType: + """Per-finger vibration intensities [unitless, 0..1]. + + Device-side schema for vibration-glove output. Manus order: + ``[Thumb, Index, Middle, Ring, Pinky]`` (see :class:`FingerIndex` for the + indices). + + Consumed by :class:`isaacteleop.haptic_devices.ManusHapticDevice`. + + Args: + num_fingers: Number of finger channels. Defaults to 5 (Manus). + + 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 OpenXR motion-controller pulse ``[amplitude, frequency_hz, duration_s]``. + + Fields, in order (see :class:`ControllerHapticPulseField`): + + * ``amplitude`` [unitless, 0..1] -- 0 stops any active pulse via + :c:func:`xrStopHapticFeedback`. + * ``frequency_hz`` [Hz] -- ``0.0`` selects ``XR_FREQUENCY_UNSPECIFIED`` + (the runtime's default frequency). + * ``duration_s`` [s] -- ``0.0`` selects ``XR_MIN_HAPTIC_DURATION`` + (the shortest pulse the runtime supports). + + Maps directly to ``XrHapticVibration``. Consumed by + :class:`isaacteleop.haptic_devices.OpenXRControllerHapticDevice`. + """ + return TensorGroupType( + "controller_haptic_pulse", + [ + NDArrayType( + "haptic_pulse", + shape=(NUM_CONTROLLER_HAPTIC_FIELDS,), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + ], + ) + + +def EndEffectorForce() -> TensorGroupType: + """3-DoF directional force at a single point ``[fx, fy, fz]`` [N]. + + Device-side schema for grounded-haptic devices like the Haply Inverse3. + Components are in the *device* frame -- spatial retargeters upstream of + 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. + """ + return TensorGroupType( + "end_effector_force", + [ + NDArrayType( + "force", + shape=(NUM_END_EFFECTOR_FORCE_AXES,), + dtype=DLDataType.FLOAT, + dtype_bits=32, + ), + ], + ) diff --git a/src/core/retargeting_engine_tests/python/test_haptic_devices.py b/src/core/retargeting_engine_tests/python/test_haptic_devices.py new file mode 100644 index 000000000..9cc40107b --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_haptic_devices.py @@ -0,0 +1,227 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Tests for ``isaacteleop.haptic_devices.openxr_controller``. + +The OpenXR controller adapter is split into a sink-time +``OpenXRControllerHapticDevice`` (queues per-side pulses without a session in +scope) and a session-aware ``OpenXRControllerHapticSource`` that drains the +queue inside ``poll_tracker(session)``. We need to lock down: + +* Queue semantics (latest-wins coalescing, per-side independence, drain-clears). +* Shape validation on ``apply()``. +* Source forwards drained pulses to ``ControllerTracker.apply_haptic_feedback`` + with the right argument shape. +* Source swallows tracker exceptions and only logs once per side. +* ``for_controllers_source`` shares the underlying tracker handle. +""" + +from typing import List, Tuple + +import numpy as np +import pytest + +from isaacteleop.haptic_devices.openxr_controller import ( + OpenXRControllerHapticDevice, + OpenXRControllerHapticSource, +) +from isaacteleop.retargeting_engine.tensor_types import ControllerHapticPulse + + +_PulseCall = Tuple[object, str, float, float, float] + + +class _RecordingControllerTracker: + """Test double for ``ControllerTracker``. + + Implements just enough of the surface used by ``OpenXRControllerHapticSource``: + ``apply_haptic_feedback`` records the call; ``fail_sides`` makes selected + sides raise so we can exercise the once-per-side error gate. + """ + + def __init__(self, fail_sides: tuple[str, ...] = ()) -> None: + self.calls: List[_PulseCall] = [] + self.fail_sides = set(fail_sides) + + def apply_haptic_feedback(self, session, side, amplitude, frequency_hz, duration_s): + if side in self.fail_sides: + raise RuntimeError(f"simulated tracker failure on {side}") + self.calls.append((session, side, amplitude, frequency_hz, duration_s)) + + +# --------------------------------------------------------------------------- +# OpenXRControllerHapticDevice +# --------------------------------------------------------------------------- + + +class TestOpenXRControllerHapticDevice: + def test_accepted_type_is_controller_haptic_pulse(self) -> None: + device = OpenXRControllerHapticDevice() + assert device.accepted_type().name == ControllerHapticPulse().name + + def test_supports_reflects_sides_argument(self) -> None: + device = OpenXRControllerHapticDevice(sides=("right",)) + assert not device.supports("left") + assert device.supports("right") + + def test_apply_queues_per_side(self) -> None: + device = OpenXRControllerHapticDevice() + device.apply("left", np.array([0.4, 200.0, 0.05], dtype=np.float32)) + device.apply("right", np.array([0.7, 100.0, 0.10], dtype=np.float32)) + + pending = device.drain_pending() + assert set(pending.keys()) == {"left", "right"} + # `apply()` round-trips through float32, so use approx on each scalar + # rather than pytest.approx-in-dict (which compares element-wise via + # __eq__ and does not see float32 rounding). + l_amp, l_freq, l_dur = pending["left"] + assert l_amp == pytest.approx(0.4) + assert l_freq == pytest.approx(200.0) + assert l_dur == pytest.approx(0.05) + r_amp, r_freq, r_dur = pending["right"] + assert r_amp == pytest.approx(0.7) + assert r_freq == pytest.approx(100.0) + assert r_dur == pytest.approx(0.10) + + def test_apply_coalesces_to_latest_per_side(self) -> None: + """``xrApplyHapticFeedback`` already supersedes any in-flight pulse on + the same action, so coalescing the queue to "latest wins" per side + within one frame is correct, not lossy.""" + device = OpenXRControllerHapticDevice() + device.apply("left", np.array([0.1, 0.0, 0.0], dtype=np.float32)) + device.apply("left", np.array([0.9, 0.0, 0.0], dtype=np.float32)) + + pending = device.drain_pending() + assert pending == {"left": (pytest.approx(0.9), 0.0, 0.0)} + + def test_drain_clears_queue(self) -> None: + device = OpenXRControllerHapticDevice() + device.apply("left", np.array([0.4, 0.0, 0.0], dtype=np.float32)) + + device.drain_pending() + assert device.drain_pending() == {} + + def test_apply_rejects_wrong_shape(self) -> None: + device = OpenXRControllerHapticDevice() + with pytest.raises(ValueError, match="3-element"): + device.apply("left", np.array([0.1, 0.2], dtype=np.float32)) + + +# --------------------------------------------------------------------------- +# OpenXRControllerHapticSource +# --------------------------------------------------------------------------- + + +class TestOpenXRControllerHapticSource: + def test_get_tracker_returns_constructor_handle(self) -> None: + device = OpenXRControllerHapticDevice() + tracker = _RecordingControllerTracker() + source = OpenXRControllerHapticSource("haptic_source", device, tracker) + assert source.get_tracker() is tracker + + def test_for_controllers_source_shares_tracker(self) -> None: + """``for_controllers_source`` is the recommended path: it must hand + out the *same* tracker instance the controllers source already owns, + so ``DeviceIOSession`` deduplicates them by raw pointer.""" + device = OpenXRControllerHapticDevice() + tracker = _RecordingControllerTracker() + + class _DummyControllersSource: + def __init__(self, t) -> None: + self._t = t + + def get_tracker(self): + return self._t + + controllers = _DummyControllersSource(tracker) + source = OpenXRControllerHapticSource.for_controllers_source( + "haptic_source", device, controllers + ) + assert source.get_tracker() is tracker + + def test_poll_tracker_drains_and_forwards(self) -> None: + device = OpenXRControllerHapticDevice() + tracker = _RecordingControllerTracker() + source = OpenXRControllerHapticSource("haptic_source", device, tracker) + + device.apply("left", np.array([0.3, 100.0, 0.05], dtype=np.float32)) + device.apply("right", np.array([0.7, 0.0, 0.0], dtype=np.float32)) + + sentinel_session = object() + result = source.poll_tracker(sentinel_session) + + assert result == {}, "no inputs declared, so an empty dict is correct" + assert len(tracker.calls) == 2 + # Verify the session sentinel and field ordering reach the C++ binding + # exactly as the tracker expects. + sides = sorted(call[1] for call in tracker.calls) + assert sides == ["left", "right"] + for session, _side, amplitude, frequency_hz, duration_s in tracker.calls: + assert session is sentinel_session + assert isinstance(amplitude, float) + assert isinstance(frequency_hz, float) + assert isinstance(duration_s, float) + + def test_poll_tracker_leaves_queue_empty(self) -> None: + device = OpenXRControllerHapticDevice() + tracker = _RecordingControllerTracker() + source = OpenXRControllerHapticSource("haptic_source", device, tracker) + + device.apply("left", np.array([0.3, 0.0, 0.0], dtype=np.float32)) + source.poll_tracker(object()) + assert device.drain_pending() == {} + + def test_poll_tracker_swallows_exceptions(self) -> None: + """A failing tracker call must not propagate; haptic feedback is a + nice-to-have and a hardware hiccup must not tear the session down.""" + device = OpenXRControllerHapticDevice() + tracker = _RecordingControllerTracker(fail_sides=("left",)) + source = OpenXRControllerHapticSource("haptic_source", device, tracker) + + device.apply("left", np.array([0.4, 0.0, 0.0], dtype=np.float32)) + device.apply("right", np.array([0.6, 0.0, 0.0], dtype=np.float32)) + + # No exception should escape, even though "left" raises internally. + source.poll_tracker(object()) + + # The right side still gets through. + assert [call[1] for call in tracker.calls] == ["right"] + + def test_poll_tracker_logs_failure_at_most_once_per_side(self, caplog) -> None: + """Once-per-side log gate keeps a chronically failing side from + flooding the log every frame.""" + device = OpenXRControllerHapticDevice() + tracker = _RecordingControllerTracker(fail_sides=("left",)) + source = OpenXRControllerHapticSource("haptic_source", device, tracker) + + for _ in range(3): + device.apply("left", np.array([0.4, 0.0, 0.0], dtype=np.float32)) + with caplog.at_level("WARNING"): + source.poll_tracker(object()) + + warnings = [ + r + for r in caplog.records + if "OpenXRControllerHapticSource" in r.getMessage() + ] + assert len(warnings) == 1, ( + "expected a single once-per-side warning, " + f"got {[r.getMessage() for r in warnings]}" + ) + + def test_compute_fn_sets_heartbeat(self) -> None: + """The heartbeat output is only there so OutputCombiner discovers the + source; ``poll_tracker`` does the real work, but ``_compute_fn`` must + still produce a value or the graph errors out.""" + from isaacteleop.retargeting_engine.interface.base_retargeter import ( + _make_output_group, + ) + + device = OpenXRControllerHapticDevice() + tracker = _RecordingControllerTracker() + source = OpenXRControllerHapticSource("haptic_source", device, tracker) + + outputs = {k: _make_output_group(v) for k, v in source.output_spec().items()} + source.compute({}, outputs) + assert outputs[OpenXRControllerHapticSource.HEARTBEAT][0] is True diff --git a/src/core/retargeting_engine_tests/python/test_haptic_sink.py b/src/core/retargeting_engine_tests/python/test_haptic_sink.py new file mode 100644 index 000000000..ac9edf176 --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_haptic_sink.py @@ -0,0 +1,211 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Tests for ``HapticSink``: vendor-agnostic dispatch from the retargeting graph +to whatever ``IHapticDevice`` adapter is plugged in. + +The sink is a sink-only node modelled after ``MessageChannelSink``: its only +output is a heartbeat boolean used so ``OutputCombiner`` discovers it as a +graph leaf. The behaviour we need to lock down here is + +* the accepted-type round-trip (so connect-time type checking works), and +* the per-side dispatch contract — call ``device.apply(side, values)`` only + when the side is both present in inputs *and* ``device.supports(side)``. +""" + +from typing import Any, List + +import numpy as np +import pytest + +from isaacteleop.haptic_devices import IHapticDevice +from isaacteleop.retargeting_engine.deviceio_source_nodes import HapticSink +from isaacteleop.retargeting_engine.interface import OutputCombiner, ValueInput +from isaacteleop.retargeting_engine.interface.base_retargeter import _make_output_group +from isaacteleop.retargeting_engine.interface.tensor_group import ( + OptionalTensorGroup, +) +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerHapticPulse, + FingerPowerVector, + NUM_HAPTIC_FINGERS, +) + + +class _RecordingDevice(IHapticDevice): + """Test double that records every ``apply()`` call. + + Restricting `supports()` lets the dispatch test confirm `HapticSink` honours + per-side availability — Haply Inverse3 will have the same shape. + """ + + def __init__( + self, + accepted=None, + supported_sides=("left", "right"), + ) -> None: + self._accepted = ( + accepted if accepted is not None else FingerPowerVector(NUM_HAPTIC_FINGERS) + ) + self._supported_sides = set(supported_sides) + self.calls: List[tuple[str, np.ndarray]] = [] + + def accepted_type(self): + return self._accepted + + def apply(self, side, values): + self.calls.append((side, np.asarray(values, dtype=np.float32).copy())) + + def supports(self, side): + return side in self._supported_sides + + +def _build_inputs(sink: HapticSink, *, left=None, right=None): + """Build a HapticSink-shaped inputs dict. + + Each side defaults to absent (an empty ``OptionalTensorGroup``) — sources + that are not wired in pass through ``BaseRetargeter._fill_optional_inputs`` + as absent at runtime, and the sink must skip them. + """ + inputs = {} + for side, value in (("left", left), ("right", right)): + spec = sink._inputs[side] + inner = spec.inner_type if spec.is_optional else spec + group = OptionalTensorGroup(inner) + if value is not None: + group[0] = np.asarray(value, dtype=np.float32) + inputs[side] = group + return inputs + + +def _build_outputs(sink: HapticSink): + return {k: _make_output_group(v) for k, v in sink.output_spec().items()} + + +def _compute(sink: HapticSink, inputs): + outputs = _build_outputs(sink) + sink.compute(inputs, outputs) + return outputs + + +# --------------------------------------------------------------------------- +# accepted_type round-trip / connect-time type check +# --------------------------------------------------------------------------- + + +class TestAcceptedType: + def test_input_spec_uses_accepted_type(self) -> None: + device = _RecordingDevice(accepted=FingerPowerVector(NUM_HAPTIC_FINGERS)) + sink = HapticSink("sink", device) + + for side in (HapticSink.LEFT, HapticSink.RIGHT): + spec = sink.input_spec()[side] + assert spec.is_optional, "sides are optional so single-handed rigs work" + assert spec.inner_type.name == device.accepted_type().name + + def test_connect_accepts_matching_upstream_type(self) -> None: + device = _RecordingDevice(accepted=ControllerHapticPulse()) + sink = HapticSink("sink", device) + + leaf = ValueInput("upstream", ControllerHapticPulse()) + # connect() raises on type mismatch; reaching the assignment is the + # implicit success signal. + sink.connect({HapticSink.LEFT: leaf.output("value")}) + + def test_connect_rejects_mismatched_upstream_type(self) -> None: + device = _RecordingDevice(accepted=ControllerHapticPulse()) + sink = HapticSink("sink", device) + + leaf = ValueInput("upstream", FingerPowerVector(NUM_HAPTIC_FINGERS)) + with pytest.raises(Exception): + # Compatibility check raises a TypeError or AssertionError depending + # on which TensorType detects the mismatch first; either is fine. + sink.connect({HapticSink.LEFT: leaf.output("value")}) + + +# --------------------------------------------------------------------------- +# Dispatch contract +# --------------------------------------------------------------------------- + + +class TestDispatch: + def test_calls_device_apply_for_each_present_supported_side(self) -> None: + device = _RecordingDevice() + sink = HapticSink("sink", device) + + left_values = np.array([0.1, 0.2, 0.3, 0.4, 0.5], dtype=np.float32) + right_values = np.array([0.5, 0.4, 0.3, 0.2, 0.1], dtype=np.float32) + _compute(sink, _build_inputs(sink, left=left_values, right=right_values)) + + sides_called = {side for side, _ in device.calls} + assert sides_called == {"left", "right"} + for side, values in device.calls: + expected = left_values if side == "left" else right_values + np.testing.assert_array_equal(values, expected) + + def test_skips_absent_sides(self) -> None: + device = _RecordingDevice() + sink = HapticSink("sink", device) + + left_values = np.array([0.1, 0.2, 0.3, 0.4, 0.5], dtype=np.float32) + _compute(sink, _build_inputs(sink, left=left_values, right=None)) + + sides_called = [side for side, _ in device.calls] + assert sides_called == ["left"] + + def test_skips_unsupported_sides(self) -> None: + device = _RecordingDevice(supported_sides=("right",)) + sink = HapticSink("sink", device) + + left_values = np.array([0.1, 0.2, 0.3, 0.4, 0.5], dtype=np.float32) + right_values = np.array([0.5, 0.4, 0.3, 0.2, 0.1], dtype=np.float32) + _compute(sink, _build_inputs(sink, left=left_values, right=right_values)) + + sides_called = [side for side, _ in device.calls] + assert sides_called == ["right"] + + def test_no_calls_when_both_sides_absent(self) -> None: + device = _RecordingDevice() + sink = HapticSink("sink", device) + + _compute(sink, _build_inputs(sink, left=None, right=None)) + + assert device.calls == [] + + +# --------------------------------------------------------------------------- +# Heartbeat / discoverability +# --------------------------------------------------------------------------- + + +class TestHeartbeat: + def test_heartbeat_is_set_each_step(self) -> None: + device = _RecordingDevice() + sink = HapticSink("sink", device) + + outputs = _compute(sink, _build_inputs(sink, left=None, right=None)) + + assert outputs[HapticSink.HEARTBEAT][0] is True + + def test_sink_is_reachable_from_output_combiner(self) -> None: + """OutputCombiner must enumerate the sink as a leaf via the heartbeat. + + This locks down the discovery invariant called out in the haptic-sink + docstring: a custom combiner that does not declare any sink output + will not discover the sink, so haptics never fire. + """ + device = _RecordingDevice() + sink = HapticSink("sink", device) + + # No upstream — just include the heartbeat so OutputCombiner walks + # back to the sink. Real users would wire `.LEFT`/`.RIGHT`; we are + # only testing graph discovery here. + combiner = OutputCombiner( + {HapticSink.HEARTBEAT: sink.output(HapticSink.HEARTBEAT)} + ) + + leaves: List[Any] = combiner.get_leaf_nodes() + assert sink in leaves, ( + "HapticSink must be discoverable from a combiner that selects its heartbeat" + ) diff --git a/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py b/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py new file mode 100644 index 000000000..62daafdd5 --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py @@ -0,0 +1,360 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Tests for ``isaacteleop.retargeters.tactile_retargeters``. + +Covers the composable spatial primitives (``Vector3FrameTransform``, +``WorldForceAccumulator``, ``MagnitudeReducer``) and the per-device mappers +that turn sim-side ``TactileVector`` / ``TactileHeatmap`` flows into the +device-side schemas (``FingerPowerVector`` / ``ControllerHapticPulse``). + +The shared gain/deadband/saturation curve and EMA smoothing live behind +``_apply_gain_curve`` / ``_smooth_ema``; we test them indirectly through +``TactileVectorToFingerPower`` (the canonical Manus-shaped consumer). +""" + +import numpy as np +import numpy.testing as npt +import pytest + +from isaacteleop.retargeters.tactile_retargeters import ( + MagnitudeReducer, + TactileHeatmapToControllerPulse, + TactileHeatmapToFingerPower, + TactileHeatmapToWristPulse, + TactileVectorToControllerPulse, + TactileVectorToFingerPower, + Vector3FrameTransform, + WorldForceAccumulator, +) +from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + ExecutionEvents, + ExecutionState, + TensorGroup, +) +from isaacteleop.retargeting_engine.interface.base_retargeter import _make_output_group +from isaacteleop.retargeting_engine.interface.retargeter_core_types import GraphTime +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerHapticPulseField, + FingerIndex, + NUM_HAPTIC_FINGERS, +) + + +def _make_context(*, reset: bool = False) -> ComputeContext: + return ComputeContext( + graph_time=GraphTime(sim_time_ns=0, real_time_ns=0), + execution_events=ExecutionEvents( + reset=reset, execution_state=ExecutionState.RUNNING + ), + ) + + +def _build_inputs(retargeter, raw): + inputs = {} + spec = retargeter.input_spec() + for name, value in raw.items(): + tg = TensorGroup(spec[name]) + tg[0] = np.asarray(value, dtype=np.float32) + inputs[name] = tg + return inputs + + +def _build_outputs(retargeter): + return {k: _make_output_group(v) for k, v in retargeter.output_spec().items()} + + +def _run(retargeter, raw, *, reset=False): + inputs = _build_inputs(retargeter, raw) + outputs = _build_outputs(retargeter) + retargeter.compute(inputs, outputs, _make_context(reset=reset)) + return outputs + + +# --------------------------------------------------------------------------- +# Composable spatial primitives +# --------------------------------------------------------------------------- + + +class TestVector3FrameTransform: + """``Vector3FrameTransform`` is rotation-only by design (forces are free + vectors). The translation column of the transform must not leak into the + output, even when set.""" + + def test_identity_passthrough(self) -> None: + node = Vector3FrameTransform("xform") + outputs = _run( + node, + { + "vec": [1.0, 2.0, 3.0], + "transform": np.eye(4, dtype=np.float32), + }, + ) + npt.assert_array_almost_equal( + np.asarray(outputs["vec"][0]), np.array([1.0, 2.0, 3.0], dtype=np.float32) + ) + + def test_90deg_z_rotation(self) -> None: + """Rotation about Z by +90 degrees: x -> y, y -> -x, z -> z.""" + node = Vector3FrameTransform("xform") + rot_z_90 = np.eye(4, dtype=np.float32) + rot_z_90[:3, :3] = np.array( + [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32 + ) + + outputs = _run(node, {"vec": [1.0, 0.0, 0.0], "transform": rot_z_90}) + npt.assert_array_almost_equal( + np.asarray(outputs["vec"][0]), np.array([0.0, 1.0, 0.0], dtype=np.float32) + ) + + def test_translation_is_ignored(self) -> None: + """Pile a big translation into the matrix; the output must be + unchanged because forces are free vectors.""" + node = Vector3FrameTransform("xform") + transform = np.eye(4, dtype=np.float32) + transform[:3, 3] = [10.0, -20.0, 30.0] + + outputs = _run(node, {"vec": [1.0, 2.0, 3.0], "transform": transform}) + npt.assert_array_almost_equal( + np.asarray(outputs["vec"][0]), np.array([1.0, 2.0, 3.0], dtype=np.float32) + ) + + +class TestWorldForceAccumulator: + def test_default_uniform_weights_sum_inputs(self) -> None: + node = WorldForceAccumulator("acc", num_inputs=2) + outputs = _run( + node, + {"in_0": [1.0, 2.0, 3.0], "in_1": [10.0, 20.0, 30.0]}, + ) + npt.assert_array_almost_equal( + np.asarray(outputs["vec"][0]), + np.array([11.0, 22.0, 33.0], dtype=np.float32), + ) + + def test_constructor_weights_scale_inputs(self) -> None: + node = WorldForceAccumulator("acc", num_inputs=2, weights=[0.5, 2.0]) + outputs = _run( + node, + {"in_0": [4.0, 0.0, 0.0], "in_1": [1.0, 0.0, 0.0]}, + ) + npt.assert_array_almost_equal( + np.asarray(outputs["vec"][0]), + np.array([0.5 * 4.0 + 2.0 * 1.0, 0.0, 0.0], dtype=np.float32), + ) + + def test_rejects_zero_inputs(self) -> None: + with pytest.raises(ValueError, match="num_inputs"): + WorldForceAccumulator("acc", num_inputs=0) + + def test_rejects_mismatched_weight_length(self) -> None: + with pytest.raises(ValueError, match="weights length"): + WorldForceAccumulator("acc", num_inputs=2, weights=[1.0, 2.0, 3.0]) + + +class TestMagnitudeReducer: + @pytest.mark.parametrize( + "vec, expected", + [ + ([3.0, 4.0, 0.0], 5.0), + ([0.0, 0.0, 0.0], 0.0), + ([-1.0, -2.0, -2.0], 3.0), + ], + ) + def test_norm_mode(self, vec, expected) -> None: + node = MagnitudeReducer("mag", mode="norm") + outputs = _run(node, {"vec": vec}) + assert float(np.asarray(outputs["scalar"][0])[0]) == pytest.approx(expected) + + def test_axis_modes_take_absolute_value(self) -> None: + for mode, axis in [("axis_x", 0), ("axis_y", 1), ("axis_z", 2)]: + node = MagnitudeReducer(f"mag_{mode}", mode=mode) + vec = [0.0, 0.0, 0.0] + vec[axis] = -2.5 + outputs = _run(node, {"vec": vec}) + assert float(np.asarray(outputs["scalar"][0])[0]) == pytest.approx(2.5) + + def test_rejects_unknown_mode(self) -> None: + with pytest.raises(ValueError, match="unknown mode"): + MagnitudeReducer("mag", mode="dot_with_normal") + + +# --------------------------------------------------------------------------- +# Per-device mappers — gain / deadband / saturation curve +# --------------------------------------------------------------------------- + + +class TestTactileVectorToFingerPower: + def test_default_one_to_one_finger_groups(self) -> None: + node = TactileVectorToFingerPower("ftf", num_taxels=NUM_HAPTIC_FINGERS) + # Per-finger raw values map straight through with default gain=1, deadband=0. + 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_deadband_suppresses_low_signal(self) -> None: + node = TactileVectorToFingerPower( + "ftf", + num_taxels=NUM_HAPTIC_FINGERS, + deadband=0.3, + ) + outputs = _run(node, {"tactile": [0.1, 0.2, 0.3, 0.4, 0.5]}) + powers = np.asarray(outputs["powers"][0]) + # 0.1, 0.2 are below deadband -> 0; 0.3, 0.4, 0.5 -> 0.0, 0.1, 0.2 + npt.assert_array_almost_equal( + powers, np.array([0.0, 0.0, 0.0, 0.1, 0.2], dtype=np.float32) + ) + + def test_saturation_clamps_high_signal(self) -> None: + node = TactileVectorToFingerPower( + "ftf", + num_taxels=NUM_HAPTIC_FINGERS, + gain=10.0, + saturation=0.7, + ) + outputs = _run(node, {"tactile": [0.0, 0.0, 0.0, 0.0, 1.0]}) + powers = np.asarray(outputs["powers"][0]) + assert powers[FingerIndex.PINKY] == pytest.approx(0.7) + + def test_finger_groups_with_max_reduction(self) -> None: + # Two taxels per finger; pick the larger. + node = TactileVectorToFingerPower( + "ftf", + num_taxels=10, + finger_groups=[[0, 1], [2, 3], [4, 5], [6, 7], [8, 9]], + reduction="max", + ) + outputs = _run( + node, + {"tactile": [0.1, 0.9, 0.3, 0.2, 0.5, 0.4, 0.7, 0.6, 0.8, 0.0]}, + ) + npt.assert_array_almost_equal( + np.asarray(outputs["powers"][0]), + np.array([0.9, 0.3, 0.5, 0.7, 0.8], dtype=np.float32), + ) + + def test_smoothing_alpha_one_means_no_smoothing(self) -> None: + node = TactileVectorToFingerPower( + "ftf", + num_taxels=NUM_HAPTIC_FINGERS, + smoothing=1.0, + ) + outputs = _run(node, {"tactile": [0.5, 0.5, 0.5, 0.5, 0.5]}) + npt.assert_array_almost_equal( + np.asarray(outputs["powers"][0]), + np.array([0.5, 0.5, 0.5, 0.5, 0.5], dtype=np.float32), + ) + + def test_reset_clears_smoothing_state(self) -> None: + """``context.execution_events.reset`` must drop accumulated EMA + state so the next step starts from the new sample, not blended into + whatever was there before reset.""" + node = TactileVectorToFingerPower( + "ftf", + num_taxels=NUM_HAPTIC_FINGERS, + smoothing=0.1, # heavy EMA carry-over + ) + # First step warms up the EMA. + _run(node, {"tactile": [1.0, 1.0, 1.0, 1.0, 1.0]}) + assert node._smoothed is not None + + # Reset must zero the carry-over before the next compute. + outputs = _run(node, {"tactile": [0.5, 0.5, 0.5, 0.5, 0.5]}, reset=True) + # On reset the smoother re-seeds from the new sample, so the output + # is exactly the new value (not blended with the old 1.0s). + npt.assert_array_almost_equal( + np.asarray(outputs["powers"][0]), + np.array([0.5, 0.5, 0.5, 0.5, 0.5], dtype=np.float32), + ) + + def test_rejects_finger_groups_out_of_range(self) -> None: + with pytest.raises(ValueError, match="outside"): + TactileVectorToFingerPower( + "ftf", + num_taxels=3, + finger_groups=[[0], [1], [2], [3], [4]], + ) + + def test_requires_finger_groups_when_lengths_mismatch(self) -> None: + with pytest.raises(ValueError, match="finger_groups is required"): + TactileVectorToFingerPower("ftf", num_taxels=3) + + +class TestTactileVectorToControllerPulse: + def test_amplitude_packed_into_pulse(self) -> None: + node = TactileVectorToControllerPulse( + "vec_to_pulse", + num_taxels=3, + frequency_hz=120.0, + duration_s=0.05, + ) + outputs = _run(node, {"tactile": [0.1, 0.5, 0.2]}) + pulse = np.asarray(outputs["pulse"][0]) + assert pulse[ControllerHapticPulseField.AMPLITUDE] == pytest.approx(0.5) + assert pulse[ControllerHapticPulseField.FREQUENCY_HZ] == pytest.approx(120.0) + assert pulse[ControllerHapticPulseField.DURATION_S] == pytest.approx(0.05) + + def test_zero_frequency_and_duration_pass_through(self) -> None: + """Defaults of 0/0 round-trip exactly so the C++ side can map them + to ``XR_FREQUENCY_UNSPECIFIED`` / ``XR_MIN_HAPTIC_DURATION``.""" + node = TactileVectorToControllerPulse("vec_to_pulse", num_taxels=1) + outputs = _run(node, {"tactile": [0.3]}) + pulse = np.asarray(outputs["pulse"][0]) + assert pulse[ControllerHapticPulseField.FREQUENCY_HZ] == 0.0 + assert pulse[ControllerHapticPulseField.DURATION_S] == 0.0 + + def test_deadband_zeros_amplitude(self) -> None: + node = TactileVectorToControllerPulse( + "vec_to_pulse", num_taxels=1, deadband=0.5 + ) + outputs = _run(node, {"tactile": [0.3]}) + assert ( + float(np.asarray(outputs["pulse"][0])[ControllerHapticPulseField.AMPLITUDE]) + == 0.0 + ) + + +class TestHeatmapMappers: + def test_heatmap_to_finger_power_max_reduction(self) -> None: + node = TactileHeatmapToFingerPower("heat_finger", rows=2, cols=2) + # Five pads, each (2, 2). Different max per pad. + heatmap = np.array( + [ + [[0.1, 0.2], [0.3, 0.4]], + [[0.5, 0.6], [0.7, 0.8]], + [[0.0, 0.0], [0.0, 0.0]], + [[0.9, 0.1], [0.1, 0.1]], + [[0.2, 0.2], [0.2, 0.2]], + ], + dtype=np.float32, + ) + outputs = _run(node, {"heatmap": heatmap}) + npt.assert_array_almost_equal( + np.asarray(outputs["powers"][0]), + np.array([0.4, 0.8, 0.0, 0.9, 0.2], dtype=np.float32), + ) + + def test_heatmap_to_wrist_pulse_collapses_full_array(self) -> None: + node = TactileHeatmapToWristPulse( + "heat_wrist", rows=2, cols=2, num_pads=3, reduction="sum" + ) + heatmap = np.ones((3, 2, 2), dtype=np.float32) + outputs = _run(node, {"heatmap": heatmap}) + # 3 pads * 4 cells * 1.0 = 12, then clamped to saturation=1.0 default. + assert float(np.asarray(outputs["power"][0])[0]) == pytest.approx(1.0), ( + "saturation should clamp the very large sum" + ) + + def test_heatmap_to_controller_pulse(self) -> None: + node = TactileHeatmapToControllerPulse( + "heat_pulse", rows=2, cols=2, num_pads=1, frequency_hz=200.0 + ) + heatmap = np.array([[[0.0, 0.4], [0.2, 0.1]]], dtype=np.float32) + outputs = _run(node, {"heatmap": heatmap}) + pulse = np.asarray(outputs["pulse"][0]) + assert pulse[ControllerHapticPulseField.AMPLITUDE] == pytest.approx(0.4) + assert pulse[ControllerHapticPulseField.FREQUENCY_HZ] == pytest.approx(200.0) diff --git a/src/haptic_devices/CMakeLists.txt b/src/haptic_devices/CMakeLists.txt new file mode 100644 index 000000000..ca07e2459 --- /dev/null +++ b/src/haptic_devices/CMakeLists.txt @@ -0,0 +1,15 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +if(BUILD_PYTHON_BINDINGS) + add_custom_target(haptic_devices_python ALL + COMMAND ${CMAKE_COMMAND} -E copy_directory + "${CMAKE_CURRENT_SOURCE_DIR}" + "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/haptic_devices" + COMMAND ${CMAKE_COMMAND} -E rm -rf + "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/haptic_devices/__pycache__" + COMMAND ${CMAKE_COMMAND} -E rm -f + "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/haptic_devices/CMakeLists.txt" + COMMENT "Copying haptic_devices Python files to package structure" + ) +endif() diff --git a/src/haptic_devices/__init__.py b/src/haptic_devices/__init__.py new file mode 100644 index 000000000..983f29416 --- /dev/null +++ b/src/haptic_devices/__init__.py @@ -0,0 +1,26 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Vendor-agnostic haptic device adapters for Isaac Teleop. + +Each adapter implements :class:`IHapticDevice` -- the contract consumed by +:class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.HapticSink`. +``apply()`` is pure I/O: no geometry, no morphology mapping. Those concerns +live upstream in retargeters; the adapter writes the bytes the device expects +in the units it expects, full stop. + +Vendor-specific adapter modules (``manus``, ``openxr_controller``, ...) lazy- +import their backing pybind11 modules so the package itself can always be +imported even when individual vendor extensions are not built. Users that need +a specific adapter import the submodule directly:: + + from isaacteleop.haptic_devices.manus import ManusHapticDevice + from isaacteleop.haptic_devices.openxr_controller import ( + OpenXRControllerHapticDevice, + OpenXRControllerHapticSource, + ) +""" + +from .interface import IHapticDevice + +__all__ = ["IHapticDevice"] diff --git a/src/haptic_devices/interface.py b/src/haptic_devices/interface.py new file mode 100644 index 000000000..fb64828a5 --- /dev/null +++ b/src/haptic_devices/interface.py @@ -0,0 +1,100 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Vendor-agnostic :class:`IHapticDevice` interface. + +The contract is intentionally tiny: an adapter declares its accepted +device-side schema and writes one frame to hardware. Type checking happens +at ``HapticSink.connect()`` time via ``accepted_type()``; per-frame dispatch +happens via ``apply()``. +""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import Literal + +import numpy as np + +from isaacteleop.retargeting_engine.interface.tensor_group_type import TensorGroupType + + +Side = Literal["left", "right"] +"""Hand / side identifier consumed by :meth:`IHapticDevice.apply` and +:meth:`IHapticDevice.supports`.""" + + +class IHapticDevice(ABC): + """Vendor-agnostic adapter consumed by ``HapticSink``. + + Implementations live under :mod:`isaacteleop.haptic_devices` (or downstream + packages) and wrap whatever I/O channel the vendor exposes -- a vendor SDK + call, an OpenXR action, a WebSocket message, etc. + + Contract: + + * ``accepted_type()`` returns the device-side + :class:`~isaacteleop.retargeting_engine.interface.tensor_group_type.TensorGroupType` + this adapter writes to hardware (e.g. + :func:`~isaacteleop.retargeting_engine.tensor_types.FingerPowerVector` + for Manus, :func:`~isaacteleop.retargeting_engine.tensor_types.ControllerHapticPulse` + for OpenXR motion controllers). ``HapticSink`` uses this for connect-time + type checking against the upstream retargeter. + * ``apply(side, values)`` writes one frame to hardware. The values array is + already in the device's native units and frame -- spatial transforms and + morphology mapping happen upstream in retargeters. ``apply()`` must be + cheap and non-throwing; hardware errors are logged-and-no-op'd so a + transient device hiccup never tears down the pipeline. + * ``supports(side)`` reports per-side availability for adapters where the + device is single-handed (e.g. Haply Inverse3) or where one side is + disabled by config. Default: both sides supported. + + Sub-classes must not perform geometry or morphology in ``apply()``. Those + concerns belong upstream so they can be visualised and tuned through the + existing retargeter parameter UI. + """ + + @abstractmethod + def accepted_type(self) -> TensorGroupType: + """Return the device-side ``TensorGroupType`` this adapter consumes. + + The returned value is checked structurally against the upstream + retargeter's output type at ``HapticSink.connect()`` time, so wrong + wiring fails before any hardware call. + + Returns: + A ``TensorGroupType`` describing the shape and dtype of one + ``apply()`` payload. + """ + + @abstractmethod + def apply(self, side: Side, values: np.ndarray) -> None: + """Write one frame of haptic output to hardware. + + Implementations must be cheap and non-throwing. On hardware errors, + log once and return -- a transient device hiccup must not tear down + the retargeting pipeline. + + Args: + side: ``"left"`` or ``"right"``. + values: One frame of device-side values, shape matching the inner + tensor of :meth:`accepted_type`. For ``FingerPowerVector(5)`` + this is a ``(5,)`` float32 in ``[0, 1]``; for + ``ControllerHapticPulse`` it is ``[amplitude, frequency_hz, + duration_s]``. + """ + + def supports(self, side: Side) -> bool: + """Report whether this adapter writes to hardware for a given side. + + ``HapticSink`` gates calls to :meth:`apply` on this method so + single-handed devices (e.g. Haply Inverse3) cleanly no-op the + unconfigured side. + + Args: + side: ``"left"`` or ``"right"``. + + Returns: + ``True`` (default) when the side is active. + """ + return True diff --git a/src/haptic_devices/manus.py b/src/haptic_devices/manus.py new file mode 100644 index 000000000..62b1bf0db --- /dev/null +++ b/src/haptic_devices/manus.py @@ -0,0 +1,118 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Manus Metagloves Pro Haptic adapter. + +Bridges :class:`IHapticDevice` to the Manus plugin singleton via a small +pybind11 module (``_manus_haptic``). The plugin singleton must already be +alive -- the Manus hand-tracking plugin sets it up, and its connection +lifecycle is managed there. This adapter only forwards per-finger powers +to ``CoreSdk_VibrateFingersForGlove`` through the plugin's haptic entry +point. +""" + +from __future__ import annotations + +import logging +from typing import Literal + +import numpy as np + +from isaacteleop.retargeting_engine.interface.tensor_group_type import TensorGroupType +from isaacteleop.retargeting_engine.tensor_types import ( + FingerPowerVector, + NUM_HAPTIC_FINGERS, +) + +from .interface import IHapticDevice + + +logger = logging.getLogger(__name__) + + +class ManusHapticDevice(IHapticDevice): + """:class:`IHapticDevice` adapter for the Manus Metagloves Pro Haptic glove. + + Consumes :func:`FingerPowerVector(num_fingers=5) ` + in Manus order ``[Thumb, Index, Middle, Ring, Pinky]`` with values in + ``[0, 1]`` (the C++ side clamps; values outside the range are silently + rounded in). ``apply()`` is a thin shim over the pybind module that + forwards to ``ManusTracker::apply_haptic_command`` -- vendor SDK linkage + stays inside ``src/plugins/manus/`` per the AGENTS.md boundary. + + The pybind module is imported lazily on the first :meth:`apply` call so + importing :mod:`isaacteleop.haptic_devices.manus` does not require the + Manus SDK to be installed; an ``ImportError`` with a clear message is + raised at use time if the SDK is missing. + """ + + def __init__(self, num_fingers: int = NUM_HAPTIC_FINGERS) -> None: + """Construct a Manus haptic adapter. + + Args: + num_fingers: Channel count. Manus hardware fixes this to 5; + exposed for parity with :func:`FingerPowerVector` and to + catch mis-wired pipelines at ``connect()`` time. + """ + self._num_fingers = num_fingers + # Lazy-imported on first apply(); set once for the lifetime of the process. + self._pybind = None + # Log hardware errors at most once per side to keep the pipeline log clean. + self._error_logged: dict[str, bool] = {"left": False, "right": False} + + def accepted_type(self) -> TensorGroupType: + return FingerPowerVector(self._num_fingers) + + def apply(self, side: Literal["left", "right"], values: np.ndarray) -> None: + pybind = self._get_pybind() + if pybind is None: + return + + # Force the shape and dtype the C++ side expects. NumPy view; no copy + # when the upstream retargeter already produced a contiguous float32. + arr = np.asarray(values, dtype=np.float32).ravel() + if arr.size != self._num_fingers: + raise ValueError( + f"ManusHapticDevice.apply expects a {self._num_fingers}-element " + "FingerPowerVector " + f"(order [Thumb, Index, Middle, Ring, Pinky]), got shape {np.asarray(values).shape}" + ) + powers = arr.reshape(self._num_fingers) + try: + pybind.apply_haptic_command(side, powers) + except Exception as exc: + if not self._error_logged[side]: + logger.warning( + "ManusHapticDevice.apply(%s) failed (will silence further " + "errors for this side): %s", + side, + exc, + ) + self._error_logged[side] = True + + def _get_pybind(self): + """Lazy-import the Manus haptic pybind module. + + Cached after first successful import. When the module is unavailable + (Manus SDK not installed at build time), logs once and returns ``None`` + so the pipeline keeps running -- haptic feedback is a nice-to-have, + not a session-fatal dependency. + """ + if self._pybind is not None: + return self._pybind + try: + from . import _manus_haptic # type: ignore[import-not-found] + except ImportError as exc: + if not self._error_logged["left"]: + # Reuse one error_logged slot; the message is identical for both sides. + logger.warning( + "ManusHapticDevice unavailable: %s. " + "Build the Manus plugin (src/plugins/manus/) with the " + "Manus SDK installed to enable haptic output.", + exc, + ) + self._error_logged["left"] = True + self._error_logged["right"] = True + return None + self._pybind = _manus_haptic + return self._pybind diff --git a/src/haptic_devices/openxr_controller.py b/src/haptic_devices/openxr_controller.py new file mode 100644 index 000000000..235791863 --- /dev/null +++ b/src/haptic_devices/openxr_controller.py @@ -0,0 +1,262 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""OpenXR motion-controller haptic adapter (Quest, Vive, Index, Pico, ...). + +Unlike vendor-SDK gloves, OpenXR motion-controller haptics ride the standard +OpenXR action system the live trackers layer is already built around. The +integration is therefore a *tracker extension*, not a new plugin -- the +``LiveControllerTrackerImpl`` gains a haptic action and an +``apply_haptic_feedback`` method, and the public +:class:`~isaacteleop.deviceio_trackers.ControllerTracker` gains a matching +delegating method that takes a session. + +To talk through the active :class:`~isaacteleop.deviceio.DeviceIOSession`, +this module ships a pair following the existing +:class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.MessageChannelSink` +pattern: :class:`OpenXRControllerHapticDevice` enqueues frames during +``HapticSink._compute_fn`` (no session in scope there), and +:class:`OpenXRControllerHapticSource` drains the queue inside +``poll_tracker(session)`` (where TeleopSession provides the session). + +The user wires both into the pipeline via +:func:`isaaclab_teleop.tactile_helpers.build_default_openxr_controller_pipeline`. +""" + +from __future__ import annotations + +import logging +from typing import TYPE_CHECKING, Any, Iterable, Literal + +import numpy as np + +from isaacteleop.retargeting_engine.deviceio_source_nodes.interface import ( + IDeviceIOSource, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import ( + RetargeterIO, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.tensor_group_type import TensorGroupType +from isaacteleop.retargeting_engine.tensor_types import ControllerHapticPulse +from isaacteleop.retargeting_engine.tensor_types.scalar_types import BoolType + +from .interface import IHapticDevice + + +if TYPE_CHECKING: + from isaacteleop.deviceio_trackers import ControllerTracker, ITracker + + +logger = logging.getLogger(__name__) + + +_PendingPulse = tuple[float, float, float] +"""One queued pulse: ``(amplitude, frequency_hz, duration_s)``.""" + + +class OpenXRControllerHapticDevice(IHapticDevice): + """:class:`IHapticDevice` adapter for OpenXR motion-controller haptics. + + Consumes :func:`ControllerHapticPulse ` + (one ``(3,) float32`` per side: ``[amplitude, frequency_hz, duration_s]``). + ``frequency_hz == 0`` selects ``XR_FREQUENCY_UNSPECIFIED``; ``duration_s == 0`` + selects ``XR_MIN_HAPTIC_DURATION``; ``amplitude == 0`` triggers + ``xrStopHapticFeedback`` on the C++ side. + + ``apply()`` does not call OpenXR directly because the active session is + not in scope inside ``HapticSink._compute_fn`` (it lives on + :class:`~isaacteleop.teleop_session_manager.TeleopSession`, not on + individual retargeters). Instead it queues the latest pulse per side and + relies on the paired :class:`OpenXRControllerHapticSource` to drain the + queue inside ``poll_tracker(session)`` once per frame -- the same split + :class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.MessageChannelSink` + uses for outbound message-channel traffic. + + .. note:: + **One-frame haptic latency.** The :class:`OpenXRControllerHapticSource` + drains the queue at the *start* of each step (in ``poll_tracker``, + before the retargeting graph runs), while :meth:`apply` enqueues a + pulse from inside the graph. So a pulse produced in step *N* is sent + to the controller in step *N+1*'s pre-graph drain. At 60–90 Hz the + ~11–17 ms delay is below the perception threshold for vibration; for + higher-rate force-feedback paths (e.g. the planned Haply integration) + this ordering deserves a redesign. + """ + + def __init__( + self, + sides: Iterable[Literal["left", "right"]] = ("left", "right"), + ) -> None: + """Construct an OpenXR motion-controller haptic adapter. + + Args: + sides: Which sides to drive. Most motion controllers are paired + (default ``("left", "right")``), but some single-handed + controllers exist; restrict here to make + :meth:`supports` return ``False`` for the unused side. + """ + self._sides = set(sides) + # Only the most recent pulse per side per frame matters: an + # xrApplyHapticFeedback() call already supersedes any in-flight pulse + # on the same action, so coalescing here is correct, not lossy. + self._pending: dict[Literal["left", "right"], _PendingPulse] = {} + + def accepted_type(self) -> TensorGroupType: + return ControllerHapticPulse() + + def supports(self, side: Literal["left", "right"]) -> bool: + return side in self._sides + + def apply(self, side: Literal["left", "right"], values: np.ndarray) -> None: + arr = np.asarray(values, dtype=np.float32).ravel() + if arr.size != 3: + raise ValueError( + "OpenXRControllerHapticDevice.apply expects a 3-element " + "[amplitude, frequency_hz, duration_s] vector " + f"(ControllerHapticPulse), got shape {np.asarray(values).shape}" + ) + self._pending[side] = (float(arr[0]), float(arr[1]), float(arr[2])) + + def drain_pending(self) -> dict[Literal["left", "right"], _PendingPulse]: + """Return and clear the per-side pending pulses. + + Called once per frame by the paired + :class:`OpenXRControllerHapticSource` from inside ``poll_tracker``. + """ + pending, self._pending = self._pending, {} + return pending + + +class OpenXRControllerHapticSource(IDeviceIOSource): + """Drains :class:`OpenXRControllerHapticDevice`'s queue through an active session. + + This is the session-aware half of the OpenXR-controller haptic plumbing. + It is an :class:`IDeviceIOSource` so :class:`~isaacteleop.teleop_session_manager.TeleopSession` + auto-discovers it as a graph leaf, registers its underlying + :class:`~isaacteleop.deviceio_trackers.ControllerTracker` for OpenXR + extension aggregation, and calls ``poll_tracker(deviceio_session)`` once + per frame. ``poll_tracker`` drains the device queue and forwards the + pulses to ``controller_tracker.apply_haptic_feedback(session, side, ...)``. + + .. warning:: + **Discovery requires a declared output.** ``TeleopSession`` discovers + :class:`IDeviceIOSource` leaves by walking back from the outputs + declared on the user's :class:`~isaacteleop.retargeting_engine.interface.OutputCombiner`. + If you build a custom pipeline, you **must** include + :attr:`OpenXRControllerHapticSource.HEARTBEAT` (or any output of this + node) as one of the combiner's outputs, otherwise the source is never + polled, the tracker is never registered, and haptics silently never + fire. Use :func:`~isaaclab_teleop.tactile_helpers.build_default_openxr_controller_pipeline` + if you do not need a custom pipeline -- it wires the heartbeat for + you. + + .. warning:: + **Share the same** :class:`ControllerTracker` **instance with any + existing** :class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.ControllersSource`. + ``DeviceIOSession`` deduplicates trackers by raw pointer, so passing + two distinct ``ControllerTracker()`` instances will create two + ``LiveControllerTrackerImpl`` objects that both try to attach an + action set to the same ``XrSession`` -- the second attach raises + ``XR_ERROR_ACTIONSETS_ALREADY_ATTACHED`` and the session aborts. Use + :meth:`for_controllers_source` (below) or pass + ``controllers_source.get_tracker()`` explicitly to avoid this. + """ + + HEARTBEAT = "_openxr_haptic_heartbeat" + + def __init__( + self, + name: str, + device: OpenXRControllerHapticDevice, + controller_tracker: "ControllerTracker", + ) -> None: + self._device = device + self._controller_tracker = controller_tracker + # Logged-at-most-once per side so a missing C++ haptic method + # (e.g. running against an older TeleopCore build that has not + # been rebuilt with the haptic extension) does not flood the log. + self._error_logged: dict[str, bool] = {"left": False, "right": False} + super().__init__(name) + + @classmethod + def for_controllers_source( + cls, + name: str, + device: OpenXRControllerHapticDevice, + controllers_source: Any, + ) -> "OpenXRControllerHapticSource": + """Construct a haptic source that shares its tracker with a controllers source. + + Convenience wrapper around the constructor that fetches + ``controllers_source.get_tracker()`` for you, so the two sources + cannot accidentally diverge on which ``ControllerTracker`` instance + they hold. See the class docstring for why sharing matters. + + Args: + name: Unique pipeline node name. + device: The :class:`OpenXRControllerHapticDevice` whose queue this + source will drain. + controllers_source: The + :class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.ControllersSource` + already in the pipeline. Anything with a ``get_tracker()`` + method returning a ``ControllerTracker`` works (typed as + ``Any`` to avoid a circular import). + + Returns: + A new :class:`OpenXRControllerHapticSource` bound to the same + tracker as ``controllers_source``. + """ + return cls(name, device, controllers_source.get_tracker()) + + def input_spec(self) -> RetargeterIOType: + return {} + + def output_spec(self) -> RetargeterIOType: + # Heartbeat output exists purely so OutputCombiner can include this + # leaf in its graph traversal (TeleopSession discovers IDeviceIOSource + # leaves by walking back from declared OutputCombiner outputs). + # The helper in `isaaclab_teleop.tactile_helpers` wires it up. + return { + self.HEARTBEAT: TensorGroupType( + "_openxr_haptic_heartbeat", [BoolType("ok")] + ) + } + + def get_tracker(self) -> "ITracker": + return self._controller_tracker + + def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: + for side, ( + amplitude, + frequency_hz, + duration_s, + ) in self._device.drain_pending().items(): + try: + self._controller_tracker.apply_haptic_feedback( + deviceio_session, + side, + amplitude, + frequency_hz, + duration_s, + ) + except Exception as exc: + if not self._error_logged[side]: + logger.warning( + "OpenXRControllerHapticSource.poll_tracker(%s) failed " + "(will silence further errors for this side): %s", + side, + exc, + ) + self._error_logged[side] = True + # The IDeviceIOSource contract expects a dict matching input_spec; we + # have no inputs so an empty dict is correct. + return {} + + def _compute_fn( + self, + inputs: RetargeterIO, + outputs: RetargeterIO, + context: Any, + ) -> None: + outputs[self.HEARTBEAT][0] = True diff --git a/src/plugins/manus/CMakeLists.txt b/src/plugins/manus/CMakeLists.txt index c265db09c..434212297 100644 --- a/src/plugins/manus/CMakeLists.txt +++ b/src/plugins/manus/CMakeLists.txt @@ -107,6 +107,10 @@ add_subdirectory(core) add_library(isaac::manus_plugin ALIAS manus_plugin_core) add_subdirectory(app) add_subdirectory(tools) +# Python bindings — wraps ManusTracker's haptic API for +# isaacteleop.haptic_devices.manus.ManusHapticDevice. Inner CMakeLists is +# a no-op when BUILD_PYTHON_BINDINGS is off. +add_subdirectory(python) # Install Manus SDK shared library install(FILES "${_MANUS_LIB}" diff --git a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp index feb2d8ddc..a5e050105 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -105,6 +105,68 @@ std::vector ManusTracker::get_right_node_info() const return m_right_node_info; } +bool ManusTracker::supports_haptics(bool is_left) const +{ + 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()) + { + return false; + } + glove_id = *opt; + } + + bool out = false; + if (CoreSdk_DoesSkeletonGloveSupportHaptics(glove_id, &out) != SDKReturnCode::SDKReturnCode_Success) + { + return false; + } + return out; +} + +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) + { + clamped[i] = std::clamp(powers[i], 0.0f, 1.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); diff --git a/src/plugins/manus/inc/core/manus_hand_tracking_plugin.hpp b/src/plugins/manus/inc/core/manus_hand_tracking_plugin.hpp index 85cd95f43..063b2690b 100644 --- a/src/plugins/manus/inc/core/manus_hand_tracking_plugin.hpp +++ b/src/plugins/manus/inc/core/manus_hand_tracking_plugin.hpp @@ -13,6 +13,8 @@ #include #include +#include +#include #include #include #include @@ -41,6 +43,28 @@ 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. + /// + /// Bridges Isaac Teleop's haptic feedback flow (see + /// isaacteleop.haptic_devices.ManusHapticDevice) to + /// `CoreSdk_VibrateFingersForGlove`. Powers are interpreted in Manus + /// order [Thumb, Index, Middle, Ring, Pinky] and are clamped to [0, 1]. + /// + /// No-ops (and logs at most once per side) when: + /// - the glove for the requested side is not connected, or + /// - the connected glove reports no haptic support, or + /// - the SDK call itself returns a non-success code. + /// + /// Thread-safe — `landscape_mutex` guards the per-side glove id used to + /// look up the SDK target. + void apply_haptic_command(bool is_left, const std::array& powers); + + /// Whether the glove for the given side is currently connected and + /// reports support for haptic vibration (via + /// `CoreSdk_DoesSkeletonGloveSupportHaptics`). Returns false when no + /// glove is connected on that side. Thread-safe. + bool supports_haptics(bool is_left) const; + private: // Lifecycle explicit ManusTracker(const std::string& app_name) noexcept(false); @@ -78,11 +102,19 @@ 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 — kept separate from landscape_mutex so a vibration call + // does not contend with the landscape callback for the duration of the + // SDK call. 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; diff --git a/src/plugins/manus/python/CMakeLists.txt b/src/plugins/manus/python/CMakeLists.txt new file mode 100644 index 000000000..d1fb458d5 --- /dev/null +++ b/src/plugins/manus/python/CMakeLists.txt @@ -0,0 +1,52 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +# Python bindings for the Manus plugin's haptic surface. Built only when +# Python bindings are enabled; the Manus plugin's outer CMakeLists already +# returns early when the Manus SDK is missing, so reaching this file at all +# implies ManusSDK is available. +if(NOT BUILD_PYTHON_BINDINGS) + return() +endif() + +pybind11_add_module(manus_haptic_py + manus_haptic_bindings.cpp +) + +target_link_libraries(manus_haptic_py + PRIVATE + manus_plugin_core +) + +set_target_properties(manus_haptic_py PROPERTIES + OUTPUT_NAME "_manus_haptic" + # Place the .so next to the Python adapter under isaacteleop.haptic_devices + # in the wheel-staging tree so ManusHapticDevice can do + # `from . import _manus_haptic` without any PYTHONPATH gymnastics. This is + # also the path setuptools' package_data picks up when building the wheel. + LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/haptic_devices" + # RPATH spans both the wheel install and the cmake install (`make install`) + # layouts: + # - `$ORIGIN` covers the cmake install (this .so lands in + # `${CMAKE_INSTALL_LIBDIR}` next to libIsaacTeleopPluginsManus.so / + # libManusSDK*.so). + # - `$ORIGIN/../..` and `$ORIGIN/../../lib` are wheel-install attempts: + # when the wheel is installed into site-packages the .so lives at + # site-packages/isaacteleop/haptic_devices/, and `$ORIGIN/../..` is + # site-packages/. Whether libIsaacTeleopPluginsManus.so is reachable + # from there depends on how the wheel actually packages the plugin + # core (currently not bundled by package_data — verify before + # shipping a wheel). + BUILD_WITH_INSTALL_RPATH NO + SKIP_BUILD_RPATH NO + BUILD_RPATH "${_ISAAC_MANUS_EFFECTIVE_BUILD_RPATH_STR}" + INSTALL_RPATH "$ORIGIN;$ORIGIN/../..;$ORIGIN/../../lib" +) + +# cmake-install layout: the pybind module lands in lib/ next to the Manus +# plugin core. The wheel layout is handled separately via the package_data +# entry "isaacteleop.haptic_devices = [\"*.so\", ...]" in pyproject.toml.in, +# which picks up the LIBRARY_OUTPUT_DIRECTORY above. +install(TARGETS manus_haptic_py + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} COMPONENT manus +) diff --git a/src/plugins/manus/python/manus_haptic_bindings.cpp b/src/plugins/manus/python/manus_haptic_bindings.cpp new file mode 100644 index 000000000..daf072023 --- /dev/null +++ b/src/plugins/manus/python/manus_haptic_bindings.cpp @@ -0,0 +1,91 @@ +// SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +// SPDX-License-Identifier: Apache-2.0 + +// Pybind11 module exposing `ManusTracker`'s haptic surface to Python. +// +// The Python adapter (`isaacteleop.haptic_devices.manus.ManusHapticDevice`) +// lazy-imports this module as `isaacteleop.haptic_devices._manus_haptic`. +// Keeping the binding intentionally tiny — two free functions over the +// singleton — confines ManusSDK linkage to `src/plugins/manus/` per the +// repo's vendor-SDK boundary in AGENTS.md. + +#include + +#include +#include + +#include +#include +#include + +namespace py = pybind11; + +namespace +{ + +// Convert a Python "side" string to the bool flag ManusTracker uses internally. +// Matches the public contract of isaacteleop.haptic_devices.IHapticDevice. +bool side_string_to_is_left(const std::string& side) +{ + if (side == "left") + { + return true; + } + if (side == "right") + { + return false; + } + throw py::value_error("side must be \"left\" or \"right\", got \"" + side + "\""); +} + +// Coerce a NumPy array of any contiguous-enough layout into 5 float32 powers +// without copying when it is already (5,) float32 contiguous. +std::array powers_array_from_numpy(py::array_t arr) +{ + if (arr.ndim() != 1 || arr.shape(0) != 5) + { + throw py::value_error("powers must be a 1-D array of length 5 (Thumb, Index, Middle, Ring, Pinky)"); + } + std::array out{}; + const auto* data = static_cast(arr.data()); + for (size_t i = 0; i < 5; ++i) + { + out[i] = data[i]; + } + return out; +} + +} // namespace + +PYBIND11_MODULE(_manus_haptic, m) +{ + m.doc() = "Manus glove haptic output bindings (private, used by isaacteleop.haptic_devices.manus)."; + + m.def( + "apply_haptic_command", + [](const std::string& side, py::array_t powers) + { + const bool is_left = side_string_to_is_left(side); + const std::array powers_arr = powers_array_from_numpy(std::move(powers)); + // ManusTracker is a singleton initialised by the Manus hand-tracking + // plugin. Calling instance() here is safe: if the plugin has already + // been started, we get the existing instance; otherwise we fall + // through to the same lazy initialisation the plugin uses and the + // call simply no-ops until a glove is detected. + plugins::manus::ManusTracker::instance().apply_haptic_command(is_left, powers_arr); + }, + py::arg("side"), + py::arg("powers"), + "Vibrate the five finger motors of the glove on the given side.\n" + "side: 'left' or 'right'. powers: (5,) float32 in [0, 1], order Thumb/Index/Middle/Ring/Pinky."); + + m.def( + "supports_haptics", + [](const std::string& side) + { + const bool is_left = side_string_to_is_left(side); + return plugins::manus::ManusTracker::instance().supports_haptics(is_left); + }, + py::arg("side"), + "Whether the glove on the given side is connected and reports haptic support."); +} diff --git a/src/retargeters/tactile_retargeters.py b/src/retargeters/tactile_retargeters.py new file mode 100644 index 000000000..54ad5936b --- /dev/null +++ b/src/retargeters/tactile_retargeters.py @@ -0,0 +1,942 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Tactile / haptic retargeters. + +Two groups, both vendor-neutral: + +* **Composable spatial primitives** (:class:`Vector3FrameTransform`, + :class:`WorldForceAccumulator`, :class:`MagnitudeReducer`) operate on + sim-side :func:`~isaacteleop.retargeting_engine.tensor_types.TactileVector` + flows. They are the building blocks every per-device mapper composes with. + +* **Per-device mappers** (:class:`TactileVectorToFingerPower`, + :class:`TactileHeatmapToFingerPower`, :class:`TactileHeatmapToWristPulse`, + :class:`TactileVectorToControllerPulse`, + :class:`TactileHeatmapToControllerPulse`) translate sim-side tactile data + into one of the device-side schemas declared in + :mod:`~isaacteleop.retargeting_engine.tensor_types.tactile_types`. They are + named after the target schema, not the vendor -- any vendor whose device + accepts the same schema reuses the same mapper. + +When the Haply force-feedback adapter lands, ``ContactForceToEndEffectorForce`` +and ``TactileHeatmapToEndEffectorForce`` slot into this file with no +restructuring; they compose the same spatial primitives above. +""" + +from __future__ import annotations + +from typing import Literal + +import numpy as np + +from isaacteleop.retargeting_engine.interface import ( + BaseRetargeter, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.parameter_state import ParameterState +from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaacteleop.retargeting_engine.interface.tunable_parameter import ( + FloatParameter, + VectorParameter, +) +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerHapticPulse, + ControllerHapticPulseField, + FingerPowerVector, + NUM_CONTROLLER_HAPTIC_FIELDS, + NUM_HAPTIC_FINGERS, + TactileHeatmap, + TactileVector, + TransformMatrix, +) + + +# ============================================================================ +# Composable spatial primitives (vendor-neutral) +# ============================================================================ + + +class Vector3FrameTransform(BaseRetargeter): + """Rotate a sim-frame :func:`TactileVector(3)` into a new frame. + + .. note:: + **Rotation-only.** This node is force-vector oriented: ``TactileVector(3)`` + is treated as a *free vector* (e.g. a contact force or torque), + so the translation column of the input ``TransformMatrix`` is + intentionally ignored and only the upper-left 3x3 rotation block is + applied. To transform a *position* (where translation matters), use a + full 4x4 affine multiply at a layer that knows the input is a point, + not in this node. + + Inputs: + - ``"vec"``: :func:`TactileVector(3) ` + (a 3-vector free quantity) in the source frame. + - ``"transform"``: :func:`TransformMatrix ` + ``(4, 4)`` -- the source-to-target homogeneous transform; only the + rotation block ``M[:3, :3]`` is read. + + Outputs: + - ``"vec"``: :func:`TactileVector(3)` in the target frame. + + Used to compose Concern A (frame mismatch) of the tactile-haptic factoring + described in the design doc. The ``"transform"`` input is typically the + optional ``world_T_haptic`` :class:`~isaacteleop.retargeting_engine.interface.value_input.ValueInput` + leaf fed by IsaacTeleopDevice when ``IsaacTeleopHapticCfg.haptic_device_frame_prim_path`` + is set, but any pipeline node producing a ``TransformMatrix`` is valid. + """ + + INPUT_VEC = "vec" + INPUT_TRANSFORM = "transform" + OUTPUT_VEC = "vec" + + def input_spec(self) -> RetargeterIOType: + return { + self.INPUT_VEC: TactileVector(3), + self.INPUT_TRANSFORM: TransformMatrix(), + } + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_VEC: TactileVector(3)} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + vec = np.asarray(inputs[self.INPUT_VEC][0], dtype=np.float32).reshape(3) + matrix = np.asarray(inputs[self.INPUT_TRANSFORM][0], dtype=np.float32).reshape( + 4, 4 + ) + rotated = (matrix[:3, :3] @ vec).astype(np.float32) + outputs[self.OUTPUT_VEC][0] = rotated + + +class WorldForceAccumulator(BaseRetargeter): + """Sum N :func:`TactileVector(3)` inputs into one, with optional per-input weights. + + Inputs: + - ``"in_0"``, ``"in_1"``, ... ``"in_{num_inputs - 1}"``: each a + :func:`TactileVector(3) ` + in a common frame. + + Outputs: + - ``"vec"``: :func:`TactileVector(3)` = sum of ``weights[i] * in_i``. + + Tunable parameters: + - ``weights``: :class:`VectorParameter` of length ``num_inputs`` + (default: all ones). Exposed in the tuning UI so an operator can + attenuate or zero out individual contributing bodies live. + + Useful upstream of :class:`Vector3FrameTransform` to combine forces from + multiple sim contact sensors (e.g. left and right Franka fingers) before + rotating into the device frame. + """ + + OUTPUT_VEC = "vec" + + def __init__( + self, + name: str, + num_inputs: int, + weights: np.ndarray | list[float] | None = None, + ) -> None: + if num_inputs < 1: + raise ValueError( + f"WorldForceAccumulator '{name}' requires num_inputs >= 1, got {num_inputs}" + ) + self._num_inputs = num_inputs + + if weights is None: + default_weights = np.ones(num_inputs, dtype=np.float32) + else: + default_weights = np.asarray(weights, dtype=np.float32).reshape(-1) + if default_weights.shape[0] != num_inputs: + raise ValueError( + f"WorldForceAccumulator '{name}': weights length " + f"{default_weights.shape[0]} does not match num_inputs {num_inputs}" + ) + + # synced into self._weights before each compute by BaseRetargeter + self._weights: np.ndarray = default_weights.copy() + + param_state = ParameterState( + name, + [ + VectorParameter( + name="weights", + description="Per-input contribution weights (length num_inputs).", + element_names=[f"in_{i}" for i in range(num_inputs)], + default_value=default_weights, + sync_fn=lambda v: setattr( + self, "_weights", np.asarray(v, dtype=np.float32) + ), + ), + ], + ) + super().__init__(name=name, parameter_state=param_state) + + def input_spec(self) -> RetargeterIOType: + return {f"in_{i}": TactileVector(3) for i in range(self._num_inputs)} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_VEC: TactileVector(3)} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + accumulated = np.zeros(3, dtype=np.float32) + for i in range(self._num_inputs): + arr = np.asarray(inputs[f"in_{i}"][0], dtype=np.float32).reshape(3) + accumulated += float(self._weights[i]) * arr + outputs[self.OUTPUT_VEC][0] = accumulated + + +_MagnitudeMode = Literal["norm", "axis_x", "axis_y", "axis_z"] + + +class MagnitudeReducer(BaseRetargeter): + """Reduce a :func:`TactileVector(3)` to a :func:`TactileVector(1)` scalar. + + Inputs: + - ``"vec"``: :func:`TactileVector(3) `. + + Outputs: + - ``"scalar"``: :func:`TactileVector(1) ` + (the reduced magnitude). + + The reduction *mode* is fixed at construction time: + + * ``"norm"`` -- Euclidean length ``||vec||_2``. + * ``"axis_x"`` / ``"axis_y"`` / ``"axis_z"`` -- absolute value of the + corresponding component (use these after :class:`Vector3FrameTransform` + when the device cares about pressure normal to a known axis). + + Used to bridge directional contact data (a 3-vector force) into the + frame-invariant device schemas that only care about magnitude + (:func:`FingerPowerVector`, :func:`ControllerHapticPulse`). + + .. note:: + The design doc lists the original mode set as + ``{'norm', 'axis_z', 'dot_with_normal'}``. The ``axis_z`` mode is a + special case of the per-axis modes shipped here (``axis_x`` / + ``axis_y`` / ``axis_z``); ``dot_with_normal`` was deferred to a later + change because it can be expressed as a pre-rotation via + :class:`Vector3FrameTransform` plus ``axis_z``, and there is no v1 + consumer that needs it directly. + """ + + INPUT_VEC = "vec" + OUTPUT_SCALAR = "scalar" + + def __init__(self, name: str, mode: _MagnitudeMode = "norm") -> None: + if mode not in ("norm", "axis_x", "axis_y", "axis_z"): + raise ValueError( + f"MagnitudeReducer '{name}': unknown mode '{mode}'. " + "Must be one of: 'norm', 'axis_x', 'axis_y', 'axis_z'." + ) + self._mode = mode + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + return {self.INPUT_VEC: TactileVector(3)} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_SCALAR: TactileVector(1)} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + vec = np.asarray(inputs[self.INPUT_VEC][0], dtype=np.float32).reshape(3) + if self._mode == "norm": + scalar = float(np.linalg.norm(vec)) + elif self._mode == "axis_x": + scalar = float(abs(vec[0])) + elif self._mode == "axis_y": + scalar = float(abs(vec[1])) + else: # axis_z + scalar = float(abs(vec[2])) + outputs[self.OUTPUT_SCALAR][0] = np.array([scalar], dtype=np.float32) + + +# ============================================================================ +# Helpers shared across per-device mappers +# ============================================================================ + + +def _apply_gain_curve( + raw: np.ndarray, gain: float, deadband: float, saturation: float +) -> np.ndarray: + """Apply the standard gain / deadband / saturation curve, in [0, 1]. + + 1. Below ``deadband`` -> zero (suppresses noise). + 2. Above ``deadband`` -> ``gain * (raw - deadband)``. + 3. Clipped to ``[0, saturation]``. + + Always returns a non-negative float32 array of the input shape. + """ + raw = np.asarray(raw, dtype=np.float32) + deadbanded = np.maximum(0.0, raw - deadband) + scaled = gain * deadbanded + 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 Manus max). + - ``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 (Manus clamps 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 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 v1 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 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 +# ============================================================================ + + +class TactileVectorToControllerPulse(BaseRetargeter): + """Reduce a per-taxel :func:`TactileVector` to one :func:`ControllerHapticPulse`. + + Covers the canonical "G1 grip-pressure -> Quest controller rumble" case: + each taxel is the contact-force magnitude on one fingertip; one pulse per + hand summarises the contact state. + + Inputs: + - ``"tactile"``: :func:`TactileVector(num_taxels) `. + + Outputs: + - ``"pulse"``: :func:`ControllerHapticPulse ` + = ``[amplitude, frequency_hz, duration_s]``. + + The taxels are reduced to a single magnitude via ``reduction`` + (``"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. Both + default to ``0.0`` so every conformant OpenXR runtime is free to pick + ``XR_FREQUENCY_UNSPECIFIED`` and ``XR_MIN_HAPTIC_DURATION``. + + Tunable parameters: ``gain``, ``deadband``, ``saturation``, + ``frequency_hz``, ``duration_s``. + + .. note:: + Unlike :class:`TactileVectorToFingerPower`, this mapper has no + ``smoothing`` parameter. ``xrApplyHapticFeedback`` already supersedes + any in-flight pulse on the same action every frame, so the runtime + sees a fresh amplitude each step. EMA-smoothing the amplitude in + Python before that overwrite would just shift the latency without + meaningfully shaping the felt vibration, so the parameter is + intentionally omitted. Add an upstream low-pass retargeter on the + :class:`TactileVector` input if you need temporal shaping. + """ + + INPUT_TACTILE = "tactile" + OUTPUT_PULSE = "pulse" + + def __init__( + self, + name: str, + num_taxels: int, + 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 reduction not in ("max", "mean", "sum"): + raise ValueError( + f"TactileVectorToControllerPulse '{name}': unknown reduction '{reduction}'" + ) + self._num_taxels = num_taxels + 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="OpenXR pulse frequency [Hz]. 0 = XR_FREQUENCY_UNSPECIFIED.", + 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="OpenXR pulse duration [s]. 0 = XR_MIN_HAPTIC_DURATION.", + 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_TACTILE: TactileVector(self._num_taxels)} + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_PULSE: ControllerHapticPulse()} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + raw = np.asarray(inputs[self.INPUT_TACTILE][0], dtype=np.float32).reshape( + self._num_taxels + ) + + if self._reduction == "max": + scalar = float(raw.max()) if raw.size else 0.0 + elif self._reduction == "mean": + scalar = float(raw.mean()) if raw.size else 0.0 + else: # sum + scalar = float(raw.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 + + +class TactileHeatmapToControllerPulse(BaseRetargeter): + """Collapse a :func:`TactileHeatmap` to one :func:`ControllerHapticPulse`. + + Same tunables and semantics as :class:`TactileVectorToControllerPulse`; + the only difference is the input schema. The full + ``(num_pads, rows, cols)`` heatmap is reduced to one scalar via the + chosen ``reduction``. + + Like :class:`TactileVectorToControllerPulse`, this mapper has no + ``smoothing`` parameter on purpose -- see that class's note. + """ + + INPUT_HEATMAP = "heatmap" + OUTPUT_PULSE = "pulse" + + 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, + frequency_hz: float = 0.0, + duration_s: float = 0.0, + ) -> None: + if reduction not in ("max", "mean", "sum"): + raise ValueError( + f"TactileHeatmapToControllerPulse '{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._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="OpenXR pulse frequency [Hz]. 0 = XR_FREQUENCY_UNSPECIFIED.", + 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="OpenXR pulse duration [s]. 0 = XR_MIN_HAPTIC_DURATION.", + 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_HEATMAP: TactileHeatmap(self._rows, self._cols, self._num_pads) + } + + def output_spec(self) -> RetargeterIOType: + return {self.OUTPUT_PULSE: ControllerHapticPulse()} + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> 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()) + + 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 From 2a127dad71cd72dcb285e375932564250354743c Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Fri, 22 May 2026 11:15:46 -0400 Subject: [PATCH 2/5] Addressing feedback --- .../python/hand_pinch_haptic_example.py | 57 ++----- .../openxr_controller_haptic_example.py | 8 + .../cpp/live_controller_tracker_impl.cpp | 31 +++- .../python/test_tactile_retargeters.py | 99 ++++++++++++ src/retargeters/tactile_retargeters.py | 150 +++++++++++++++++- 5 files changed, 291 insertions(+), 54 deletions(-) diff --git a/examples/haptic_feedback/python/hand_pinch_haptic_example.py b/examples/haptic_feedback/python/hand_pinch_haptic_example.py index 523044c36..eb782b191 100644 --- a/examples/haptic_feedback/python/hand_pinch_haptic_example.py +++ b/examples/haptic_feedback/python/hand_pinch_haptic_example.py @@ -37,6 +37,7 @@ from __future__ import annotations import argparse +import math import sys import time from typing import Any @@ -48,6 +49,7 @@ OpenXRControllerHapticDevice, OpenXRControllerHapticSource, ) +from isaacteleop.retargeters.tactile_retargeters import FingerPowerToControllerPulse from isaacteleop.retargeting_engine.deviceio_source_nodes import ( ControllersSource, HandsSource, @@ -64,14 +66,12 @@ ) from isaacteleop.retargeting_engine.interface.tensor_group_type import OptionalType from isaacteleop.retargeting_engine.tensor_types import ( - ControllerHapticPulse, ControllerHapticPulseField, FingerIndex, FingerPowerVector, HandInput, HandInputIndex, HandJointIndex, - NUM_CONTROLLER_HAPTIC_FIELDS, NUM_HAPTIC_FINGERS, ) from isaacteleop.teleop_session_manager import TeleopSession, TeleopSessionConfig @@ -204,51 +204,6 @@ def _compute_fn( outputs[self.OUTPUT_POWERS][0] = powers -class FingerPowerToControllerPulse(BaseRetargeter): - """Collapse a :func:`FingerPowerVector` to one :func:`ControllerHapticPulse`. - - Single-channel adapter for users routing pinch feedback to an OpenXR - motion controller (one motor per hand). Takes the max across fingers -- - matches "the most-pinched finger drives the rumble" intuition. - - Pure shim, no tunables -- frequency / duration come from the constructor - so the controller-side pulse parameters are still configurable but the - amplitude is unmodified from the FingerPowerVector input. - """ - - INPUT_POWERS = "powers" - OUTPUT_PULSE = "pulse" - - def __init__( - self, - name: str, - frequency_hz: float = 0.0, - duration_s: float = 0.0, - num_fingers: int = NUM_HAPTIC_FINGERS, - ) -> None: - self._frequency_hz = float(frequency_hz) - self._duration_s = float(duration_s) - self._num_fingers = int(num_fingers) - super().__init__(name=name) - - 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: ComputeContext - ) -> None: - powers = np.asarray(inputs[self.INPUT_POWERS][0], dtype=np.float32) - amplitude = float(powers.max()) if powers.size else 0.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 - - # ============================================================================ # Pipeline builders # ============================================================================ @@ -438,6 +393,11 @@ def _positive_float(value: str) -> float: n = float(value) except ValueError: raise argparse.ArgumentTypeError(f"expected a positive float, got {value!r}") + # Reject nan / inf before the range check: `nan <= 0.0` is False, so a bare + # range check would silently accept non-finite values and leak them into + # the pinch-distance / haptic-pulse parameters. + if not math.isfinite(n): + raise argparse.ArgumentTypeError(f"must be finite, got {n}") if n <= 0.0: raise argparse.ArgumentTypeError(f"must be > 0, got {n}") return n @@ -448,12 +408,15 @@ def _non_negative_float(value: str) -> float: n = float(value) except ValueError: raise argparse.ArgumentTypeError(f"expected a number, got {value!r}") + if not math.isfinite(n): + raise argparse.ArgumentTypeError(f"must be finite, got {n}") if n < 0.0: raise argparse.ArgumentTypeError(f"must be >= 0, got {n}") return n def _unit_float(value: str) -> float: + # _non_negative_float rejects nan/inf, so this stays in [0, 1]. n = _non_negative_float(value) if n > 1.0: raise argparse.ArgumentTypeError(f"must be in [0, 1], got {n}") diff --git a/examples/haptic_feedback/python/openxr_controller_haptic_example.py b/examples/haptic_feedback/python/openxr_controller_haptic_example.py index 42133a083..925e04e2f 100644 --- a/examples/haptic_feedback/python/openxr_controller_haptic_example.py +++ b/examples/haptic_feedback/python/openxr_controller_haptic_example.py @@ -298,6 +298,11 @@ def _positive_float(value: str) -> float: n = float(value) except ValueError: raise argparse.ArgumentTypeError(f"expected a positive float, got {value!r}") + # Reject nan / inf before the range check: `nan <= 0.0` is False, so a bare + # range check would silently accept non-finite amplitudes / durations and + # leak them into the haptic pipeline. + if not math.isfinite(n): + raise argparse.ArgumentTypeError(f"must be finite, got {n}") if n <= 0.0: raise argparse.ArgumentTypeError(f"must be > 0, got {n}") return n @@ -308,12 +313,15 @@ def _non_negative_float(value: str) -> float: n = float(value) except ValueError: raise argparse.ArgumentTypeError(f"expected a number, got {value!r}") + if not math.isfinite(n): + raise argparse.ArgumentTypeError(f"must be finite, got {n}") if n < 0.0: raise argparse.ArgumentTypeError(f"must be >= 0, got {n}") return n def _unit_float(value: str) -> float: + # _non_negative_float rejects nan/inf, so this stays in [0, 1]. n = _non_negative_float(value) if n > 1.0: raise argparse.ArgumentTypeError(f"must be in [0, 1], got {n}") diff --git a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp index 8ef6d1694..c1618f826 100644 --- a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp @@ -471,13 +471,27 @@ void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, const size_t slot = is_left ? 0 : 1; const char* const side_name = is_left ? "left" : "right"; + // Sanitize non-finite inputs (NaN / +-Inf) up front: + // * std::clamp does not sanitize NaN (NaN-vs-bound comparisons are + // unordered, so NaN passes through unchanged); + // * `nan <= 0.0f` is false, so NaN amplitude / duration would skip the + // "explicit stop" / "runtime default" branches below; and + // * static_cast(NaN or out-of-range Inf) is undefined + // behaviour per [conv.fpint] (XrDuration is int64). + // Mapping non-finite to zero routes them through the existing zero-handling + // paths (amplitude=0 -> stop, duration_s=0 -> XR_MIN_HAPTIC_DURATION, + // frequency_hz=0 -> XR_FREQUENCY_UNSPECIFIED). + const float safe_amplitude = std::isfinite(amplitude) ? amplitude : 0.0f; + const float safe_duration_s = std::isfinite(duration_s) ? duration_s : 0.0f; + const float safe_frequency_hz = std::isfinite(frequency_hz) ? frequency_hz : 0.0f; + XrHapticActionInfo info{ XR_TYPE_HAPTIC_ACTION_INFO }; info.action = haptic_action_; info.subactionPath = subaction_path; // amplitude==0 maps to an explicit stop instead of a zero-amplitude pulse, // so dropping below the upstream deadband cleanly aborts an in-flight rumble. - if (amplitude <= 0.0f) + if (safe_amplitude <= 0.0f) { if (core_funcs_.xrStopHapticFeedback == nullptr) { @@ -508,14 +522,21 @@ void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, } XrHapticVibration vibration{ XR_TYPE_HAPTIC_VIBRATION }; - vibration.amplitude = std::clamp(amplitude, 0.0f, 1.0f); + vibration.amplitude = std::clamp(safe_amplitude, 0.0f, 1.0f); // duration_s == 0 -> shortest pulse the runtime supports. Use double for // the seconds->nanoseconds conversion: float has ~7 decimal digits and - // can lose tens of microseconds for multi-second pulses. + // can lose tens of microseconds for multi-second pulses. Clamp the + // converted nanoseconds to a safe range that round-trips to int64 without + // overflow (real haptic pulses are sub-second, so 1e18 ns ~= 31 years is a + // generous upper bound that stays well below INT64_MAX ~= 9.22e18). + constexpr double k_max_duration_ns = 1.0e18; vibration.duration = - (duration_s <= 0.0f) ? XR_MIN_HAPTIC_DURATION : static_cast(static_cast(duration_s) * 1.0e9); + (safe_duration_s <= 0.0f) + ? XR_MIN_HAPTIC_DURATION + : static_cast( + std::clamp(static_cast(safe_duration_s) * 1.0e9, 0.0, k_max_duration_ns)); // frequency_hz == 0 -> let the runtime pick. - vibration.frequency = (frequency_hz <= 0.0f) ? XR_FREQUENCY_UNSPECIFIED : frequency_hz; + vibration.frequency = (safe_frequency_hz <= 0.0f) ? XR_FREQUENCY_UNSPECIFIED : safe_frequency_hz; const XrResult apply_result = core_funcs_.xrApplyHapticFeedback( session_, &info, reinterpret_cast(&vibration)); 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 62daafdd5..ab69172f2 100644 --- a/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py +++ b/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py @@ -19,6 +19,7 @@ import pytest from isaacteleop.retargeters.tactile_retargeters import ( + FingerPowerToControllerPulse, MagnitudeReducer, TactileHeatmapToControllerPulse, TactileHeatmapToFingerPower, @@ -318,6 +319,104 @@ def test_deadband_zeros_amplitude(self) -> None: ) +class TestFingerPowerToControllerPulse: + """``FingerPowerToControllerPulse`` collapses an already-reduced + ``FingerPowerVector`` (per-finger glove output) to a single controller + pulse. Bridges glove-style pipelines to single-channel motor rumble; this + is the public version of the helper that used to live in the hand-pinch + example.""" + + def test_max_reduction_picks_strongest_finger(self) -> None: + node = FingerPowerToControllerPulse( + "finger_power_to_pulse", + num_fingers=NUM_HAPTIC_FINGERS, + reduction="max", + frequency_hz=120.0, + duration_s=0.05, + ) + outputs = _run(node, {"powers": [0.1, 0.4, 0.9, 0.2, 0.3]}) + pulse = np.asarray(outputs["pulse"][0]) + assert pulse[ControllerHapticPulseField.AMPLITUDE] == pytest.approx(0.9) + assert pulse[ControllerHapticPulseField.FREQUENCY_HZ] == pytest.approx(120.0) + assert pulse[ControllerHapticPulseField.DURATION_S] == pytest.approx(0.05) + + def test_mean_reduction_averages_fingers(self) -> None: + node = FingerPowerToControllerPulse( + "finger_power_to_pulse", + num_fingers=NUM_HAPTIC_FINGERS, + reduction="mean", + ) + outputs = _run(node, {"powers": [0.1, 0.2, 0.3, 0.4, 0.5]}) + pulse = np.asarray(outputs["pulse"][0]) + assert pulse[ControllerHapticPulseField.AMPLITUDE] == pytest.approx(0.3) + + def test_zero_frequency_and_duration_pass_through(self) -> None: + """Defaults of 0/0 round-trip exactly so the C++ side can map them + to ``XR_FREQUENCY_UNSPECIFIED`` / ``XR_MIN_HAPTIC_DURATION``.""" + node = FingerPowerToControllerPulse( + "finger_power_to_pulse", num_fingers=NUM_HAPTIC_FINGERS + ) + outputs = _run(node, {"powers": [0.0, 0.0, 0.5, 0.0, 0.0]}) + pulse = np.asarray(outputs["pulse"][0]) + assert pulse[ControllerHapticPulseField.FREQUENCY_HZ] == 0.0 + assert pulse[ControllerHapticPulseField.DURATION_S] == 0.0 + + def test_deadband_suppresses_weak_signal(self) -> None: + """Lets a custom controller pulse stay quiet under a per-finger + threshold even though the upstream FingerPowerVector is non-zero.""" + node = FingerPowerToControllerPulse( + "finger_power_to_pulse", + num_fingers=NUM_HAPTIC_FINGERS, + deadband=0.5, + ) + outputs = _run(node, {"powers": [0.1, 0.2, 0.3, 0.0, 0.0]}) + assert ( + float(np.asarray(outputs["pulse"][0])[ControllerHapticPulseField.AMPLITUDE]) + == 0.0 + ) + + def test_gain_scales_post_deadband(self) -> None: + """Operator can boost rumble independently of the upstream amplitude.""" + node = FingerPowerToControllerPulse( + "finger_power_to_pulse", + num_fingers=NUM_HAPTIC_FINGERS, + deadband=0.1, + gain=5.0, + saturation=1.0, + ) + outputs = _run(node, {"powers": [0.0, 0.0, 0.3, 0.0, 0.0]}) + amp = float( + np.asarray(outputs["pulse"][0])[ControllerHapticPulseField.AMPLITUDE] + ) + # raw = 0.3, deadband -> 0.2, gain*0.2 = 1.0, clamped to saturation=1.0. + assert amp == pytest.approx(1.0) + + def test_saturation_caps_amplitude(self) -> None: + node = FingerPowerToControllerPulse( + "finger_power_to_pulse", + num_fingers=NUM_HAPTIC_FINGERS, + gain=10.0, + saturation=0.4, + ) + outputs = _run(node, {"powers": [0.0, 0.0, 1.0, 0.0, 0.0]}) + amp = float( + np.asarray(outputs["pulse"][0])[ControllerHapticPulseField.AMPLITUDE] + ) + assert amp == pytest.approx(0.4) + + def test_rejects_zero_fingers(self) -> None: + with pytest.raises(ValueError, match="num_fingers"): + FingerPowerToControllerPulse("finger_power_to_pulse", num_fingers=0) + + def test_rejects_unknown_reduction(self) -> None: + with pytest.raises(ValueError, match="unknown reduction"): + FingerPowerToControllerPulse( + "finger_power_to_pulse", + num_fingers=NUM_HAPTIC_FINGERS, + reduction="median", # type: ignore[arg-type] + ) + + class TestHeatmapMappers: def test_heatmap_to_finger_power_max_reduction(self) -> None: node = TactileHeatmapToFingerPower("heat_finger", rows=2, cols=2) diff --git a/src/retargeters/tactile_retargeters.py b/src/retargeters/tactile_retargeters.py index 54ad5936b..f4893a9d5 100644 --- a/src/retargeters/tactile_retargeters.py +++ b/src/retargeters/tactile_retargeters.py @@ -14,8 +14,10 @@ * **Per-device mappers** (:class:`TactileVectorToFingerPower`, :class:`TactileHeatmapToFingerPower`, :class:`TactileHeatmapToWristPulse`, :class:`TactileVectorToControllerPulse`, - :class:`TactileHeatmapToControllerPulse`) translate sim-side tactile data - into one of the device-side schemas declared in + :class:`TactileHeatmapToControllerPulse`, + :class:`FingerPowerToControllerPulse`) translate sim-side tactile data -- + or already-reduced device-side per-finger power -- into one of the + device-side schemas declared in :mod:`~isaacteleop.retargeting_engine.tensor_types.tactile_types`. They are named after the target schema, not the vendor -- any vendor whose device accepts the same schema reuses the same mapper. @@ -940,3 +942,147 @@ 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-style output to single-channel controller rumble: + given an already-reduced device-side + :func:`FingerPowerVector(num_fingers) ` + in ``[0, 1]`` (e.g. the output of :class:`TactileVectorToFingerPower` or + of an upstream pinch-proximity retargeter), this collapses the channels + to a single amplitude with the chosen ``reduction`` and pairs it with + constant ``frequency_hz`` / ``duration_s`` parameters. + + The same gain / deadband / saturation curve as + :class:`TactileVectorToControllerPulse` is applied so the controller + pulse can be tuned independently of the upstream per-finger signal -- the + operator can scale the rumble up, suppress weak signals, or cap the + saturation without touching the per-finger pipeline. + + Inputs: + - ``"powers"``: :func:`FingerPowerVector(num_fingers) `. + + Outputs: + - ``"pulse"``: :func:`ControllerHapticPulse ` + = ``[amplitude, frequency_hz, duration_s]``. + + Tunable parameters: ``gain``, ``deadband``, ``saturation``, + ``frequency_hz``, ``duration_s``. + + See :class:`TactileVectorToControllerPulse`'s note for why this mapper has + no ``smoothing`` parameter. + """ + + 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="OpenXR pulse frequency [Hz]. 0 = XR_FREQUENCY_UNSPECIFIED.", + 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="OpenXR pulse duration [s]. 0 = XR_MIN_HAPTIC_DURATION.", + 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 From 74e3589a894acbb834faaeec4c4263e11b58b773 Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Fri, 22 May 2026 11:23:49 -0400 Subject: [PATCH 3/5] running clang format --- src/core/AGENTS.md | 1 + .../cpp/controller_tracker.cpp | 7 ++---- .../deviceio_trackers/controller_tracker.hpp | 7 ++---- .../cpp/live_controller_tracker_impl.cpp | 24 ++++++++----------- .../oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp | 8 +++---- .../cpp/replay_controller_tracker_impl.hpp | 5 +--- .../manus/core/manus_hand_tracking_plugin.cpp | 5 ++-- .../manus/python/manus_haptic_bindings.cpp | 7 ++---- 8 files changed, 24 insertions(+), 40 deletions(-) diff --git a/src/core/AGENTS.md b/src/core/AGENTS.md index 525e02edb..48d2cb13d 100644 --- a/src/core/AGENTS.md +++ b/src/core/AGENTS.md @@ -19,3 +19,4 @@ If work under **`src/core/`** went wrong—**user** correction, **pre-commit/CI* - Preserve existing `TeleopSession` lifecycle flag semantics unless changing the public/context-manager contract intentionally; use tests to lock down cleanup details before altering them. - After Python test or session-manager edits, let `ruff format`/pre-commit own wrapping and rerun the hook when it modifies files. - **Sink/source `IDeviceIOSource` leaves are only discovered when reachable from a declared `OutputCombiner` output.** `TeleopSession._discover_sources` calls `pipeline.get_leaf_nodes()`, which walks back from the combiner's outputs. A sink/source whose only purpose is a side effect (haptic output, message-channel send) must therefore expose at least one output (a heartbeat boolean is the established pattern) **and** the user's combiner must include it. Every new sink/source of this shape needs both an explicit heartbeat output and a docstring warning that custom combiners must wire it up — silent no-discovery is the recurring footgun. +- **Run `clang-format -i` on touched C++ files before pushing.** The repo's `pre-commit` config runs `ruff` for Python but does **not** run `clang-format`; CI (`build-ubuntu.yml`) installs `clang-format-14` and rejects unformatted C++ as `-Wclang-format-violations`. Format locally with the system `clang-format` (matches CI's version 14) — e.g. `clang-format -i $(git diff --name-only main -- '*.cpp' '*.hpp' '*.h' '*.cc')` — and verify with `clang-format --dry-run --Werror `. diff --git a/src/core/deviceio_trackers/cpp/controller_tracker.cpp b/src/core/deviceio_trackers/cpp/controller_tracker.cpp index d3fc21a3f..68fd2e88d 100644 --- a/src/core/deviceio_trackers/cpp/controller_tracker.cpp +++ b/src/core/deviceio_trackers/cpp/controller_tracker.cpp @@ -20,11 +20,8 @@ const ControllerSnapshotTrackedT& ControllerTracker::get_right_controller(const return static_cast(session.get_tracker_impl(*this)).get_right_controller(); } -void ControllerTracker::apply_haptic_feedback(const ITrackerSession& session, - bool is_left, - float amplitude, - float frequency_hz, - float duration_s) const +void ControllerTracker::apply_haptic_feedback( + const ITrackerSession& session, bool is_left, float amplitude, float frequency_hz, float duration_s) const { static_cast(session.get_tracker_impl(*this)) .apply_haptic_feedback(is_left, amplitude, frequency_hz, duration_s); diff --git a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp index 8bc6fab67..ffb3ca9a8 100644 --- a/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp +++ b/src/core/deviceio_trackers/cpp/inc/deviceio_trackers/controller_tracker.hpp @@ -39,11 +39,8 @@ class ControllerTracker : public ITracker /// shortest supported pulse. See the base interface /// (:class:`IControllerTrackerImpl::apply_haptic_feedback`) for the full /// contract. - void apply_haptic_feedback(const ITrackerSession& session, - bool is_left, - float amplitude, - float frequency_hz, - float duration_s) const; + void apply_haptic_feedback( + const ITrackerSession& session, bool is_left, float amplitude, float frequency_hz, float duration_s) const; private: static constexpr const char* TRACKER_NAME = "ControllerTracker"; diff --git a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp index c1618f826..430418739 100644 --- a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp @@ -460,10 +460,7 @@ const ControllerSnapshotTrackedT& LiveControllerTrackerImpl::get_right_controlle return right_tracked_; } -void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, - float amplitude, - float frequency_hz, - float duration_s) const +void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, float amplitude, float frequency_hz, float duration_s) const { // The haptic action was created against both subaction paths during init, // so targeting one side is a matter of choosing the matching path here. @@ -505,8 +502,8 @@ void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, bool expected = false; if (stop_haptic_error_logged_[slot].compare_exchange_strong(expected, true)) { - std::cerr << "[ControllerTracker] xrStopHapticFeedback(" << side_name << ") failed: " - << static_cast(stop_result) + std::cerr << "[ControllerTracker] xrStopHapticFeedback(" << side_name + << ") failed: " << static_cast(stop_result) << "; further errors for this side will be silenced." << std::endl; } } @@ -531,22 +528,21 @@ void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, // generous upper bound that stays well below INT64_MAX ~= 9.22e18). constexpr double k_max_duration_ns = 1.0e18; vibration.duration = - (safe_duration_s <= 0.0f) - ? XR_MIN_HAPTIC_DURATION - : static_cast( - std::clamp(static_cast(safe_duration_s) * 1.0e9, 0.0, k_max_duration_ns)); + (safe_duration_s <= 0.0f) ? + XR_MIN_HAPTIC_DURATION : + static_cast(std::clamp(static_cast(safe_duration_s) * 1.0e9, 0.0, k_max_duration_ns)); // frequency_hz == 0 -> let the runtime pick. vibration.frequency = (safe_frequency_hz <= 0.0f) ? XR_FREQUENCY_UNSPECIFIED : safe_frequency_hz; - const XrResult apply_result = core_funcs_.xrApplyHapticFeedback( - session_, &info, reinterpret_cast(&vibration)); + const XrResult apply_result = + core_funcs_.xrApplyHapticFeedback(session_, &info, reinterpret_cast(&vibration)); if (XR_FAILED(apply_result)) { bool expected = false; if (apply_haptic_error_logged_[slot].compare_exchange_strong(expected, true)) { - std::cerr << "[ControllerTracker] xrApplyHapticFeedback(" << side_name << ") failed: " - << static_cast(apply_result) + std::cerr << "[ControllerTracker] xrApplyHapticFeedback(" << side_name + << ") failed: " << static_cast(apply_result) << "; further errors for this side will be silenced." << std::endl; } } diff --git a/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp b/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp index 655de6d03..3a3776536 100644 --- a/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp +++ b/src/core/oxr_utils/cpp/inc/oxr_utils/oxr_funcs.hpp @@ -109,10 +109,10 @@ struct OpenXRCoreFunctions // Haptic output (optional — tracker haptic methods check for null pointers // and silently no-op when the runtime does not advertise them). - getProcAddr(instance, "xrApplyHapticFeedback", - reinterpret_cast(&results.xrApplyHapticFeedback)); - getProcAddr(instance, "xrStopHapticFeedback", - reinterpret_cast(&results.xrStopHapticFeedback)); + getProcAddr( + instance, "xrApplyHapticFeedback", reinterpret_cast(&results.xrApplyHapticFeedback)); + getProcAddr( + instance, "xrStopHapticFeedback", reinterpret_cast(&results.xrStopHapticFeedback)); return results; } diff --git a/src/core/replay_trackers/cpp/replay_controller_tracker_impl.hpp b/src/core/replay_trackers/cpp/replay_controller_tracker_impl.hpp index eeb857b04..386235e32 100644 --- a/src/core/replay_trackers/cpp/replay_controller_tracker_impl.hpp +++ b/src/core/replay_trackers/cpp/replay_controller_tracker_impl.hpp @@ -30,10 +30,7 @@ class ReplayControllerTrackerImpl : public IControllerTrackerImpl const ControllerSnapshotTrackedT& get_left_controller() const override; const ControllerSnapshotTrackedT& get_right_controller() const override; // Replay sessions do not drive hardware — haptic feedback is a no-op here. - void apply_haptic_feedback(bool /*is_left*/, - float /*amplitude*/, - float /*frequency_hz*/, - float /*duration_s*/) const override + void apply_haptic_feedback(bool /*is_left*/, float /*amplitude*/, float /*frequency_hz*/, float /*duration_s*/) const override { } diff --git a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp index a5e050105..f847b3489 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -159,9 +159,8 @@ void ManusTracker::apply_haptic_command(bool is_left, const std::array 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) + 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; } } diff --git a/src/plugins/manus/python/manus_haptic_bindings.cpp b/src/plugins/manus/python/manus_haptic_bindings.cpp index daf072023..377763493 100644 --- a/src/plugins/manus/python/manus_haptic_bindings.cpp +++ b/src/plugins/manus/python/manus_haptic_bindings.cpp @@ -10,7 +10,6 @@ // repo's vendor-SDK boundary in AGENTS.md. #include - #include #include @@ -74,8 +73,7 @@ PYBIND11_MODULE(_manus_haptic, m) // call simply no-ops until a glove is detected. plugins::manus::ManusTracker::instance().apply_haptic_command(is_left, powers_arr); }, - py::arg("side"), - py::arg("powers"), + py::arg("side"), py::arg("powers"), "Vibrate the five finger motors of the glove on the given side.\n" "side: 'left' or 'right'. powers: (5,) float32 in [0, 1], order Thumb/Index/Middle/Ring/Pinky."); @@ -86,6 +84,5 @@ PYBIND11_MODULE(_manus_haptic, m) const bool is_left = side_string_to_is_left(side); return plugins::manus::ManusTracker::instance().supports_haptics(is_left); }, - py::arg("side"), - "Whether the glove on the given side is connected and reports haptic support."); + py::arg("side"), "Whether the glove on the given side is connected and reports haptic support."); } From 4de8dd81db8d3f50c79f918ce36698b6ed0f1aac Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Fri, 22 May 2026 12:11:42 -0400 Subject: [PATCH 4/5] Cleaning up comments and CI failures --- .../python/hand_pinch_haptic_example.py | 5 +- .../openxr_controller_haptic_example.py | 5 +- .../cpp/live_controller_tracker_impl.cpp | 35 +--- .../deviceio_source_nodes/haptic_sink.py | 77 +++----- .../python/tensor_types/tactile_types.py | 38 ++-- src/haptic_devices/__init__.py | 19 +- src/haptic_devices/interface.py | 82 ++------ src/haptic_devices/manus.py | 64 ++----- src/haptic_devices/openxr_controller.py | 176 ++++++------------ src/retargeters/tactile_retargeters.py | 174 +++++------------ 10 files changed, 188 insertions(+), 487 deletions(-) diff --git a/examples/haptic_feedback/python/hand_pinch_haptic_example.py b/examples/haptic_feedback/python/hand_pinch_haptic_example.py index eb782b191..5e95ae545 100644 --- a/examples/haptic_feedback/python/hand_pinch_haptic_example.py +++ b/examples/haptic_feedback/python/hand_pinch_haptic_example.py @@ -393,9 +393,7 @@ def _positive_float(value: str) -> float: n = float(value) except ValueError: raise argparse.ArgumentTypeError(f"expected a positive float, got {value!r}") - # Reject nan / inf before the range check: `nan <= 0.0` is False, so a bare - # range check would silently accept non-finite values and leak them into - # the pinch-distance / haptic-pulse parameters. + # nan/inf must be rejected before the range check (nan <= 0.0 is False). if not math.isfinite(n): raise argparse.ArgumentTypeError(f"must be finite, got {n}") if n <= 0.0: @@ -416,7 +414,6 @@ def _non_negative_float(value: str) -> float: def _unit_float(value: str) -> float: - # _non_negative_float rejects nan/inf, so this stays in [0, 1]. n = _non_negative_float(value) if n > 1.0: raise argparse.ArgumentTypeError(f"must be in [0, 1], got {n}") diff --git a/examples/haptic_feedback/python/openxr_controller_haptic_example.py b/examples/haptic_feedback/python/openxr_controller_haptic_example.py index 925e04e2f..35508f9e5 100644 --- a/examples/haptic_feedback/python/openxr_controller_haptic_example.py +++ b/examples/haptic_feedback/python/openxr_controller_haptic_example.py @@ -298,9 +298,7 @@ def _positive_float(value: str) -> float: n = float(value) except ValueError: raise argparse.ArgumentTypeError(f"expected a positive float, got {value!r}") - # Reject nan / inf before the range check: `nan <= 0.0` is False, so a bare - # range check would silently accept non-finite amplitudes / durations and - # leak them into the haptic pipeline. + # nan/inf must be rejected before the range check (nan <= 0.0 is False). if not math.isfinite(n): raise argparse.ArgumentTypeError(f"must be finite, got {n}") if n <= 0.0: @@ -321,7 +319,6 @@ def _non_negative_float(value: str) -> float: def _unit_float(value: str) -> float: - # _non_negative_float rejects nan/inf, so this stays in [0, 1]. n = _non_negative_float(value) if n > 1.0: raise argparse.ArgumentTypeError(f"must be in [0, 1], got {n}") diff --git a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp index 430418739..08e6fb3fe 100644 --- a/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp +++ b/src/core/live_trackers/cpp/live_controller_tracker_impl.cpp @@ -462,22 +462,14 @@ const ControllerSnapshotTrackedT& LiveControllerTrackerImpl::get_right_controlle void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, float amplitude, float frequency_hz, float duration_s) const { - // The haptic action was created against both subaction paths during init, - // so targeting one side is a matter of choosing the matching path here. const XrPath subaction_path = is_left ? left_hand_path_ : right_hand_path_; const size_t slot = is_left ? 0 : 1; const char* const side_name = is_left ? "left" : "right"; - // Sanitize non-finite inputs (NaN / +-Inf) up front: - // * std::clamp does not sanitize NaN (NaN-vs-bound comparisons are - // unordered, so NaN passes through unchanged); - // * `nan <= 0.0f` is false, so NaN amplitude / duration would skip the - // "explicit stop" / "runtime default" branches below; and - // * static_cast(NaN or out-of-range Inf) is undefined - // behaviour per [conv.fpint] (XrDuration is int64). - // Mapping non-finite to zero routes them through the existing zero-handling - // paths (amplitude=0 -> stop, duration_s=0 -> XR_MIN_HAPTIC_DURATION, - // frequency_hz=0 -> XR_FREQUENCY_UNSPECIFIED). + // Map non-finite inputs (NaN / +-Inf) to zero so they hit the explicit-stop + // / runtime-default branches below. Without this, std::clamp leaves NaN + // unchanged (unordered comparisons) and static_cast(NaN/Inf) + // is UB per [conv.fpint] (XrDuration is int64). const float safe_amplitude = std::isfinite(amplitude) ? amplitude : 0.0f; const float safe_duration_s = std::isfinite(duration_s) ? duration_s : 0.0f; const float safe_frequency_hz = std::isfinite(frequency_hz) ? frequency_hz : 0.0f; @@ -486,8 +478,8 @@ void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, float amplit info.action = haptic_action_; info.subactionPath = subaction_path; - // amplitude==0 maps to an explicit stop instead of a zero-amplitude pulse, - // so dropping below the upstream deadband cleanly aborts an in-flight rumble. + // amplitude==0 issues an explicit stop so an in-flight rumble aborts when + // the upstream deadband closes. if (safe_amplitude <= 0.0f) { if (core_funcs_.xrStopHapticFeedback == nullptr) @@ -497,8 +489,6 @@ void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, float amplit const XrResult stop_result = core_funcs_.xrStopHapticFeedback(session_, &info); if (XR_FAILED(stop_result)) { - // Don't tear the session down on transient failures, but log the - // first occurrence per side so chronic problems are not invisible. bool expected = false; if (stop_haptic_error_logged_[slot].compare_exchange_strong(expected, true)) { @@ -512,26 +502,19 @@ void LiveControllerTrackerImpl::apply_haptic_feedback(bool is_left, float amplit if (core_funcs_.xrApplyHapticFeedback == nullptr) { - // Runtime does not advertise the haptic entry point — silently no-op. - // This keeps haptic feedback a nice-to-have rather than a session-fatal - // dependency for runtimes that omit it (e.g. some headless test rigs). + // Runtime does not advertise the entry point — silently no-op. return; } XrHapticVibration vibration{ XR_TYPE_HAPTIC_VIBRATION }; vibration.amplitude = std::clamp(safe_amplitude, 0.0f, 1.0f); - // duration_s == 0 -> shortest pulse the runtime supports. Use double for - // the seconds->nanoseconds conversion: float has ~7 decimal digits and - // can lose tens of microseconds for multi-second pulses. Clamp the - // converted nanoseconds to a safe range that round-trips to int64 without - // overflow (real haptic pulses are sub-second, so 1e18 ns ~= 31 years is a - // generous upper bound that stays well below INT64_MAX ~= 9.22e18). + // 1e18 ns (~31 years) caps the converted duration well below INT64_MAX so + // the cast cannot overflow on absurdly large finite inputs. constexpr double k_max_duration_ns = 1.0e18; vibration.duration = (safe_duration_s <= 0.0f) ? XR_MIN_HAPTIC_DURATION : static_cast(std::clamp(static_cast(safe_duration_s) * 1.0e9, 0.0, k_max_duration_ns)); - // frequency_hz == 0 -> let the runtime pick. vibration.frequency = (safe_frequency_hz <= 0.0f) ? XR_FREQUENCY_UNSPECIFIED : safe_frequency_hz; const XrResult apply_result = diff --git a/src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py b/src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py index 51ff81746..f95bf1427 100644 --- a/src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py @@ -13,7 +13,7 @@ from __future__ import annotations -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, Any, Literal, cast import numpy as np @@ -30,64 +30,39 @@ class HapticSink(BaseRetargeter): """Per-frame sink for haptic feedback through any :class:`IHapticDevice` adapter. + Calls :meth:`IHapticDevice.apply` for each side whose input is present + *and* :meth:`IHapticDevice.supports` returns ``True``, so single-handed + devices cleanly no-op the unused side. + Inputs: - - ``"left"`` / ``"right"``: optional ``device.accepted_type()`` payloads - (one frame of device-side values per side). Optional so a one-handed - rig can wire only the side it actually drives. + - ``"left"`` / ``"right"``: optional ``device.accepted_type()`` payloads. Outputs: - - ``"_haptic_heartbeat"``: a single bool, always ``True``. Exposed only - so :class:`~isaacteleop.retargeting_engine.interface.output_combiner.OutputCombiner` - can include the sink in its graph traversal -- the combiner only - executes nodes that are reachable from a declared output. The - heartbeat is never consumed by downstream code and the - ``_`` prefix in its name signals "internal plumbing"; the helpers in - :mod:`isaaclab_teleop.tactile_helpers` wire it up automatically. - - .. warning:: - If you build a custom :class:`OutputCombiner` instead of going through - the ``isaaclab_teleop.tactile_helpers`` helpers, you **must** include - :attr:`HapticSink.HEARTBEAT` (or any other output reachable from this - node) as one of the combiner's outputs. A reachable output is the - only way ``OutputCombiner.get_leaf_nodes`` will discover the sink and - run its ``_compute_fn``; without it, ``device.apply()`` is never - called and haptics silently do not fire. - - The sink calls :meth:`IHapticDevice.apply` for each side whose input is - present *and* :meth:`IHapticDevice.supports` returns ``True`` -- this lets - single-handed devices (e.g. Haply Inverse3) cleanly no-op the unused side - without the upstream pipeline knowing. - - Modeled after :class:`MessageChannelSink`. Whether the adapter writes - to hardware synchronously (Manus, via the plugin singleton) or queues - for a paired :class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.interface.IDeviceIOSource` - to flush against the active session (OpenXR controllers) is the - adapter's concern -- ``HapticSink`` is identical in both cases. + - ``"_haptic_heartbeat"``: always ``True``. Required so + :class:`OutputCombiner` reaches the sink during graph traversal -- + a custom combiner that omits this output will never invoke the sink + and haptics will silently not fire. """ - LEFT = "left" - RIGHT = "right" - HEARTBEAT = "_haptic_heartbeat" + # Literal annotations so the iteration in ``_compute_fn`` is compatible + # with ``IHapticDevice.{supports,apply}``, both of which take ``Side``. + LEFT: Literal["left"] = "left" + RIGHT: Literal["right"] = "right" + HEARTBEAT: Literal["_haptic_heartbeat"] = "_haptic_heartbeat" def __init__(self, name: str, device: "IHapticDevice") -> None: - """Construct a haptic sink bound to ``device``. - - Args: - name: Unique pipeline node name. - device: Vendor-specific :class:`IHapticDevice` adapter. The - accepted type is captured from ``device.accepted_type()`` - at construction time and used in :meth:`input_spec`. - """ self._device = device super().__init__(name) @property def device(self) -> "IHapticDevice": - """Return the adapter this sink writes to.""" return self._device def input_spec(self) -> RetargeterIOType: - accepted = self._device.accepted_type() + # ``IHapticDevice`` lives outside the mypy target tree and imports + # ``TensorGroupType`` via its absolute path; the cast bridges the two + # views of the same runtime class. + accepted = cast(TensorGroupType, self._device.accepted_type()) return { self.LEFT: OptionalType(accepted), self.RIGHT: OptionalType(accepted), @@ -104,17 +79,7 @@ def _compute_fn( ) -> None: for side in (self.LEFT, self.RIGHT): group = inputs[side] - if group.is_none: - continue - if not self._device.supports(side): + if group.is_none or not self._device.supports(side): continue - # Pull tensor 0 (the only inner tensor of every device-side schema - # in v1: FingerPowerVector, ControllerHapticPulse, EndEffectorForce - # are all single-NDArray groups). Materialise as a CPU NumPy array - # since adapters write to host-side hardware APIs that do not - # speak DLPack directly. - values = np.asarray(group[0]) - self._device.apply(side, values) - # Heartbeat output exists purely so OutputCombiner can include this - # sink in its graph traversal; the value is never consumed. + self._device.apply(side, np.asarray(group[0])) outputs[self.HEARTBEAT][0] = True 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 d16123b38..e24e95848 100644 --- a/src/core/retargeting_engine/python/tensor_types/tactile_types.py +++ b/src/core/retargeting_engine/python/tensor_types/tactile_types.py @@ -1,22 +1,15 @@ # SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -""" -TensorGroupType definitions for tactile feedback and haptic output. - -Sim-side schemas (TactileVector, TactileHeatmap) carry contact / pressure data -from simulators (e.g. Isaac Lab's ContactSensor) into the retargeting pipeline. -Device-side schemas (FingerPowerVector, ControllerHapticPulse, EndEffectorForce) -describe what a haptic device adapter will accept. - -Retargeters map sim-side -> device-side schemas; vendor adapters implementing -``isaacteleop.haptic_devices.IHapticDevice`` declare which device-side schema -they accept via ``accepted_type()`` and the ``HapticSink`` retargeter uses that -for connect-time type checking. - -The existing :func:`TransformMatrix` factory in ``standard_types`` is reused -unchanged for the optional ``world_T_haptic`` frame leaf -- no new transform -schema is introduced here. +"""TensorGroupType definitions for tactile feedback and haptic output. + +Sim-side schemas (``TactileVector``, ``TactileHeatmap``) carry contact data +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 @@ -45,15 +38,10 @@ def TactileVector(num_taxels: int) -> TensorGroupType: """Per-taxel scalar magnitudes (or N-element vector) [N, depending on use]. - Generic sim-side schema for tactile / contact data. The same type covers: - - * a single contact force magnitude (``num_taxels == 1``), - * a row of taxels on a finger pad (``num_taxels == N``), - * a 3-vector force or 3-vector position (``num_taxels == 3``) used by the - composable spatial primitives (e.g. :class:`Vector3FrameTransform`). - - The semantic meaning of each entry is set by the retargeter consuming - this group; this schema only fixes shape and dtype. + Generic sim-side schema covering single contact magnitudes + (``num_taxels == 1``), per-pad taxel rows, and 3-vector forces / + positions used by the composable spatial primitives. The schema fixes + shape and dtype; the consuming retargeter fixes semantics. Args: num_taxels: Number of scalar entries. diff --git a/src/haptic_devices/__init__.py b/src/haptic_devices/__init__.py index 983f29416..15190976b 100644 --- a/src/haptic_devices/__init__.py +++ b/src/haptic_devices/__init__.py @@ -1,24 +1,13 @@ # SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""Vendor-agnostic haptic device adapters for Isaac Teleop. +"""Vendor-agnostic haptic device adapters consumed by ``HapticSink``. -Each adapter implements :class:`IHapticDevice` -- the contract consumed by -:class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.HapticSink`. -``apply()`` is pure I/O: no geometry, no morphology mapping. Those concerns -live upstream in retargeters; the adapter writes the bytes the device expects -in the units it expects, full stop. - -Vendor-specific adapter modules (``manus``, ``openxr_controller``, ...) lazy- -import their backing pybind11 modules so the package itself can always be -imported even when individual vendor extensions are not built. Users that need -a specific adapter import the submodule directly:: +Vendor-specific submodules (``manus``, ``openxr_controller``, ...) lazy-import +their backing pybind11 extensions so this package always imports cleanly. +Import the submodule directly to use a specific adapter, e.g.:: from isaacteleop.haptic_devices.manus import ManusHapticDevice - from isaacteleop.haptic_devices.openxr_controller import ( - OpenXRControllerHapticDevice, - OpenXRControllerHapticSource, - ) """ from .interface import IHapticDevice diff --git a/src/haptic_devices/interface.py b/src/haptic_devices/interface.py index fb64828a5..dcb07df36 100644 --- a/src/haptic_devices/interface.py +++ b/src/haptic_devices/interface.py @@ -1,13 +1,7 @@ # SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -"""Vendor-agnostic :class:`IHapticDevice` interface. - -The contract is intentionally tiny: an adapter declares its accepted -device-side schema and writes one frame to hardware. Type checking happens -at ``HapticSink.connect()`` time via ``accepted_type()``; per-frame dispatch -happens via ``apply()``. -""" +"""Vendor-agnostic :class:`IHapticDevice` interface.""" from __future__ import annotations @@ -20,81 +14,39 @@ Side = Literal["left", "right"] -"""Hand / side identifier consumed by :meth:`IHapticDevice.apply` and -:meth:`IHapticDevice.supports`.""" class IHapticDevice(ABC): """Vendor-agnostic adapter consumed by ``HapticSink``. - Implementations live under :mod:`isaacteleop.haptic_devices` (or downstream - packages) and wrap whatever I/O channel the vendor exposes -- a vendor SDK - call, an OpenXR action, a WebSocket message, etc. - - Contract: - - * ``accepted_type()`` returns the device-side - :class:`~isaacteleop.retargeting_engine.interface.tensor_group_type.TensorGroupType` - this adapter writes to hardware (e.g. - :func:`~isaacteleop.retargeting_engine.tensor_types.FingerPowerVector` - for Manus, :func:`~isaacteleop.retargeting_engine.tensor_types.ControllerHapticPulse` - for OpenXR motion controllers). ``HapticSink`` uses this for connect-time - type checking against the upstream retargeter. - * ``apply(side, values)`` writes one frame to hardware. The values array is - already in the device's native units and frame -- spatial transforms and - morphology mapping happen upstream in retargeters. ``apply()`` must be - cheap and non-throwing; hardware errors are logged-and-no-op'd so a - transient device hiccup never tears down the pipeline. - * ``supports(side)`` reports per-side availability for adapters where the - device is single-handed (e.g. Haply Inverse3) or where one side is - disabled by config. Default: both sides supported. - - Sub-classes must not perform geometry or morphology in ``apply()``. Those - concerns belong upstream so they can be visualised and tuned through the - existing retargeter parameter UI. + Implementations wrap whatever I/O channel the vendor exposes (vendor SDK + call, OpenXR action, WebSocket, ...). They must not perform geometry or + morphology mapping in :meth:`apply` -- those concerns live upstream in + retargeters so they can be visualised and tuned via the parameter UI. + + Implementations of :meth:`apply` must be non-throwing on hardware errors: + log-once-and-no-op so a transient device hiccup never tears down the + pipeline. """ @abstractmethod def accepted_type(self) -> TensorGroupType: - """Return the device-side ``TensorGroupType`` this adapter consumes. + """Device-side ``TensorGroupType`` this adapter consumes. - The returned value is checked structurally against the upstream - retargeter's output type at ``HapticSink.connect()`` time, so wrong - wiring fails before any hardware call. - - Returns: - A ``TensorGroupType`` describing the shape and dtype of one - ``apply()`` payload. + Checked against the upstream retargeter's output at + ``HapticSink.connect()`` time so wrong wiring fails before any + hardware call. """ @abstractmethod def apply(self, side: Side, values: np.ndarray) -> None: """Write one frame of haptic output to hardware. - Implementations must be cheap and non-throwing. On hardware errors, - log once and return -- a transient device hiccup must not tear down - the retargeting pipeline. - - Args: - side: ``"left"`` or ``"right"``. - values: One frame of device-side values, shape matching the inner - tensor of :meth:`accepted_type`. For ``FingerPowerVector(5)`` - this is a ``(5,)`` float32 in ``[0, 1]``; for - ``ControllerHapticPulse`` it is ``[amplitude, frequency_hz, - duration_s]``. + ``values`` is the inner tensor of :meth:`accepted_type` -- e.g. a + ``(5,) float32`` in ``[0, 1]`` for ``FingerPowerVector(5)``, or + ``[amplitude, frequency_hz, duration_s]`` for ``ControllerHapticPulse``. """ def supports(self, side: Side) -> bool: - """Report whether this adapter writes to hardware for a given side. - - ``HapticSink`` gates calls to :meth:`apply` on this method so - single-handed devices (e.g. Haply Inverse3) cleanly no-op the - unconfigured side. - - Args: - side: ``"left"`` or ``"right"``. - - Returns: - ``True`` (default) when the side is active. - """ + """Whether this adapter writes to hardware for ``side``. Default: yes.""" return True diff --git a/src/haptic_devices/manus.py b/src/haptic_devices/manus.py index 62b1bf0db..b1bd21317 100644 --- a/src/haptic_devices/manus.py +++ b/src/haptic_devices/manus.py @@ -3,12 +3,11 @@ """Manus Metagloves Pro Haptic adapter. -Bridges :class:`IHapticDevice` to the Manus plugin singleton via a small -pybind11 module (``_manus_haptic``). The plugin singleton must already be -alive -- the Manus hand-tracking plugin sets it up, and its connection -lifecycle is managed there. This adapter only forwards per-finger powers -to ``CoreSdk_VibrateFingersForGlove`` through the plugin's haptic entry -point. +Forwards per-finger powers to the Manus plugin singleton via the +``_manus_haptic`` pybind module, which routes to +``CoreSdk_VibrateFingersForGlove``. Assumes the Manus hand-tracking plugin +has already started the singleton; vendor SDK linkage stays inside +``src/plugins/manus/`` per the AGENTS.md boundary. """ from __future__ import annotations @@ -33,31 +32,17 @@ class ManusHapticDevice(IHapticDevice): """:class:`IHapticDevice` adapter for the Manus Metagloves Pro Haptic glove. - Consumes :func:`FingerPowerVector(num_fingers=5) ` - in Manus order ``[Thumb, Index, Middle, Ring, Pinky]`` with values in - ``[0, 1]`` (the C++ side clamps; values outside the range are silently - rounded in). ``apply()`` is a thin shim over the pybind module that - forwards to ``ManusTracker::apply_haptic_command`` -- vendor SDK linkage - stays inside ``src/plugins/manus/`` per the AGENTS.md boundary. - - The pybind module is imported lazily on the first :meth:`apply` call so - importing :mod:`isaacteleop.haptic_devices.manus` does not require the - Manus SDK to be installed; an ``ImportError`` with a clear message is - raised at use time if the SDK is missing. + Consumes ``FingerPowerVector(5)`` in Manus order + ``[Thumb, Index, Middle, Ring, Pinky]`` with values in ``[0, 1]``. The + pybind module is imported lazily so importing this module does not + require the Manus SDK to be installed; if the SDK or plugin is missing, + :meth:`apply` logs once per side and silently no-ops so the pipeline + keeps running. """ def __init__(self, num_fingers: int = NUM_HAPTIC_FINGERS) -> None: - """Construct a Manus haptic adapter. - - Args: - num_fingers: Channel count. Manus hardware fixes this to 5; - exposed for parity with :func:`FingerPowerVector` and to - catch mis-wired pipelines at ``connect()`` time. - """ self._num_fingers = num_fingers - # Lazy-imported on first apply(); set once for the lifetime of the process. self._pybind = None - # Log hardware errors at most once per side to keep the pipeline log clean. self._error_logged: dict[str, bool] = {"left": False, "right": False} def accepted_type(self) -> TensorGroupType: @@ -68,47 +53,36 @@ def apply(self, side: Literal["left", "right"], values: np.ndarray) -> None: if pybind is None: return - # Force the shape and dtype the C++ side expects. NumPy view; no copy - # when the upstream retargeter already produced a contiguous float32. arr = np.asarray(values, dtype=np.float32).ravel() if arr.size != self._num_fingers: raise ValueError( f"ManusHapticDevice.apply expects a {self._num_fingers}-element " - "FingerPowerVector " - f"(order [Thumb, Index, Middle, Ring, Pinky]), got shape {np.asarray(values).shape}" + f"FingerPowerVector (order [Thumb, Index, Middle, Ring, Pinky]), " + f"got shape {np.asarray(values).shape}" ) - powers = arr.reshape(self._num_fingers) try: - pybind.apply_haptic_command(side, powers) + pybind.apply_haptic_command(side, arr) except Exception as exc: if not self._error_logged[side]: logger.warning( - "ManusHapticDevice.apply(%s) failed (will silence further " - "errors for this side): %s", + "ManusHapticDevice.apply(%s) failed (further errors for this " + "side will be silenced): %s", side, exc, ) self._error_logged[side] = True def _get_pybind(self): - """Lazy-import the Manus haptic pybind module. - - Cached after first successful import. When the module is unavailable - (Manus SDK not installed at build time), logs once and returns ``None`` - so the pipeline keeps running -- haptic feedback is a nice-to-have, - not a session-fatal dependency. - """ if self._pybind is not None: return self._pybind try: from . import _manus_haptic # type: ignore[import-not-found] except ImportError as exc: if not self._error_logged["left"]: - # Reuse one error_logged slot; the message is identical for both sides. logger.warning( - "ManusHapticDevice unavailable: %s. " - "Build the Manus plugin (src/plugins/manus/) with the " - "Manus SDK installed to enable haptic output.", + "ManusHapticDevice unavailable: %s. Build the Manus plugin " + "(src/plugins/manus/) with the Manus SDK installed to " + "enable haptic output.", exc, ) self._error_logged["left"] = True diff --git a/src/haptic_devices/openxr_controller.py b/src/haptic_devices/openxr_controller.py index 235791863..b2575eaf5 100644 --- a/src/haptic_devices/openxr_controller.py +++ b/src/haptic_devices/openxr_controller.py @@ -3,24 +3,17 @@ """OpenXR motion-controller haptic adapter (Quest, Vive, Index, Pico, ...). -Unlike vendor-SDK gloves, OpenXR motion-controller haptics ride the standard -OpenXR action system the live trackers layer is already built around. The -integration is therefore a *tracker extension*, not a new plugin -- the -``LiveControllerTrackerImpl`` gains a haptic action and an -``apply_haptic_feedback`` method, and the public -:class:`~isaacteleop.deviceio_trackers.ControllerTracker` gains a matching -delegating method that takes a session. - -To talk through the active :class:`~isaacteleop.deviceio.DeviceIOSession`, -this module ships a pair following the existing -:class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.MessageChannelSink` -pattern: :class:`OpenXRControllerHapticDevice` enqueues frames during -``HapticSink._compute_fn`` (no session in scope there), and +Ships a sink/source pair following the ``MessageChannelSink`` pattern: +:class:`OpenXRControllerHapticDevice` queues per-frame pulses from inside +``HapticSink._compute_fn`` (no session in scope), and :class:`OpenXRControllerHapticSource` drains the queue inside -``poll_tracker(session)`` (where TeleopSession provides the session). +``poll_tracker(session)`` so the call goes through the active +``DeviceIOSession``. Routes through the existing +``LiveControllerTrackerImpl::apply_haptic_feedback`` rather than a new plugin +because OpenXR is already the abstraction the live-tracker layer is built on. -The user wires both into the pipeline via -:func:`isaaclab_teleop.tactile_helpers.build_default_openxr_controller_pipeline`. +The :func:`~isaaclab_teleop.tactile_helpers.build_default_openxr_controller_pipeline` +helper wires both into a pipeline. """ from __future__ import annotations @@ -58,48 +51,27 @@ class OpenXRControllerHapticDevice(IHapticDevice): """:class:`IHapticDevice` adapter for OpenXR motion-controller haptics. - Consumes :func:`ControllerHapticPulse ` - (one ``(3,) float32`` per side: ``[amplitude, frequency_hz, duration_s]``). - ``frequency_hz == 0`` selects ``XR_FREQUENCY_UNSPECIFIED``; ``duration_s == 0`` - selects ``XR_MIN_HAPTIC_DURATION``; ``amplitude == 0`` triggers - ``xrStopHapticFeedback`` on the C++ side. - - ``apply()`` does not call OpenXR directly because the active session is - not in scope inside ``HapticSink._compute_fn`` (it lives on - :class:`~isaacteleop.teleop_session_manager.TeleopSession`, not on - individual retargeters). Instead it queues the latest pulse per side and - relies on the paired :class:`OpenXRControllerHapticSource` to drain the - queue inside ``poll_tracker(session)`` once per frame -- the same split - :class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.MessageChannelSink` - uses for outbound message-channel traffic. - - .. note:: - **One-frame haptic latency.** The :class:`OpenXRControllerHapticSource` - drains the queue at the *start* of each step (in ``poll_tracker``, - before the retargeting graph runs), while :meth:`apply` enqueues a - pulse from inside the graph. So a pulse produced in step *N* is sent - to the controller in step *N+1*'s pre-graph drain. At 60–90 Hz the - ~11–17 ms delay is below the perception threshold for vibration; for - higher-rate force-feedback paths (e.g. the planned Haply integration) - this ordering deserves a redesign. + Consumes ``ControllerHapticPulse`` (``[amplitude, frequency_hz, + duration_s]``). ``frequency_hz == 0`` selects ``XR_FREQUENCY_UNSPECIFIED``; + ``duration_s == 0`` selects ``XR_MIN_HAPTIC_DURATION``; ``amplitude == 0`` + triggers ``xrStopHapticFeedback``. :meth:`apply` queues the pulse instead + of calling OpenXR directly because the active session is only in scope + inside :class:`OpenXRControllerHapticSource.poll_tracker`, which drains + the queue once per frame. + + Note: the source drains *before* the retargeting graph runs, so a pulse + produced in step *N* reaches the controller during step *N+1*'s pre-graph + drain. ~11–17 ms at 60–90 Hz is below the perception threshold for + vibration; force-feedback paths will want a different ordering. """ def __init__( self, sides: Iterable[Literal["left", "right"]] = ("left", "right"), ) -> None: - """Construct an OpenXR motion-controller haptic adapter. - - Args: - sides: Which sides to drive. Most motion controllers are paired - (default ``("left", "right")``), but some single-handed - controllers exist; restrict here to make - :meth:`supports` return ``False`` for the unused side. - """ self._sides = set(sides) - # Only the most recent pulse per side per frame matters: an - # xrApplyHapticFeedback() call already supersedes any in-flight pulse - # on the same action, so coalescing here is correct, not lossy. + # Latest-wins per side: xrApplyHapticFeedback already supersedes any + # in-flight pulse on the same action, so coalescing is non-lossy. self._pending: dict[Literal["left", "right"], _PendingPulse] = {} def accepted_type(self) -> TensorGroupType: @@ -119,48 +91,33 @@ def apply(self, side: Literal["left", "right"], values: np.ndarray) -> None: self._pending[side] = (float(arr[0]), float(arr[1]), float(arr[2])) def drain_pending(self) -> dict[Literal["left", "right"], _PendingPulse]: - """Return and clear the per-side pending pulses. - - Called once per frame by the paired - :class:`OpenXRControllerHapticSource` from inside ``poll_tracker``. - """ + """Return and clear the per-side pending pulses; called once per frame + by :class:`OpenXRControllerHapticSource.poll_tracker`.""" pending, self._pending = self._pending, {} return pending class OpenXRControllerHapticSource(IDeviceIOSource): - """Drains :class:`OpenXRControllerHapticDevice`'s queue through an active session. - - This is the session-aware half of the OpenXR-controller haptic plumbing. - It is an :class:`IDeviceIOSource` so :class:`~isaacteleop.teleop_session_manager.TeleopSession` - auto-discovers it as a graph leaf, registers its underlying - :class:`~isaacteleop.deviceio_trackers.ControllerTracker` for OpenXR - extension aggregation, and calls ``poll_tracker(deviceio_session)`` once - per frame. ``poll_tracker`` drains the device queue and forwards the - pulses to ``controller_tracker.apply_haptic_feedback(session, side, ...)``. - - .. warning:: - **Discovery requires a declared output.** ``TeleopSession`` discovers - :class:`IDeviceIOSource` leaves by walking back from the outputs - declared on the user's :class:`~isaacteleop.retargeting_engine.interface.OutputCombiner`. - If you build a custom pipeline, you **must** include - :attr:`OpenXRControllerHapticSource.HEARTBEAT` (or any output of this - node) as one of the combiner's outputs, otherwise the source is never - polled, the tracker is never registered, and haptics silently never - fire. Use :func:`~isaaclab_teleop.tactile_helpers.build_default_openxr_controller_pipeline` - if you do not need a custom pipeline -- it wires the heartbeat for - you. - - .. warning:: - **Share the same** :class:`ControllerTracker` **instance with any - existing** :class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.ControllersSource`. - ``DeviceIOSession`` deduplicates trackers by raw pointer, so passing - two distinct ``ControllerTracker()`` instances will create two - ``LiveControllerTrackerImpl`` objects that both try to attach an - action set to the same ``XrSession`` -- the second attach raises - ``XR_ERROR_ACTIONSETS_ALREADY_ATTACHED`` and the session aborts. Use - :meth:`for_controllers_source` (below) or pass - ``controllers_source.get_tracker()`` explicitly to avoid this. + """Session-aware drain for :class:`OpenXRControllerHapticDevice`. + + Implemented as an :class:`IDeviceIOSource` so ``TeleopSession`` discovers + it as a graph leaf, registers its ``ControllerTracker`` for OpenXR + extension aggregation, and calls ``poll_tracker(deviceio_session)`` each + frame. ``poll_tracker`` drains the device queue and forwards the pulses + to ``controller_tracker.apply_haptic_feedback(session, side, ...)``. + + Two requirements for custom pipelines: + + * **Heartbeat must be reachable from the user's ``OutputCombiner``** — the + session walks back from declared outputs to find leaves; without + ``HEARTBEAT`` (or any other output of this node) wired in, the source + is never polled and haptics silently do not fire. + * **The ``ControllerTracker`` instance must be shared** with any + ``ControllersSource`` already in the pipeline. ``DeviceIOSession`` + deduplicates trackers by pointer; two distinct instances both try to + attach an action set to the same ``XrSession`` and the second attach + raises ``XR_ERROR_ACTIONSETS_ALREADY_ATTACHED``. Use + :meth:`for_controllers_source` to avoid this footgun. """ HEARTBEAT = "_openxr_haptic_heartbeat" @@ -173,9 +130,6 @@ def __init__( ) -> None: self._device = device self._controller_tracker = controller_tracker - # Logged-at-most-once per side so a missing C++ haptic method - # (e.g. running against an older TeleopCore build that has not - # been rebuilt with the haptic extension) does not flood the log. self._error_logged: dict[str, bool] = {"left": False, "right": False} super().__init__(name) @@ -186,26 +140,12 @@ def for_controllers_source( device: OpenXRControllerHapticDevice, controllers_source: Any, ) -> "OpenXRControllerHapticSource": - """Construct a haptic source that shares its tracker with a controllers source. - - Convenience wrapper around the constructor that fetches - ``controllers_source.get_tracker()`` for you, so the two sources - cannot accidentally diverge on which ``ControllerTracker`` instance - they hold. See the class docstring for why sharing matters. - - Args: - name: Unique pipeline node name. - device: The :class:`OpenXRControllerHapticDevice` whose queue this - source will drain. - controllers_source: The - :class:`~isaacteleop.retargeting_engine.deviceio_source_nodes.ControllersSource` - already in the pipeline. Anything with a ``get_tracker()`` - method returning a ``ControllerTracker`` works (typed as - ``Any`` to avoid a circular import). - - Returns: - A new :class:`OpenXRControllerHapticSource` bound to the same - tracker as ``controllers_source``. + """Build a source that shares its tracker with ``controllers_source``. + + Prefer this over the bare constructor: it fetches + ``controllers_source.get_tracker()`` for you so the two sources + cannot diverge on the ``ControllerTracker`` instance they hold. See + the class docstring for why sharing matters. """ return cls(name, device, controllers_source.get_tracker()) @@ -213,10 +153,6 @@ def input_spec(self) -> RetargeterIOType: return {} def output_spec(self) -> RetargeterIOType: - # Heartbeat output exists purely so OutputCombiner can include this - # leaf in its graph traversal (TeleopSession discovers IDeviceIOSource - # leaves by walking back from declared OutputCombiner outputs). - # The helper in `isaaclab_teleop.tactile_helpers` wires it up. return { self.HEARTBEAT: TensorGroupType( "_openxr_haptic_heartbeat", [BoolType("ok")] @@ -234,23 +170,17 @@ def poll_tracker(self, deviceio_session: Any) -> RetargeterIO: ) in self._device.drain_pending().items(): try: self._controller_tracker.apply_haptic_feedback( - deviceio_session, - side, - amplitude, - frequency_hz, - duration_s, + deviceio_session, side, amplitude, frequency_hz, duration_s ) except Exception as exc: if not self._error_logged[side]: logger.warning( "OpenXRControllerHapticSource.poll_tracker(%s) failed " - "(will silence further errors for this side): %s", + "(further errors for this side will be silenced): %s", side, exc, ) self._error_logged[side] = True - # The IDeviceIOSource contract expects a dict matching input_spec; we - # have no inputs so an empty dict is correct. return {} def _compute_fn( diff --git a/src/retargeters/tactile_retargeters.py b/src/retargeters/tactile_retargeters.py index f4893a9d5..ae87a8ba6 100644 --- a/src/retargeters/tactile_retargeters.py +++ b/src/retargeters/tactile_retargeters.py @@ -1,30 +1,16 @@ # SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -""" -Tactile / haptic retargeters. - -Two groups, both vendor-neutral: - -* **Composable spatial primitives** (:class:`Vector3FrameTransform`, - :class:`WorldForceAccumulator`, :class:`MagnitudeReducer`) operate on - sim-side :func:`~isaacteleop.retargeting_engine.tensor_types.TactileVector` - flows. They are the building blocks every per-device mapper composes with. - -* **Per-device mappers** (:class:`TactileVectorToFingerPower`, - :class:`TactileHeatmapToFingerPower`, :class:`TactileHeatmapToWristPulse`, - :class:`TactileVectorToControllerPulse`, - :class:`TactileHeatmapToControllerPulse`, - :class:`FingerPowerToControllerPulse`) translate sim-side tactile data -- - or already-reduced device-side per-finger power -- into one of the - device-side schemas declared in - :mod:`~isaacteleop.retargeting_engine.tensor_types.tactile_types`. They are - named after the target schema, not the vendor -- any vendor whose device - accepts the same schema reuses the same mapper. - -When the Haply force-feedback adapter lands, ``ContactForceToEndEffectorForce`` -and ``TactileHeatmapToEndEffectorForce`` slot into this file with no -restructuring; they compose the same spatial primitives above. +"""Tactile / haptic retargeters (vendor-neutral). + +Two groups: + +* **Composable spatial primitives** -- :class:`Vector3FrameTransform`, + :class:`WorldForceAccumulator`, :class:`MagnitudeReducer` -- operating on + sim-side ``TactileVector`` flows. +* **Per-device mappers** -- ``Tactile{Vector,Heatmap}To{FingerPower,WristPulse,ControllerPulse}`` + and :class:`FingerPowerToControllerPulse` -- named after the target device-side + schema, not the vendor. """ from __future__ import annotations @@ -61,32 +47,19 @@ class Vector3FrameTransform(BaseRetargeter): - """Rotate a sim-frame :func:`TactileVector(3)` into a new frame. + """Rotate a sim-frame ``TactileVector(3)`` into a new frame (rotation only). - .. note:: - **Rotation-only.** This node is force-vector oriented: ``TactileVector(3)`` - is treated as a *free vector* (e.g. a contact force or torque), - so the translation column of the input ``TransformMatrix`` is - intentionally ignored and only the upper-left 3x3 rotation block is - applied. To transform a *position* (where translation matters), use a - full 4x4 affine multiply at a layer that knows the input is a point, - not in this node. + Treats the input as a free vector (e.g. a contact force / torque) and + applies only the upper-left 3x3 of the input ``TransformMatrix``; the + translation column is intentionally ignored. Use a full 4x4 affine + multiply elsewhere if the value is a position. Inputs: - - ``"vec"``: :func:`TactileVector(3) ` - (a 3-vector free quantity) in the source frame. - - ``"transform"``: :func:`TransformMatrix ` - ``(4, 4)`` -- the source-to-target homogeneous transform; only the - rotation block ``M[:3, :3]`` is read. + - ``"vec"``: ``TactileVector(3)`` in the source frame. + - ``"transform"``: ``TransformMatrix`` (4x4); only ``M[:3, :3]`` is read. Outputs: - - ``"vec"``: :func:`TactileVector(3)` in the target frame. - - Used to compose Concern A (frame mismatch) of the tactile-haptic factoring - described in the design doc. The ``"transform"`` input is typically the - optional ``world_T_haptic`` :class:`~isaacteleop.retargeting_engine.interface.value_input.ValueInput` - leaf fed by IsaacTeleopDevice when ``IsaacTeleopHapticCfg.haptic_device_frame_prim_path`` - is set, but any pipeline node producing a ``TransformMatrix`` is valid. + - ``"vec"``: ``TactileVector(3)`` in the target frame. """ INPUT_VEC = "vec" @@ -112,24 +85,14 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N class WorldForceAccumulator(BaseRetargeter): - """Sum N :func:`TactileVector(3)` inputs into one, with optional per-input weights. - - Inputs: - - ``"in_0"``, ``"in_1"``, ... ``"in_{num_inputs - 1}"``: each a - :func:`TactileVector(3) ` - in a common frame. - - Outputs: - - ``"vec"``: :func:`TactileVector(3)` = sum of ``weights[i] * in_i``. + """Weighted sum of N ``TactileVector(3)`` inputs in a common frame. - Tunable parameters: - - ``weights``: :class:`VectorParameter` of length ``num_inputs`` - (default: all ones). Exposed in the tuning UI so an operator can - attenuate or zero out individual contributing bodies live. + Tunable ``weights`` (``VectorParameter`` of length ``num_inputs``, default + all ones) lets an operator attenuate or zero out individual contributing + bodies from the tuning UI. - Useful upstream of :class:`Vector3FrameTransform` to combine forces from - multiple sim contact sensors (e.g. left and right Franka fingers) before - rotating into the device frame. + Inputs: ``"in_0"`` ... ``"in_{num_inputs - 1}"`` -- each ``TactileVector(3)``. + Outputs: ``"vec"`` -- sum of ``weights[i] * in_i``. """ OUTPUT_VEC = "vec" @@ -193,34 +156,17 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N class MagnitudeReducer(BaseRetargeter): - """Reduce a :func:`TactileVector(3)` to a :func:`TactileVector(1)` scalar. + """Reduce a ``TactileVector(3)`` to a ``TactileVector(1)`` scalar. - Inputs: - - ``"vec"``: :func:`TactileVector(3) `. - - Outputs: - - ``"scalar"``: :func:`TactileVector(1) ` - (the reduced magnitude). - - The reduction *mode* is fixed at construction time: + Bridges directional contact data into frame-invariant device schemas + (``FingerPowerVector``, ``ControllerHapticPulse``). Mode is fixed at + construction: * ``"norm"`` -- Euclidean length ``||vec||_2``. * ``"axis_x"`` / ``"axis_y"`` / ``"axis_z"`` -- absolute value of the - corresponding component (use these after :class:`Vector3FrameTransform` - when the device cares about pressure normal to a known axis). - - Used to bridge directional contact data (a 3-vector force) into the - frame-invariant device schemas that only care about magnitude - (:func:`FingerPowerVector`, :func:`ControllerHapticPulse`). - - .. note:: - The design doc lists the original mode set as - ``{'norm', 'axis_z', 'dot_with_normal'}``. The ``axis_z`` mode is a - special case of the per-axis modes shipped here (``axis_x`` / - ``axis_y`` / ``axis_z``); ``dot_with_normal`` was deferred to a later - change because it can be expressed as a pre-rotation via - :class:`Vector3FrameTransform` plus ``axis_z``, and there is no v1 - consumer that needs it directly. + corresponding component (typically chained after a + ``Vector3FrameTransform`` when the device cares about pressure normal + to a known axis). """ INPUT_VEC = "vec" @@ -695,22 +641,15 @@ class TactileVectorToControllerPulse(BaseRetargeter): The taxels are reduced to a single magnitude via ``reduction`` (``"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. Both - default to ``0.0`` so every conformant OpenXR runtime is free to pick - ``XR_FREQUENCY_UNSPECIFIED`` and ``XR_MIN_HAPTIC_DURATION``. + paired with constant ``frequency_hz`` / ``duration_s`` parameters + (defaults ``0.0`` map to ``XR_FREQUENCY_UNSPECIFIED`` / + ``XR_MIN_HAPTIC_DURATION`` on the runtime). Tunable parameters: ``gain``, ``deadband``, ``saturation``, - ``frequency_hz``, ``duration_s``. - - .. note:: - Unlike :class:`TactileVectorToFingerPower`, this mapper has no - ``smoothing`` parameter. ``xrApplyHapticFeedback`` already supersedes - any in-flight pulse on the same action every frame, so the runtime - sees a fresh amplitude each step. EMA-smoothing the amplitude in - Python before that overwrite would just shift the latency without - meaningfully shaping the felt vibration, so the parameter is - intentionally omitted. Add an upstream low-pass retargeter on the - :class:`TactileVector` input if you need temporal shaping. + ``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. """ INPUT_TACTILE = "tactile" @@ -945,34 +884,21 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N class FingerPowerToControllerPulse(BaseRetargeter): - """Reduce a :func:`FingerPowerVector` to one :func:`ControllerHapticPulse`. - - Bridges per-finger glove-style output to single-channel controller rumble: - given an already-reduced device-side - :func:`FingerPowerVector(num_fingers) ` - in ``[0, 1]`` (e.g. the output of :class:`TactileVectorToFingerPower` or - of an upstream pinch-proximity retargeter), this collapses the channels - to a single amplitude with the chosen ``reduction`` and pairs it with - constant ``frequency_hz`` / ``duration_s`` parameters. - - The same gain / deadband / saturation curve as - :class:`TactileVectorToControllerPulse` is applied so the controller - pulse can be tuned independently of the upstream per-finger signal -- the - operator can scale the rumble up, suppress weak signals, or cap the - saturation without touching the per-finger pipeline. + """Reduce a ``FingerPowerVector`` to one ``ControllerHapticPulse``. - Inputs: - - ``"powers"``: :func:`FingerPowerVector(num_fingers) `. + 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. - Outputs: - - ``"pulse"``: :func:`ControllerHapticPulse ` - = ``[amplitude, frequency_hz, duration_s]``. - - Tunable parameters: ``gain``, ``deadband``, ``saturation``, - ``frequency_hz``, ``duration_s``. + Inputs: ``"powers"`` -- ``FingerPowerVector(num_fingers)``. + Outputs: ``"pulse"`` -- ``[amplitude, frequency_hz, duration_s]``. - See :class:`TactileVectorToControllerPulse`'s note for why this mapper has - no ``smoothing`` parameter. + Tunable: ``gain``, ``deadband``, ``saturation``, ``frequency_hz``, + ``duration_s``. No ``smoothing`` for the same reason as + :class:`TactileVectorToControllerPulse`. """ INPUT_POWERS = "powers" From e5239b42f761a9881d36920f80b4281de54fa8ad Mon Sep 17 00:00:00 2001 From: Rafael Wiltz Date: Fri, 22 May 2026 12:46:12 -0400 Subject: [PATCH 5/5] Fix windows CI build --- src/core/AGENTS.md | 1 + src/core/python/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/core/AGENTS.md b/src/core/AGENTS.md index 48d2cb13d..54f34f88e 100644 --- a/src/core/AGENTS.md +++ b/src/core/AGENTS.md @@ -20,3 +20,4 @@ If work under **`src/core/`** went wrong—**user** correction, **pre-commit/CI* - After Python test or session-manager edits, let `ruff format`/pre-commit own wrapping and rerun the hook when it modifies files. - **Sink/source `IDeviceIOSource` leaves are only discovered when reachable from a declared `OutputCombiner` output.** `TeleopSession._discover_sources` calls `pipeline.get_leaf_nodes()`, which walks back from the combiner's outputs. A sink/source whose only purpose is a side effect (haptic output, message-channel send) must therefore expose at least one output (a heartbeat boolean is the established pattern) **and** the user's combiner must include it. Every new sink/source of this shape needs both an explicit heartbeat output and a docstring warning that custom combiners must wire it up — silent no-discovery is the recurring footgun. - **Run `clang-format -i` on touched C++ files before pushing.** The repo's `pre-commit` config runs `ruff` for Python but does **not** run `clang-format`; CI (`build-ubuntu.yml`) installs `clang-format-14` and rejects unformatted C++ as `-Wclang-format-violations`. Format locally with the system `clang-format` (matches CI's version 14) — e.g. `clang-format -i $(git diff --name-only main -- '*.cpp' '*.hpp' '*.h' '*.cc')` — and verify with `clang-format --dry-run --Werror `. +- **Wire any new Python-only `add_subdirectory` package into `python_package`'s explicit `DEPENDS`.** ``src/core/python/CMakeLists.txt::python_package`` lists every staging-tree contributor (e.g. ``retargeters_python``); a new pure-Python custom target that copies files into ``python_package//isaacteleop//`` must be added there too. Linux Make may run the new target before the wheel build by accident, but Windows ninja parallelizes ``ALL`` targets and races, producing ``error: package directory 'isaacteleop\\' does not exist`` from setuptools. diff --git a/src/core/python/CMakeLists.txt b/src/core/python/CMakeLists.txt index 040df7101..0e0865768 100644 --- a/src/core/python/CMakeLists.txt +++ b/src/core/python/CMakeLists.txt @@ -82,7 +82,7 @@ add_custom_target(python_package ALL COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-retargeters.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-retargeters-lite.txt" "${CMAKE_BINARY_DIR}/python_package/$/" COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-grounding.txt" "${CMAKE_BINARY_DIR}/python_package/$/" - DEPENDS deviceio_trackers_py deviceio_session_py oxr_py plugin_manager_py schema_py retargeting_engine_python retargeters_python retargeting_engine_ui_python teleop_session_manager_python cloudxr_python + DEPENDS deviceio_trackers_py deviceio_session_py oxr_py plugin_manager_py schema_py retargeting_engine_python retargeters_python haptic_devices_python retargeting_engine_ui_python teleop_session_manager_python cloudxr_python COMMENT "Preparing Python package structure" )