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..5e95ae545 --- /dev/null +++ b/examples/haptic_feedback/python/hand_pinch_haptic_example.py @@ -0,0 +1,612 @@ +# 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 math +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.retargeters.tactile_retargeters import FingerPowerToControllerPulse +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 ( + ControllerHapticPulseField, + FingerIndex, + FingerPowerVector, + HandInput, + HandInputIndex, + HandJointIndex, + 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 + + +# ============================================================================ +# 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}") + # 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: + 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 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: + 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..35508f9e5 --- /dev/null +++ b/examples/haptic_feedback/python/openxr_controller_haptic_example.py @@ -0,0 +1,521 @@ +# 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}") + # 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: + 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 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: + 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..54f34f88e 100644 --- a/src/core/AGENTS.md +++ b/src/core/AGENTS.md @@ -18,3 +18,6 @@ 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. +- **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/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..68fd2e88d 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,11 @@ 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..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 @@ -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,22 @@ 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..08e6fb3fe 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 +{ + 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"; + + // 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; + + XrHapticActionInfo info{ XR_TYPE_HAPTIC_ACTION_INFO }; + info.action = haptic_action_; + info.subactionPath = subaction_path; + + // 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) + { + return; + } + const XrResult stop_result = core_funcs_.xrStopHapticFeedback(session_, &info); + if (XR_FAILED(stop_result)) + { + 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 entry point — silently no-op. + return; + } + + XrHapticVibration vibration{ XR_TYPE_HAPTIC_VIBRATION }; + vibration.amplitude = std::clamp(safe_amplitude, 0.0f, 1.0f); + // 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)); + 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)); + 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..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 @@ -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/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" ) 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..386235e32 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,10 @@ 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..f95bf1427 --- /dev/null +++ b/src/core/retargeting_engine/python/deviceio_source_nodes/haptic_sink.py @@ -0,0 +1,85 @@ +# 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, Literal, cast + +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. + + 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. + + Outputs: + - ``"_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. + """ + + # 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: + self._device = device + super().__init__(name) + + @property + def device(self) -> "IHapticDevice": + return self._device + + def input_spec(self) -> RetargeterIOType: + # ``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), + } + + 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 or not self._device.supports(side): + continue + self._device.apply(side, np.asarray(group[0])) + 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..e24e95848 --- /dev/null +++ b/src/core/retargeting_engine/python/tensor_types/tactile_types.py @@ -0,0 +1,175 @@ +# 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 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 +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 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. + + 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..ab69172f2 --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_tactile_retargeters.py @@ -0,0 +1,459 @@ +# 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 ( + FingerPowerToControllerPulse, + 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 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) + # 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..15190976b --- /dev/null +++ b/src/haptic_devices/__init__.py @@ -0,0 +1,15 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Vendor-agnostic haptic device adapters consumed by ``HapticSink``. + +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 .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..dcb07df36 --- /dev/null +++ b/src/haptic_devices/interface.py @@ -0,0 +1,52 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Vendor-agnostic :class:`IHapticDevice` interface.""" + +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"] + + +class IHapticDevice(ABC): + """Vendor-agnostic adapter consumed by ``HapticSink``. + + 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: + """Device-side ``TensorGroupType`` this adapter consumes. + + 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. + + ``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: + """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 new file mode 100644 index 000000000..b1bd21317 --- /dev/null +++ b/src/haptic_devices/manus.py @@ -0,0 +1,92 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Manus Metagloves Pro Haptic adapter. + +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 + +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 ``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: + self._num_fingers = num_fingers + self._pybind = None + 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 + + 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 " + f"FingerPowerVector (order [Thumb, Index, Middle, Ring, Pinky]), " + f"got shape {np.asarray(values).shape}" + ) + try: + pybind.apply_haptic_command(side, arr) + except Exception as exc: + if not self._error_logged[side]: + logger.warning( + "ManusHapticDevice.apply(%s) failed (further errors for this " + "side will be silenced): %s", + side, + exc, + ) + self._error_logged[side] = True + + def _get_pybind(self): + 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"]: + 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..b2575eaf5 --- /dev/null +++ b/src/haptic_devices/openxr_controller.py @@ -0,0 +1,192 @@ +# 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, ...). + +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)`` 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 :func:`~isaaclab_teleop.tactile_helpers.build_default_openxr_controller_pipeline` +helper wires both into a 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 ``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: + self._sides = set(sides) + # 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: + 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 :class:`OpenXRControllerHapticSource.poll_tracker`.""" + pending, self._pending = self._pending, {} + return pending + + +class OpenXRControllerHapticSource(IDeviceIOSource): + """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" + + def __init__( + self, + name: str, + device: OpenXRControllerHapticDevice, + controller_tracker: "ControllerTracker", + ) -> None: + self._device = device + self._controller_tracker = controller_tracker + 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": + """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()) + + def input_spec(self) -> RetargeterIOType: + return {} + + def output_spec(self) -> RetargeterIOType: + 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 " + "(further errors for this side will be silenced): %s", + side, + exc, + ) + self._error_logged[side] = True + 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..f847b3489 100644 --- a/src/plugins/manus/core/manus_hand_tracking_plugin.cpp +++ b/src/plugins/manus/core/manus_hand_tracking_plugin.cpp @@ -105,6 +105,67 @@ 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..377763493 --- /dev/null +++ b/src/plugins/manus/python/manus_haptic_bindings.cpp @@ -0,0 +1,88 @@ +// 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..ae87a8ba6 --- /dev/null +++ b/src/retargeters/tactile_retargeters.py @@ -0,0 +1,1014 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""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 + +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 ``TactileVector(3)`` into a new frame (rotation only). + + 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"``: ``TactileVector(3)`` in the source frame. + - ``"transform"``: ``TransformMatrix`` (4x4); only ``M[:3, :3]`` is read. + + Outputs: + - ``"vec"``: ``TactileVector(3)`` in the target frame. + """ + + 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): + """Weighted sum of N ``TactileVector(3)`` inputs in a common frame. + + Tunable ``weights`` (``VectorParameter`` of length ``num_inputs``, default + all ones) lets an operator attenuate or zero out individual contributing + bodies from the tuning UI. + + Inputs: ``"in_0"`` ... ``"in_{num_inputs - 1}"`` -- each ``TactileVector(3)``. + Outputs: ``"vec"`` -- sum of ``weights[i] * in_i``. + """ + + 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 ``TactileVector(3)`` to a ``TactileVector(1)`` scalar. + + 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 (typically chained after a + ``Vector3FrameTransform`` when the device cares about pressure normal + to a known axis). + """ + + 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 + (defaults ``0.0`` map to ``XR_FREQUENCY_UNSPECIFIED`` / + ``XR_MIN_HAPTIC_DURATION`` on the runtime). + + Tunable parameters: ``gain``, ``deadband``, ``saturation``, + ``frequency_hz``, ``duration_s``. No ``smoothing`` parameter: + ``xrApplyHapticFeedback`` supersedes any in-flight pulse every frame, so + EMA-smoothing in Python only shifts latency. Add an upstream low-pass + retargeter on the ``TactileVector`` input if you need temporal shaping. + """ + + 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 + + +class FingerPowerToControllerPulse(BaseRetargeter): + """Reduce a ``FingerPowerVector`` to one ``ControllerHapticPulse``. + + Bridges per-finger glove output to single-channel controller rumble. + Collapses the channels to a single amplitude via ``reduction``, applies + the same gain / deadband / saturation curve as + :class:`TactileVectorToControllerPulse` (so the rumble can be tuned + independently of the upstream signal), and pairs the result with constant + ``frequency_hz`` / ``duration_s`` parameters. + + Inputs: ``"powers"`` -- ``FingerPowerVector(num_fingers)``. + Outputs: ``"pulse"`` -- ``[amplitude, frequency_hz, duration_s]``. + + Tunable: ``gain``, ``deadband``, ``saturation``, ``frequency_hz``, + ``duration_s``. No ``smoothing`` for the same reason as + :class:`TactileVectorToControllerPulse`. + """ + + INPUT_POWERS = "powers" + OUTPUT_PULSE = "pulse" + + def __init__( + self, + name: str, + num_fingers: int = NUM_HAPTIC_FINGERS, + reduction: Literal["max", "mean", "sum"] = "max", + gain: float = 1.0, + deadband: float = 0.0, + saturation: float = 1.0, + frequency_hz: float = 0.0, + duration_s: float = 0.0, + ) -> None: + if num_fingers < 1: + raise ValueError( + f"FingerPowerToControllerPulse '{name}' requires num_fingers >= 1, got {num_fingers}" + ) + if reduction not in ("max", "mean", "sum"): + raise ValueError( + f"FingerPowerToControllerPulse '{name}': unknown reduction '{reduction}'" + ) + + self._num_fingers = num_fingers + self._reduction = reduction + + self._gain = gain + self._deadband = deadband + self._saturation = saturation + self._frequency_hz = frequency_hz + self._duration_s = duration_s + + param_state = ParameterState( + name, + [ + FloatParameter( + name="gain", + description="Scale factor applied after the deadband.", + default_value=gain, + min_value=0.0, + max_value=100.0, + sync_fn=lambda v: setattr(self, "_gain", float(v)), + ), + FloatParameter( + name="deadband", + description="Amplitude below which the pulse is suppressed.", + default_value=deadband, + min_value=0.0, + max_value=10.0, + sync_fn=lambda v: setattr(self, "_deadband", float(v)), + ), + FloatParameter( + name="saturation", + description="Maximum pulse amplitude in [0, 1].", + default_value=saturation, + min_value=0.0, + max_value=1.0, + sync_fn=lambda v: setattr(self, "_saturation", float(v)), + ), + FloatParameter( + name="frequency_hz", + description="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