diff --git a/.github/actions/setup-v2d-src/action.yml b/.github/actions/setup-v2d-src/action.yml new file mode 100644 index 000000000..62e29de58 --- /dev/null +++ b/.github/actions/setup-v2d-src/action.yml @@ -0,0 +1,100 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +name: Setup robotic_grounding source bundle +description: | + Clone jiwenc-nv/v2d:retargeter at the SHA pinned in deps/v2d/version.txt + and copy `robotic_grounding/` into deps/v2d/src/robotic_grounding/. The + Teleop wheel build (src/core/python/CMakeLists.txt) then bundles that + subtree alongside isaacteleop, so `pip install isaacteleop[grounding]` + on the resulting wheel works without a separate robotic_grounding install. + + Forks without V2D_RETARGETER_TOKEN no-op cleanly: the action exits + without populating deps/v2d/src/, the wheel build skips bundling, and + Sharpa retargeter tests skip via the `_HAS_PINOCCHIO` guard. + +inputs: + github-token: + description: PAT with read access to jiwenc-nv/v2d. Empty on forks; action no-ops then. + required: false + default: '' + +outputs: + bundled: + description: '"true" if deps/v2d/src/robotic_grounding/ was populated, else "false".' + value: ${{ steps.locate.outputs.bundled }} + +runs: + using: composite + steps: + - name: Skip on forks (no token) + if: inputs.github-token == '' + shell: bash + run: | + echo "::warning::V2D_RETARGETER_TOKEN is empty; skipping robotic_grounding source fetch." + echo "Sharpa retargeter tests will skip via the _HAS_PINOCCHIO guard." + + - name: Read pinned V2D ref + id: pin + if: inputs.github-token != '' + shell: bash + run: | + if [ ! -f deps/v2d/version.txt ]; then + echo "::error::deps/v2d/version.txt is missing; cannot pin V2D commit." + exit 1 + fi + SHA=$(grep -vE '^\s*(#|$)' deps/v2d/version.txt | head -1 | tr -d '[:space:]') + if [ -z "$SHA" ]; then + echo "::error::deps/v2d/version.txt contains no SHA." + exit 1 + fi + if ! [[ "$SHA" =~ ^[0-9a-fA-F]{40}$ ]]; then + echo "::error::deps/v2d/version.txt must pin a full 40-char git commit SHA, not a branch or tag (got: ${SHA})." + exit 1 + fi + echo "sha=${SHA}" >> "$GITHUB_OUTPUT" + echo "Pinned V2D ref: ${SHA}" + + - name: Cache robotic_grounding source bundle + id: cache + if: inputs.github-token != '' + uses: actions/cache@v5 + with: + path: deps/v2d/src + # Source bundle is pure files; cache key only depends on V2D SHA. + key: v2d-src-${{ steps.pin.outputs.sha }} + + - name: Clone robotic_grounding source + if: inputs.github-token != '' && steps.cache.outputs.cache-hit != 'true' + shell: bash + env: + GH_TOKEN: ${{ inputs.github-token }} + V2D_REF: ${{ steps.pin.outputs.sha }} + run: | + set -euo pipefail + rm -rf /tmp/v2d deps/v2d/src + mkdir -p deps/v2d/src + + # Whole retargeter branch is ~25 MB so a normal clone is fine. + gh repo clone jiwenc-nv/v2d /tmp/v2d -- --branch retargeter + git -C /tmp/v2d checkout "${V2D_REF}" + + cp -a \ + /tmp/v2d/robotic_grounding/source/robotic_grounding/robotic_grounding \ + deps/v2d/src/robotic_grounding + find deps/v2d/src/robotic_grounding -type d -name __pycache__ \ + -exec rm -rf {} + 2>/dev/null || true + + rm -rf /tmp/v2d + + - name: Locate bundle + id: locate + shell: bash + run: | + if [ -f deps/v2d/src/robotic_grounding/__init__.py ]; then + echo "bundled=true" >> "$GITHUB_OUTPUT" + echo "robotic_grounding/ source present at deps/v2d/src/robotic_grounding/" + else + echo "bundled=false" >> "$GITHUB_OUTPUT" + echo "::warning::deps/v2d/src/robotic_grounding/ is missing; wheel build will skip [grounding]." + fi diff --git a/.github/workflows/build-ubuntu.yml b/.github/workflows/build-ubuntu.yml index 58b17402e..f552d8ae4 100644 --- a/.github/workflows/build-ubuntu.yml +++ b/.github/workflows/build-ubuntu.yml @@ -60,6 +60,22 @@ jobs: with: ngc-api-key: ${{ secrets.NGC_TELEOP_CORE_GITHUB_SERVICE_KEY }} + # robotic_grounding source bundle for the [grounding] extra (Sharpa retargeter). + # Disabled on release branches (and PRs targeting them) to keep release + # artifacts free of V2D source: V2D is private/pre-release and shouldn't + # ship with anything stamped as a release. Also no-ops on forks that + # lack V2D_RETARGETER_TOKEN. In every disabled case the wheel build skips + # bundling robotic_grounding/, and Sharpa tests skip via the + # _HAS_PINOCCHIO guard. Must run BEFORE `Configure CMake` so the wheel + # staging step in src/core/python/CMakeLists.txt picks up + # deps/v2d/src/robotic_grounding/. + - name: Setup robotic_grounding source bundle + id: setup-v2d-src + if: ${{ !startsWith(github.ref_name, 'release/') && !startsWith(github.base_ref, 'release/') }} + uses: ./.github/actions/setup-v2d-src + with: + github-token: ${{ secrets.V2D_RETARGETER_TOKEN }} + # Hunter cache - caches depthai dependencies (OpenSSL, CURL, etc.) - name: Cache Hunter packages uses: actions/cache@v5 @@ -91,7 +107,8 @@ jobs: -DCMAKE_CXX_COMPILER_LAUNCHER=ccache \ -DBUILD_PLUGIN_OAK_CAMERA=ON \ -DBUILD_VIZ=ON \ - -DENABLE_CLOUDXR_BUNDLE_CHECK=ON + -DENABLE_CLOUDXR_BUNDLE_CHECK=ON \ + -DBUNDLE_ROBOTIC_GROUNDING=${{ steps.setup-v2d-src.outputs.bundled || 'false' }} - name: Build run: cmake --build build --parallel 4 diff --git a/.gitignore b/.gitignore index 604fa12ec..220850c71 100644 --- a/.gitignore +++ b/.gitignore @@ -50,3 +50,11 @@ dist/ # SPDX report project.spdx + +# robotic_grounding source bundle (cloned locally / in CI from +# jiwenc-nv/v2d:retargeter; never committed -- repopulated deterministically +# from deps/v2d/version.txt). The Teleop wheel build copies this subtree +# into the wheel staging dir so [grounding] users get robotic_grounding +# without a separate install. V2D source must NOT be tracked here. +deps/v2d/src/ +deps/v2d/wheels/ diff --git a/deps/v2d/version.txt b/deps/v2d/version.txt new file mode 100644 index 000000000..3f2285959 --- /dev/null +++ b/deps/v2d/version.txt @@ -0,0 +1,10 @@ +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Pinned commit on jiwenc-nv/v2d:retargeter that the [grounding] extra +# (Sharpa retargeter) is built against. Edit this single line to upgrade +# the bundled robotic_grounding; rerun scripts/setup_v2d_src.sh after. +# +# See docs/source/references/grounding_extra.rst for the end-to-end flow. + +27657fbbc85af946eaf58484aaf0bf6fead215e3 diff --git a/docs/source/index.rst b/docs/source/index.rst index 76be2079a..8313f9aeb 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -59,7 +59,7 @@ Table of Contents references/requirements references/build device/index - references/retargeting + references/retargeting/index references/mcap_record_replay references/oob_teleop_control references/license diff --git a/docs/source/references/retargeting.rst b/docs/source/references/retargeting/index.rst similarity index 98% rename from docs/source/references/retargeting.rst rename to docs/source/references/retargeting/index.rst index dcaf239f9..81da6c89b 100644 --- a/docs/source/references/retargeting.rst +++ b/docs/source/references/retargeting/index.rst @@ -308,4 +308,10 @@ If the built-in retargeters do not cover your use case, you can implement a cust ``IDeviceIOSource`` subclass for custom input devices. See the `retargeters README `_ -and :doc:`Contributing Guide <../getting_started/contributing>` for details. +and :doc:`Contributing Guide <../../getting_started/contributing>` for details. + +.. toctree:: + :maxdepth: 1 + :caption: Retargeter setup guides + + sharpa diff --git a/docs/source/references/retargeting/sharpa.rst b/docs/source/references/retargeting/sharpa.rst new file mode 100644 index 000000000..4a7bd01c5 --- /dev/null +++ b/docs/source/references/retargeting/sharpa.rst @@ -0,0 +1,216 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +Retargeter: Manus to Sharpa +=========================== + +``SharpaHandRetargeter`` maps live hand-tracking poses from a `Manus glove +`_ (or any other source feeding the OpenXR +hand-tracking layer) onto Sharpa hand joint angles, frame by frame, via +optimization-based inverse kinematics. ``SharpaBiManualRetargeter`` is a +thin combiner that interleaves left and right outputs into a single +target-ordered vector for downstream control. + +At a glance +----------- + +.. list-table:: + :header-rows: 1 + :widths: 22 78 + + * - Stage + - What happens + * - Input + - 26-joint OpenXR ``HandInput`` for the configured side (xyzw quats), + sourced from a Manus glove plugin or any other OpenXR hand-tracking + provider. + * - Repack + - Drop OpenXR palm + non-thumb metacarpals to land on the canonical + MANO 21-joint layout, and convert quaternions to wxyz. + * - IK + - ``robotic_grounding.retarget.hand_kinematics.SharpaHandKinematics`` + runs Pink IK on a Pinocchio model loaded from the Sharpa MJCF, with + a FreeFlyer root that re-anchors the wrist each frame. + * - Warm-start + - Previous-frame qpos is kept and reused (with the wrist re-pinned to + the new tracker reading) to keep the IK locally smooth. A frame + with any invalid input joint zeros the output and resets the + warm-start. + * - Output + - Sharpa finger DOFs (everything Pinocchio reports past the + FreeFlyer), optionally reordered by ``hand_joint_names``. + +The retargeter intentionally contains no IK math itself: joint orderings, +frame mappings, rotation corrections, and Pink/Pinocchio configuration all +live in ``robotic_grounding`` (V2D). This module is the OpenXR-shaped +adapter on top of it. + +.. seealso:: + + :doc:`/device/manus` -- installing the Manus plugin so its tracking + shows up on the OpenXR hand layer that this retargeter consumes. + + :doc:`index` -- the broader retargeting interface and pipeline-builder + pattern. + +Why the ``[grounding]`` extra exists +------------------------------------ + +The Sharpa kinematics model, MJCFs, meshes, and the IK setup that this +retargeter calls all live in the ``robotic_grounding`` package (part of +V2D). V2D is on track to be fully open sourced; until then its source +isn't public, so the wheel has two build modes: + +* **Default** — wheel ships without ``robotic_grounding``. Sharpa + retargeter imports skip cleanly, so forks and OSS contributors can + build and use the rest of Teleop unaffected. +* **With** ``-DBUNDLE_ROBOTIC_GROUNDING=TRUE`` — the build pulls + ``robotic_grounding`` from the pinned SHA in ``deps/v2d/version.txt`` + and bundles it into the wheel. Installing ``isaacteleop[grounding]`` + then resolves all imports, and the Sharpa MJCFs ship with the wheel. + +.. note:: + + ``-DBUNDLE_ROBOTIC_GROUNDING=TRUE`` is a temporary bridge. Once V2D is + fully open sourced, ``robotic_grounding`` will be a normal public + dependency of the ``[grounding]`` extra and the bundling flag (along + with ``scripts/setup_v2d_src.sh`` and the ``V2D_RETARGETER_TOKEN`` + gating in CI) will go away. + +The next two sections cover that opt-in build, how to use the retargeter +once the extra is in place, and how to verify the install. + +Build the ``[grounding]`` extra +------------------------------- + +Prerequisites: + +* `gh CLI `_ installed and ``gh auth login``\ 'd + with read access to ``jiwenc-nv/v2d``. +* A configured Teleop build tree. + +.. code-block:: console + + $ scripts/setup_v2d_src.sh + $ cmake -B build -DBUNDLE_ROBOTIC_GROUNDING=TRUE + $ cmake --build build --target python_wheel + $ uv pip install -e .[grounding] + +The first command populates ``deps/v2d/src/robotic_grounding/`` from the +SHA pinned in ``deps/v2d/version.txt``. The CMake flag tells the wheel +build to bundle that subtree alongside ``isaacteleop``. + +If the wheel was built without ``-DBUNDLE_ROBOTIC_GROUNDING=TRUE``, the +import raises ``ModuleNotFoundError`` with a pointer back to +``scripts/setup_v2d_src.sh``. + +Use it from Python +------------------ + +.. code-block:: python + + from isaacteleop.retargeters import ( + SharpaHandRetargeter, + SharpaHandRetargeterConfig, + ) + +The Sharpa MJCFs and meshes ship inside the bundled ``robotic_grounding`` +package -- resolve them with ``importlib.resources``: + +.. code-block:: python + + from importlib.resources import files + + xml_dir = files("robotic_grounding") / "assets" / "xmls" / "sharpawave" + right_mjcf = str(xml_dir / "right_sharpawave_nomesh.xml") # mesh-free, fast + # or "right_sharpawave.xml" if you also have the STL meshes + + cfg = SharpaHandRetargeterConfig(hand_side="right", robot_asset_path=right_mjcf) + retargeter = SharpaHandRetargeter(cfg, name="sharpa_right") + +Key ``SharpaHandRetargeterConfig`` fields: + +* ``robot_asset_path`` — the Sharpa MJCF path (``..._nomesh.xml`` is the + mesh-free variant used in tests and on machines without the STLs). +* ``hand_side`` — ``"left"`` or ``"right"``. +* ``hand_joint_names`` — optional output ordering override; defaults to + whatever finger joints Pinocchio discovers in the MJCF, in model order. +* ``source_to_robot_scale`` — MANO-to-robot length scale. +* ``solver`` / ``max_iter`` / ``frequency`` / + ``frame_tasks_converged_threshold`` — Pink IK knobs forwarded to + ``SharpaHandKinematics``. + +For bimanual control, instantiate two ``SharpaHandRetargeter``\ s and +wrap them with ``SharpaBiManualRetargeter`` so a single output vector is +produced in your target joint order. + +Run the example +--------------- + +The repo ships a bimanual demo at +``examples/retargeting/python/sharpa_hand_retargeter_demo.py``: + +.. code-block:: console + + # Synthetic curl animation (no headset, no GUI required): + $ python examples/retargeting/python/sharpa_hand_retargeter_demo.py --synthetic + + # Live bimanual from a connected Quest headset: + $ python examples/retargeting/python/sharpa_hand_retargeter_demo.py + + # Custom MJCFs (e.g. the mesh-bearing variants): + $ python examples/retargeting/python/sharpa_hand_retargeter_demo.py \ + --left-mjcf /path/to/left_sharpawave.xml \ + --right-mjcf /path/to/right_sharpawave.xml + +The synthetic mode is the smoke test: if it animates a curl trajectory +and prints non-zero finger qpos each frame, the install is good. + +Validate +-------- + +Two checks; either by itself is sufficient. + +**End-to-end pytest** -- exercises the full Pinocchio + Pink IK pipeline +through the Teleop wrapper (init, warm-start persistence, open vs. curled +hand, absent-hand zeros, etc.): + +.. code-block:: console + + $ ctest --test-dir build -R retargeting_test_sharpa_hand_retargeter --output-on-failure + ... + 100% tests passed, 0 tests failed out of 1 + +The ``Test command`` line printed by ``ctest -V`` should include +``--extra grounding``. If it doesn't, the wheel build skipped bundling -- +re-check that ``cmake -B build`` was invoked with +``-DBUNDLE_ROBOTIC_GROUNDING=TRUE`` after running ``setup_v2d_src.sh``. + +**Full retargeting suite** -- regression coverage in case the wrapper +introduced a typing or import regression elsewhere: + +.. code-block:: console + + $ ctest --test-dir build -R '^retargeting_' --output-on-failure + ... + 100% tests passed, 0 tests failed out of 16 + +CI +-- + +The workflow at ``.github/workflows/build-ubuntu.yml`` runs the same flow +via the ``setup-v2d-src`` composite action, gated on the +``V2D_RETARGETER_TOKEN`` repo secret (a PAT scoped read-only to +``jiwenc-nv/v2d``). The action sets ``-DBUNDLE_ROBOTIC_GROUNDING`` from +its own ``bundled`` output; on forks without the secret the action no-ops +and the flag is ``false``. + +Public artifact safety: a Release-only step strips ``robotic_grounding/`` +out of every wheel before ``actions/upload-artifact`` runs, so V2D source +never reaches the public artifact channel. + +Bumping the bundled ``robotic_grounding`` +----------------------------------------- + +Edit the SHA in ``deps/v2d/version.txt`` and rerun +``scripts/setup_v2d_src.sh``. diff --git a/examples/retargeting/python/sharpa_hand_retargeter_demo.py b/examples/retargeting/python/sharpa_hand_retargeter_demo.py new file mode 100755 index 000000000..f75a7df4c --- /dev/null +++ b/examples/retargeting/python/sharpa_hand_retargeter_demo.py @@ -0,0 +1,348 @@ +#!/usr/bin/env python3 +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Sharpa Hand Retargeter Demo — bimanual teleop sample app. + +Two modes: + + Live (default): + Reads OpenXR hand tracking from a Quest headset via TeleopSession and + feeds both hands through SharpaHandRetargeter IK in real time. + + Synthetic (--synthetic): + Animates a curl sequence on a single right hand with no headset required. + +Usage: + # Live bimanual from Quest headset: + python sharpa_hand_retargeter_demo.py + + # Synthetic animation (no headset): + python sharpa_hand_retargeter_demo.py --synthetic + + # Custom MJCF paths: + python sharpa_hand_retargeter_demo.py --left-mjcf /path/to/left.xml --right-mjcf /path/to/right.xml +""" + +import argparse +import sys +import time + +import numpy as np + +from isaacteleop.retargeters import SharpaHandRetargeter, SharpaHandRetargeterConfig + + +def _default_mjcf(name: str) -> str: + """Resolve a Sharpa MJCF shipped inside the robotic_grounding wheel. + + The `[grounding]` extra requires `robotic_grounding` to be installed + (see src/retargeters/README.md). If the user invokes this demo without + it, the import of SharpaHandRetargeter above already raised a clear + error long before this function is called. + """ + from importlib.resources import files + + return str(files("robotic_grounding") / "assets" / "xmls" / "sharpawave" / name) + + +DEFAULT_LEFT_MJCF = _default_mjcf("left_sharpawave_nomesh.xml") +DEFAULT_RIGHT_MJCF = _default_mjcf("right_sharpawave_nomesh.xml") + + +# --------------------------------------------------------------------------- +# Synthetic mode helpers (no headset) +# --------------------------------------------------------------------------- + + +def _run_synthetic(mjcf_path: str) -> int: + from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + ExecutionEvents, + ExecutionState, + OptionalTensorGroup, + TensorGroup, + ) + from isaacteleop.retargeting_engine.interface.retargeter_core_types import GraphTime + from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalTensorGroupType, + ) + from isaacteleop.retargeting_engine.tensor_types import ( + HandInput, + HandInputIndex, + HandJointIndex, + NUM_HAND_JOINTS, + ) + + ID_QUAT = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) + + def _make_context() -> ComputeContext: + return ComputeContext( + graph_time=GraphTime(sim_time_ns=0, real_time_ns=0), + execution_events=ExecutionEvents( + reset=False, execution_state=ExecutionState.RUNNING + ), + ) + + def _build_io(retargeter): + inputs = {} + for k, v in retargeter.input_spec().items(): + if isinstance(v, OptionalTensorGroupType): + inputs[k] = OptionalTensorGroup(v) + else: + inputs[k] = TensorGroup(v) + outputs = {} + for k, v in retargeter.output_spec().items(): + if isinstance(v, OptionalTensorGroupType): + outputs[k] = OptionalTensorGroup(v) + else: + outputs[k] = TensorGroup(v) + return inputs, outputs + + def make_hand_pose(curl_factor: float = 0.0) -> TensorGroup: + tg = TensorGroup(HandInput()) + positions = np.zeros((NUM_HAND_JOINTS, 3), dtype=np.float32) + orientations = np.tile(ID_QUAT, (NUM_HAND_JOINTS, 1)) + valid = np.ones(NUM_HAND_JOINTS, dtype=np.uint8) + + positions[HandJointIndex.WRIST] = [0, 0, 0] + positions[HandJointIndex.THUMB_METACARPAL] = [0.02, 0.02, 0] + positions[HandJointIndex.THUMB_PROXIMAL] = [0.04, 0.04, 0] + dx = -0.01 * curl_factor + dz = -0.015 * curl_factor + positions[HandJointIndex.THUMB_DISTAL] = [0.06 + dx, 0.06, dz] + positions[HandJointIndex.THUMB_TIP] = [0.07 + 2 * dx, 0.07, 2 * dz] + + finger_specs = [ + ( + HandJointIndex.INDEX_PROXIMAL, + HandJointIndex.INDEX_INTERMEDIATE, + HandJointIndex.INDEX_DISTAL, + HandJointIndex.INDEX_TIP, + 0.03, + ), + ( + HandJointIndex.MIDDLE_PROXIMAL, + HandJointIndex.MIDDLE_INTERMEDIATE, + HandJointIndex.MIDDLE_DISTAL, + HandJointIndex.MIDDLE_TIP, + 0.01, + ), + ( + HandJointIndex.RING_PROXIMAL, + HandJointIndex.RING_INTERMEDIATE, + HandJointIndex.RING_DISTAL, + HandJointIndex.RING_TIP, + -0.01, + ), + ( + HandJointIndex.LITTLE_PROXIMAL, + HandJointIndex.LITTLE_INTERMEDIATE, + HandJointIndex.LITTLE_DISTAL, + HandJointIndex.LITTLE_TIP, + -0.03, + ), + ] + for prox, inter, dist, tip, y_offset in finger_specs: + positions[prox] = [0, y_offset, 0.04] + positions[inter] = [0, y_offset, 0.07] + cx = -0.02 * curl_factor + positions[dist] = [cx, y_offset, 0.09 - 0.01 * curl_factor] + positions[tip] = [2 * cx, y_offset, 0.09 - 0.03 * curl_factor] + + tg[HandInputIndex.JOINT_POSITIONS] = positions + tg[HandInputIndex.JOINT_ORIENTATIONS] = orientations + tg[HandInputIndex.JOINT_RADII] = ( + np.ones(NUM_HAND_JOINTS, dtype=np.float32) * 0.01 + ) + tg[HandInputIndex.JOINT_VALID] = valid + return tg + + print(f"[1] Loading MJCF: {mjcf_path}") + cfg = SharpaHandRetargeterConfig( + robot_asset_path=mjcf_path, + hand_side="right", + max_iter=100, + frequency=200.0, + ) + retargeter = SharpaHandRetargeter(cfg, name="sharpa_demo") + + joint_names = [t.name for t in retargeter.output_spec()["hand_joints"].types] + print(f" {len(joint_names)} finger joints loaded.") + print() + + n_frames = 20 + curl_sequence = np.concatenate( + [ + np.linspace(0.0, 1.0, n_frames), + np.linspace(1.0, 0.0, n_frames), + ] + ) + + print(f"[2] Running {len(curl_sequence)} frames (open -> curled -> open)...") + header = ( + f"{'frame':>5} {'curl':>5} {'dt':>7} " + + " ".join(f"{n[:12]:>12}" for n in joint_names[:6]) + + " ..." + ) + print(header) + print("-" * len(header)) + + for idx, curl in enumerate(curl_sequence): + inputs, outputs = _build_io(retargeter) + inputs["hand_right"] = make_hand_pose(curl) + t0 = time.perf_counter() + retargeter.compute(inputs, outputs, _make_context()) + dt_ms = (time.perf_counter() - t0) * 1000.0 + vals = " ".join( + f"{float(outputs['hand_joints'][i]):12.4f}" + for i in range(min(6, len(joint_names))) + ) + print(f"{idx:5d} {curl:5.2f} {dt_ms:5.1f}ms {vals} ...") + + print("\n[3] Done.") + return 0 + + +# --------------------------------------------------------------------------- +# Live bimanual mode (Quest headset) +# --------------------------------------------------------------------------- + + +def _run_live(left_mjcf: str, right_mjcf: str, duration: float) -> int: + from isaacteleop.retargeting_engine.deviceio_source_nodes import HandsSource + from isaacteleop.retargeting_engine.interface import OutputCombiner + from isaacteleop.teleop_session_manager import TeleopSession, TeleopSessionConfig + + print("[1] Loading MJCFs...") + print(f" Left : {left_mjcf}") + print(f" Right: {right_mjcf}") + + hands = HandsSource(name="hands") + + left_cfg = SharpaHandRetargeterConfig( + robot_asset_path=left_mjcf, + hand_side="left", + max_iter=100, + frequency=200.0, + ) + right_cfg = SharpaHandRetargeterConfig( + robot_asset_path=right_mjcf, + hand_side="right", + max_iter=100, + frequency=200.0, + ) + left_retargeter = SharpaHandRetargeter(left_cfg, name="sharpa_left") + right_retargeter = SharpaHandRetargeter(right_cfg, name="sharpa_right") + + left_joint_names = [ + t.name for t in left_retargeter.output_spec()["hand_joints"].types + ] + right_joint_names = [ + t.name for t in right_retargeter.output_spec()["hand_joints"].types + ] + print(f" Left joints ({len(left_joint_names)}): {left_joint_names[:3]} ...") + print(f" Right joints ({len(right_joint_names)}): {right_joint_names[:3]} ...") + print() + + connected_left = left_retargeter.connect( + {HandsSource.LEFT: hands.output(HandsSource.LEFT)} + ) + connected_right = right_retargeter.connect( + {HandsSource.RIGHT: hands.output(HandsSource.RIGHT)} + ) + + pipeline = OutputCombiner( + { + "left_hand_joints": connected_left.output("hand_joints"), + "right_hand_joints": connected_right.output("hand_joints"), + } + ) + + session_config = TeleopSessionConfig( + app_name="SharpaHandBiManualDemo", + trackers=[], + pipeline=pipeline, + ) + + print(f"[2] Starting TeleopSession (duration={duration:.0f}s)...") + print(" Waiting for Quest hand tracking...") + print() + + with TeleopSession(session_config) as session: + start_time = time.time() + + while time.time() - start_time < duration: + result = session.step() + + left_vals = list(result["left_hand_joints"]) + right_vals = list(result["right_hand_joints"]) + + if session.frame_count % 30 == 0: + elapsed = session.get_elapsed_time() + l_str = ", ".join(f"{v:.3f}" for v in left_vals[:4]) + r_str = ", ".join(f"{v:.3f}" for v in right_vals[:4]) + print( + f"[{elapsed:6.1f}s] frame {session.frame_count:5d} " + f"L: [{l_str}, ...] R: [{r_str}, ...]" + ) + + time.sleep(0.016) + + print("\n[3] Done.") + return 0 + + +# --------------------------------------------------------------------------- +# Entry point +# --------------------------------------------------------------------------- + + +def main(): + parser = argparse.ArgumentParser( + description="Sharpa Hand Retargeter Demo (bimanual)", + ) + parser.add_argument( + "--synthetic", + action="store_true", + help="Run synthetic curl animation instead of live Quest input.", + ) + parser.add_argument( + "--left-mjcf", + type=str, + default=DEFAULT_LEFT_MJCF, + help="Path to the left-hand Sharpa MJCF XML file.", + ) + parser.add_argument( + "--right-mjcf", + type=str, + default=DEFAULT_RIGHT_MJCF, + help="Path to the right-hand Sharpa MJCF XML file.", + ) + parser.add_argument( + "--duration", + type=float, + default=60.0, + help="Duration in seconds for the live session (default: 60).", + ) + args = parser.parse_args() + + print() + print("=" * 70) + print(" Sharpa Hand Retargeter Demo (Bimanual)") + print("=" * 70) + print() + + if args.synthetic: + print(" Mode: SYNTHETIC (no headset)") + print() + return _run_synthetic(args.right_mjcf) + else: + print(" Mode: LIVE (Quest hand tracking)") + print() + return _run_live(args.left_mjcf, args.right_mjcf, args.duration) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/scripts/setup_v2d_src.sh b/scripts/setup_v2d_src.sh new file mode 100755 index 000000000..d2e51613b --- /dev/null +++ b/scripts/setup_v2d_src.sh @@ -0,0 +1,68 @@ +#!/usr/bin/env bash +# SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Populate deps/v2d/src/robotic_grounding/ with V2D's robotic_grounding +# Python package at the SHA pinned in deps/v2d/version.txt. +# +# CMake (src/core/python/CMakeLists.txt) detects deps/v2d/src/robotic_grounding/ +# at configure time and, if present, copies the subtree into the wheel +# staging dir. The resulting Teleop wheel ships robotic_grounding alongside +# isaacteleop, so `pip install isaacteleop[grounding]` Just Works -- no +# separate wheel install. deps/v2d/src/ is gitignored so V2D source never +# enters Teleop's git history. +# +# Requires: +# * gh CLI installed and `gh auth login`'d with read access to +# jiwenc-nv/v2d (or whichever fork holds the retargeter branch). +# +# Usage: +# scripts/setup_v2d_src.sh +# cmake -B build && cmake --build build # picks up deps/v2d/src/ + +set -euo pipefail + +REPO_ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)" +cd "${REPO_ROOT}" + +VERSION_FILE="deps/v2d/version.txt" +if [ ! -f "${VERSION_FILE}" ]; then + echo "error: ${VERSION_FILE} is missing." >&2 + exit 1 +fi + +V2D_REF=$(grep -vE '^\s*(#|$)' "${VERSION_FILE}" | head -1 | tr -d '[:space:]') +if [ -z "${V2D_REF}" ]; then + echo "error: ${VERSION_FILE} contains no SHA." >&2 + exit 1 +fi +if ! [[ "${V2D_REF}" =~ ^[0-9a-fA-F]{40}$ ]]; then + echo "error: ${VERSION_FILE} must pin a full 40-char git commit SHA, not a branch or tag (got: ${V2D_REF})." >&2 + exit 1 +fi +echo "Pinned V2D ref: ${V2D_REF}" + +if ! command -v gh >/dev/null 2>&1; then + echo "error: gh CLI not found. Install from https://cli.github.com/" >&2 + echo " and run 'gh auth login' with read access to jiwenc-nv/v2d." >&2 + exit 1 +fi + +DST="deps/v2d/src/robotic_grounding" +mkdir -p deps/v2d/src +rm -rf "${DST}" + +# Whole retargeter branch is ~25 MB so a normal clone is fine. Clone the +# branch tip then check out the exact pinned SHA. +WORK_DIR=$(mktemp -d -t v2d-src.XXXXXX) +trap 'rm -rf "${WORK_DIR}"' EXIT +gh repo clone jiwenc-nv/v2d "${WORK_DIR}/v2d" -- --branch retargeter +git -C "${WORK_DIR}/v2d" checkout "${V2D_REF}" + +cp -a \ + "${WORK_DIR}/v2d/robotic_grounding/source/robotic_grounding/robotic_grounding" \ + "${DST}" +# Drop bytecode caches if any slipped through. +find "${DST}" -type d -name __pycache__ -exec rm -rf {} + 2>/dev/null || true + +echo "Done -> ${DST} (cloned from jiwenc-nv/v2d @ ${V2D_REF})" diff --git a/src/core/python/CMakeLists.txt b/src/core/python/CMakeLists.txt index 09b374265..cf06dd870 100644 --- a/src/core/python/CMakeLists.txt +++ b/src/core/python/CMakeLists.txt @@ -4,6 +4,31 @@ # Python wheel packaging (combines OXR, DEVICEIO, MCAP, PluginManager, and Schema modules). +# Opt-in: bundle deps/v2d/src/robotic_grounding/ into the wheel so +# `pip install isaacteleop[grounding]` resolves the Sharpa retargeter. +# Populate the source via scripts/setup_v2d_src.sh first; then configure +# with -DBUNDLE_ROBOTIC_GROUNDING=TRUE. Default OFF -- forks / OSS builds +# produce a wheel without robotic_grounding, and Sharpa imports skip. +option(BUNDLE_ROBOTIC_GROUNDING "Bundle V2D's robotic_grounding into the wheel" OFF) + +set(ROBOTIC_GROUNDING_SRC "${CMAKE_SOURCE_DIR}/deps/v2d/src/robotic_grounding") +if(BUNDLE_ROBOTIC_GROUNDING) + if(NOT EXISTS "${ROBOTIC_GROUNDING_SRC}/__init__.py") + message(FATAL_ERROR + "BUNDLE_ROBOTIC_GROUNDING=TRUE but ${ROBOTIC_GROUNDING_SRC}/ " + "is missing. Run scripts/setup_v2d_src.sh first.") + endif() + # Entries injected into pyproject.toml.in's `packages = [...]`. + set(ROBOTIC_GROUNDING_PACKAGES_BLOCK + " \"robotic_grounding\",\n \"robotic_grounding.retarget\",\n") + set(ROBOTIC_GROUNDING_PACKAGE_DATA_BLOCK + "robotic_grounding = [\"assets/xmls/sharpawave/*.xml\"]") + message(STATUS "[grounding] bundling robotic_grounding/ from ${ROBOTIC_GROUNDING_SRC}") +else() + set(ROBOTIC_GROUNDING_PACKAGES_BLOCK "") + set(ROBOTIC_GROUNDING_PACKAGE_DATA_BLOCK "") +endif() + # Generate pyproject.toml from template with ISAAC_TELEOP_PYPROJECT_VERSION configure_file( "${CMAKE_CURRENT_SOURCE_DIR}/pyproject.toml.in" @@ -43,10 +68,25 @@ add_custom_target(python_package ALL COMMAND ${CMAKE_COMMAND} -E copy "${CMAKE_CURRENT_SOURCE_DIR}/requirements-cloudxr.txt" "${CMAKE_BINARY_DIR}/python_package/$/" 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 COMMENT "Preparing Python package structure" ) +# When deps/v2d/src/robotic_grounding/ is populated (script or CI action), +# stage it into the wheel staging dir so the resulting Teleop wheel ships +# robotic_grounding alongside isaacteleop. +if(BUNDLE_ROBOTIC_GROUNDING) + add_custom_command(TARGET python_package POST_BUILD + COMMAND ${CMAKE_COMMAND} -E rm -rf + "${CMAKE_BINARY_DIR}/python_package/$/robotic_grounding" + COMMAND ${CMAKE_COMMAND} -E copy_directory + "${ROBOTIC_GROUNDING_SRC}" + "${CMAKE_BINARY_DIR}/python_package/$/robotic_grounding" + COMMENT "Bundling robotic_grounding/ into wheel staging dir for [grounding] extra" + ) +endif() + # Generate Python type stubs (.pyi) for IDE intellisense # Uses the same Python version that built the .so files (via uv --python) # Each module is processed separately to avoid import chain issues diff --git a/src/core/python/pyproject.toml.in b/src/core/python/pyproject.toml.in index a77d45630..dce159e0e 100644 --- a/src/core/python/pyproject.toml.in +++ b/src/core/python/pyproject.toml.in @@ -30,6 +30,7 @@ optional-dependencies.dev = {file = ["requirements-dev.txt"]} optional-dependencies.ui = {file = ["requirements-ui.txt"]} optional-dependencies.retargeters = {file = ["requirements-retargeters.txt"]} optional-dependencies.retargeters-lite = {file = ["requirements-retargeters-lite.txt"]} +optional-dependencies.grounding = {file = ["requirements-grounding.txt"]} optional-dependencies.cloudxr = {file = ["requirements-cloudxr.txt"]} [tool.setuptools] @@ -52,7 +53,7 @@ packages = [ "isaacteleop.retargeting_engine_ui", "isaacteleop.teleop_session_manager", "isaacteleop.cloudxr", -] +@ROBOTIC_GROUNDING_PACKAGES_BLOCK@] include-package-data = false [tool.setuptools.package-data] @@ -65,6 +66,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"] +@ROBOTIC_GROUNDING_PACKAGE_DATA_BLOCK@ [tool.uv] # Only use managed Python installations (not system Python) diff --git a/src/core/python/requirements-grounding.txt b/src/core/python/requirements-grounding.txt new file mode 100644 index 000000000..c1b037a62 --- /dev/null +++ b/src/core/python/requirements-grounding.txt @@ -0,0 +1,20 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Runtime deps for retargeters that consume `robotic_grounding` +# (e.g. SharpaHandRetargeter -- IK-based via Pinocchio + Pink). +# +# `robotic_grounding` itself is NOT listed here. The Teleop wheel build +# bundles the package source from deps/v2d/src/robotic_grounding/ when V2D +# access is available (see scripts/setup_v2d_src.sh and +# .github/actions/setup-v2d-src/), so `pip install isaacteleop[grounding]` +# pulls only the open-source deps below; `import robotic_grounding...` is +# satisfied by the bundled source. Wheels built without V2D access don't +# include robotic_grounding/, and Sharpa retargeter imports skip cleanly +# via the lazy-import wrapper in src/retargeters/__init__.py. + +# Sharpa hand retargeter (Pinocchio + Pink IK) +pin>=2.7.0 +pin-pink>=4.0.0 +loop-rate-limiters>=1.0.0 +daqp>=0.5.0 diff --git a/src/core/python/requirements-retargeters.txt b/src/core/python/requirements-retargeters.txt index f37fbf414..bb539f7ab 100644 --- a/src/core/python/requirements-retargeters.txt +++ b/src/core/python/requirements-retargeters.txt @@ -1,3 +1,6 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + dex-retargeting>=0.4.6,<0.6.0 scipy>=1.15.0 pyyaml>=6.0.3 @@ -6,3 +9,8 @@ torch>=2.7.0 # nlopt: architecture-specific so x86_64 can use pre-built wheels; arm builds from sdist nlopt==2.6.2; platform_machine == "aarch64" or platform_machine == "arm64" nlopt>=2.6.2; platform_machine != "aarch64" and platform_machine != "arm64" + +# NOTE: The Sharpa retargeter's Pinocchio/Pink/daqp/loop-rate-limiters deps live +# in `requirements-grounding.txt`, transitively via the `robotic_grounding` +# wheel. They were briefly listed here while sharpa_hand_retargeter.py imported +# pinocchio/pink directly; that is no longer the case. diff --git a/src/core/retargeting_engine_tests/python/CMakeLists.txt b/src/core/retargeting_engine_tests/python/CMakeLists.txt index 75e0c6471..5feb90623 100644 --- a/src/core/retargeting_engine_tests/python/CMakeLists.txt +++ b/src/core/retargeting_engine_tests/python/CMakeLists.txt @@ -1,4 +1,4 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 # ============================================================================== @@ -14,15 +14,33 @@ file(GLOB TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/test_*.py" ) +# Sharpa test needs the robotic_grounding runtime deps (pin, pin-pink, ...) that +# live in the [grounding] extra. The robotic_grounding package itself is staged +# into ${CMAKE_BINARY_DIR}/python_package// alongside isaacteleop by +# src/core/python/CMakeLists.txt when -DBUNDLE_ROBOTIC_GROUNDING=TRUE. Gating +# --extra grounding on the same flag keeps the test invocation in lockstep +# with the wheel build. Without bundling, the test still runs but skips via +# the _HAS_PINOCCHIO guard. +set(GROUNDING_EXTRA_ARGS "") +if(BUNDLE_ROBOTIC_GROUNDING) + set(GROUNDING_EXTRA_ARGS --extra grounding) +endif() + # Add each test file as a separate CTest test foreach(test_file ${TEST_FILES}) # Get test name from filename (remove .py and path) get_filename_component(test_name "${test_file}" NAME_WE) + # Sharpa test alone needs the [grounding] runtime deps; other tests don't. + set(extra_uv_args "") + if(test_name STREQUAL "test_sharpa_hand_retargeter") + set(extra_uv_args ${GROUNDING_EXTRA_ARGS}) + endif() + # Add the test add_test( NAME "retargeting_${test_name}" - COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev pytest -v --tb=short "${test_file}" + COMMAND uv run --python ${ISAAC_TELEOP_PYTHON_VERSION} --extra dev ${extra_uv_args} pytest -v --tb=short "${test_file}" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" ) diff --git a/src/core/retargeting_engine_tests/python/pyproject.toml b/src/core/retargeting_engine_tests/python/pyproject.toml index 4c80380f7..4bd4b5fdf 100644 --- a/src/core/retargeting_engine_tests/python/pyproject.toml +++ b/src/core/retargeting_engine_tests/python/pyproject.toml @@ -21,6 +21,18 @@ test = [ "numpy", "scipy", ] +# Mirrors src/core/python/requirements-grounding.txt. The Sharpa unit test +# imports robotic_grounding (staged into PYTHONPATH from the wheel build); +# its runtime deps come from this extra. +grounding = [ + "numpy", + "scipy", + "torch>=2.7.0", + "pin>=2.7", + "pin-pink>=4.0", + "loop-rate-limiters", + "daqp", +] [tool.pytest.ini_options] pythonpath = ["."] diff --git a/src/core/retargeting_engine_tests/python/test_sharpa_hand_retargeter.py b/src/core/retargeting_engine_tests/python/test_sharpa_hand_retargeter.py new file mode 100644 index 000000000..6a14c8f6f --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_sharpa_hand_retargeter.py @@ -0,0 +1,338 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Tests for SharpaHandRetargeter. + +Resolves a stripped-down MJCF (no mesh references) from the +``robotic_grounding`` wheel via ``importlib.resources``. Requires +the ``grounding`` optional extra; tests skip cleanly otherwise. +""" + +import numpy as np +import pytest + +from isaacteleop.retargeting_engine.interface import ( + ComputeContext, + ExecutionEvents, + ExecutionState, + OptionalTensorGroup, + TensorGroup, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import GraphTime +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalTensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + HandInput, + HandInputIndex, + HandJointIndex, + NUM_HAND_JOINTS, +) + +_HAS_PINOCCHIO = True +try: + from isaacteleop.retargeters import ( + SharpaHandRetargeter, + SharpaHandRetargeterConfig, + ) +except ModuleNotFoundError: + _HAS_PINOCCHIO = False + +_requires_pinocchio = pytest.mark.skipif( + not _HAS_PINOCCHIO, + reason=( + "requires robotic_grounding (pip install 'isaacteleop[grounding]' + " + "scripts/setup_v2d_src.sh, see src/retargeters/README.md)" + ), +) + +# --------------------------------------------------------------------------- +# Paths +# --------------------------------------------------------------------------- + +# Sharpa MJCFs ship inside the robotic_grounding wheel as package_data. +# The _nomesh variant strips / blocks so Pinocchio can load the +# model without the 24 MB of LFS-tracked STL meshes -- exactly what we need +# for hermetic unit tests. +if _HAS_PINOCCHIO: + from importlib.resources import files as _files + + SHARPA_MJCF = str( + _files("robotic_grounding") + / "assets" + / "xmls" + / "sharpawave" + / "right_sharpawave_nomesh.xml" + ) +else: + SHARPA_MJCF = "" # tests are skipped via _HAS_PINOCCHIO anyway + +# --------------------------------------------------------------------------- +# Helpers (mirror test_dual_input_retargeters.py patterns) +# --------------------------------------------------------------------------- + +ID_QUAT = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) + + +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_io(retargeter): + inputs = {} + for k, v in retargeter.input_spec().items(): + if isinstance(v, OptionalTensorGroupType): + inputs[k] = OptionalTensorGroup(v) + else: + inputs[k] = TensorGroup(v) + outputs = {} + for k, v in retargeter.output_spec().items(): + if isinstance(v, OptionalTensorGroupType): + outputs[k] = OptionalTensorGroup(v) + else: + outputs[k] = TensorGroup(v) + return inputs, outputs + + +def _make_hand_input_open() -> TensorGroup: + """Build a straight / open hand pose at the origin.""" + tg = TensorGroup(HandInput()) + positions = np.zeros((NUM_HAND_JOINTS, 3), dtype=np.float32) + orientations = np.tile(ID_QUAT, (NUM_HAND_JOINTS, 1)) + valid = np.ones(NUM_HAND_JOINTS, dtype=np.uint8) + + # Wrist at origin + positions[HandJointIndex.WRIST] = [0, 0, 0] + + # Thumb: straight along +X +Y + positions[HandJointIndex.THUMB_METACARPAL] = [0.02, 0.02, 0] + positions[HandJointIndex.THUMB_PROXIMAL] = [0.04, 0.04, 0] + positions[HandJointIndex.THUMB_DISTAL] = [0.06, 0.06, 0] + positions[HandJointIndex.THUMB_TIP] = [0.08, 0.08, 0] + + # Index: straight along +Z + positions[HandJointIndex.INDEX_PROXIMAL] = [0, 0.03, 0.04] + positions[HandJointIndex.INDEX_INTERMEDIATE] = [0, 0.03, 0.07] + positions[HandJointIndex.INDEX_DISTAL] = [0, 0.03, 0.10] + positions[HandJointIndex.INDEX_TIP] = [0, 0.03, 0.13] + + # Middle + positions[HandJointIndex.MIDDLE_PROXIMAL] = [0, 0.01, 0.04] + positions[HandJointIndex.MIDDLE_INTERMEDIATE] = [0, 0.01, 0.07] + positions[HandJointIndex.MIDDLE_DISTAL] = [0, 0.01, 0.10] + positions[HandJointIndex.MIDDLE_TIP] = [0, 0.01, 0.13] + + # Ring + positions[HandJointIndex.RING_PROXIMAL] = [0, -0.01, 0.04] + positions[HandJointIndex.RING_INTERMEDIATE] = [0, -0.01, 0.07] + positions[HandJointIndex.RING_DISTAL] = [0, -0.01, 0.10] + positions[HandJointIndex.RING_TIP] = [0, -0.01, 0.13] + + # Pinky + positions[HandJointIndex.LITTLE_PROXIMAL] = [0, -0.03, 0.04] + positions[HandJointIndex.LITTLE_INTERMEDIATE] = [0, -0.03, 0.07] + positions[HandJointIndex.LITTLE_DISTAL] = [0, -0.03, 0.10] + positions[HandJointIndex.LITTLE_TIP] = [0, -0.03, 0.13] + + tg[HandInputIndex.JOINT_POSITIONS] = positions + tg[HandInputIndex.JOINT_ORIENTATIONS] = orientations + tg[HandInputIndex.JOINT_RADII] = np.ones(NUM_HAND_JOINTS, dtype=np.float32) * 0.01 + tg[HandInputIndex.JOINT_VALID] = valid + return tg + + +def _make_hand_input_curled() -> TensorGroup: + """Build a hand pose with curled fingers.""" + tg = TensorGroup(HandInput()) + positions = np.zeros((NUM_HAND_JOINTS, 3), dtype=np.float32) + orientations = np.tile(ID_QUAT, (NUM_HAND_JOINTS, 1)) + valid = np.ones(NUM_HAND_JOINTS, dtype=np.uint8) + + positions[HandJointIndex.WRIST] = [0, 0, 0] + + # Thumb (slightly curved) + positions[HandJointIndex.THUMB_METACARPAL] = [0.02, 0.02, 0] + positions[HandJointIndex.THUMB_PROXIMAL] = [0.04, 0.04, 0] + positions[HandJointIndex.THUMB_DISTAL] = [0.05, 0.05, -0.01] + positions[HandJointIndex.THUMB_TIP] = [0.055, 0.055, -0.025] + + # Index (curled) + positions[HandJointIndex.INDEX_PROXIMAL] = [0, 0.03, 0.04] + positions[HandJointIndex.INDEX_INTERMEDIATE] = [0, 0.03, 0.07] + positions[HandJointIndex.INDEX_DISTAL] = [-0.02, 0.03, 0.08] + positions[HandJointIndex.INDEX_TIP] = [-0.04, 0.03, 0.07] + + # Middle (curled) + positions[HandJointIndex.MIDDLE_PROXIMAL] = [0, 0.01, 0.04] + positions[HandJointIndex.MIDDLE_INTERMEDIATE] = [0, 0.01, 0.07] + positions[HandJointIndex.MIDDLE_DISTAL] = [-0.02, 0.01, 0.08] + positions[HandJointIndex.MIDDLE_TIP] = [-0.04, 0.01, 0.07] + + # Ring (curled) + positions[HandJointIndex.RING_PROXIMAL] = [0, -0.01, 0.04] + positions[HandJointIndex.RING_INTERMEDIATE] = [0, -0.01, 0.07] + positions[HandJointIndex.RING_DISTAL] = [-0.02, -0.01, 0.08] + positions[HandJointIndex.RING_TIP] = [-0.04, -0.01, 0.07] + + # Pinky (curled) + positions[HandJointIndex.LITTLE_PROXIMAL] = [0, -0.03, 0.04] + positions[HandJointIndex.LITTLE_INTERMEDIATE] = [0, -0.03, 0.07] + positions[HandJointIndex.LITTLE_DISTAL] = [-0.02, -0.03, 0.08] + positions[HandJointIndex.LITTLE_TIP] = [-0.04, -0.03, 0.07] + + tg[HandInputIndex.JOINT_POSITIONS] = positions + tg[HandInputIndex.JOINT_ORIENTATIONS] = orientations + tg[HandInputIndex.JOINT_RADII] = np.ones(NUM_HAND_JOINTS, dtype=np.float32) * 0.01 + tg[HandInputIndex.JOINT_VALID] = valid + return tg + + +# --------------------------------------------------------------------------- +# Expected joint names from the MJCF (22 finger DOFs) +# --------------------------------------------------------------------------- + +EXPECTED_FINGER_JOINTS = [ + "right_thumb_CMC_FE", + "right_thumb_CMC_AA", + "right_thumb_MCP_FE", + "right_thumb_MCP_AA", + "right_thumb_IP", + "right_index_MCP_FE", + "right_index_MCP_AA", + "right_index_PIP", + "right_index_DIP", + "right_middle_MCP_FE", + "right_middle_MCP_AA", + "right_middle_PIP", + "right_middle_DIP", + "right_ring_MCP_FE", + "right_ring_MCP_AA", + "right_ring_PIP", + "right_ring_DIP", + "right_pinky_CMC", + "right_pinky_MCP_FE", + "right_pinky_MCP_AA", + "right_pinky_PIP", + "right_pinky_DIP", +] + + +# =========================================================================== +# Tests +# =========================================================================== + + +@_requires_pinocchio +class TestSharpaHandRetargeter: + @pytest.fixture() + def retargeter(self): + cfg = SharpaHandRetargeterConfig( + robot_asset_path=SHARPA_MJCF, + hand_side="right", + max_iter=50, + frequency=200.0, + ) + return SharpaHandRetargeter(cfg, name="sharpa_test") + + def test_init_loads_model(self, retargeter): + """Verify model loading discovers the expected finger joints.""" + spec = retargeter.output_spec() + joint_names = [t.name for t in spec["hand_joints"].types] + assert joint_names == EXPECTED_FINGER_JOINTS + + def test_input_spec_has_hand_key(self, retargeter): + spec = retargeter.input_spec() + assert "hand_right" in spec + + def test_absent_hand_outputs_zeros(self, retargeter): + """Absent (optional) hand input should produce all-zero output.""" + inputs, outputs = _build_io(retargeter) + retargeter.compute(inputs, outputs, _make_context()) + + for i in range(len(EXPECTED_FINGER_JOINTS)): + assert outputs["hand_joints"][i] == pytest.approx(0.0) + + def test_open_hand_produces_output(self, retargeter): + """An open hand pose should produce finite joint angles.""" + inputs, outputs = _build_io(retargeter) + inputs["hand_right"] = _make_hand_input_open() + + retargeter.compute(inputs, outputs, _make_context()) + + values = [ + float(outputs["hand_joints"][i]) for i in range(len(EXPECTED_FINGER_JOINTS)) + ] + assert all(np.isfinite(values)), f"Non-finite values: {values}" + + def test_curled_hand_produces_nonzero_joints(self, retargeter): + """A curled hand should produce noticeably non-zero joint angles.""" + inputs, outputs = _build_io(retargeter) + inputs["hand_right"] = _make_hand_input_curled() + + retargeter.compute(inputs, outputs, _make_context()) + + values = [ + float(outputs["hand_joints"][i]) for i in range(len(EXPECTED_FINGER_JOINTS)) + ] + assert any(abs(v) > 0.01 for v in values), ( + f"Expected at least some non-zero joints for curled hand, got: {values}" + ) + + def test_warm_starting_persistence(self, retargeter): + """Second frame should reuse warm-start from the first frame.""" + inputs, outputs = _build_io(retargeter) + inputs["hand_right"] = _make_hand_input_curled() + + retargeter.compute(inputs, outputs, _make_context()) + assert retargeter._qpos_prev is not None, ( + "qpos_prev should be set after first frame" + ) + + qpos_after_first = retargeter._qpos_prev.copy() + + # Slightly perturbed pose for second frame + inputs2, outputs2 = _build_io(retargeter) + hand2 = _make_hand_input_curled() + positions = np.from_dlpack(hand2[HandInputIndex.JOINT_POSITIONS]).copy() + positions += 0.001 # small perturbation + hand2[HandInputIndex.JOINT_POSITIONS] = positions + inputs2["hand_right"] = hand2 + + retargeter.compute(inputs2, outputs2, _make_context()) + + qpos_after_second = retargeter._qpos_prev.copy() + + # The second qpos should be close to the first (warm-started) + # but not identical (different input). Skip the 7-DOF FreeFlyer + # prefix (3 translation + 4 quat) that the wrist pose is anchored to. + FREEFLYER_NQ = 7 + diff = np.linalg.norm( + qpos_after_second[FREEFLYER_NQ:] - qpos_after_first[FREEFLYER_NQ:] + ) + assert diff < 1.0, ( + f"Warm-started second frame should be close to first, diff={diff}" + ) + + def test_absent_then_valid_resets_warm_start(self, retargeter): + """Feeding absent input should reset warm-start state.""" + inputs, outputs = _build_io(retargeter) + inputs["hand_right"] = _make_hand_input_curled() + retargeter.compute(inputs, outputs, _make_context()) + assert retargeter._qpos_prev is not None + + # Feed absent input + inputs_absent, outputs_absent = _build_io(retargeter) + retargeter.compute(inputs_absent, outputs_absent, _make_context()) + assert retargeter._qpos_prev is None, ( + "qpos_prev should be reset after absent input" + ) + + +if __name__ == "__main__": + pytest.main([__file__, "-v"]) diff --git a/src/retargeters/__init__.py b/src/retargeters/__init__.py index 7c279d5eb..3552f1262 100644 --- a/src/retargeters/__init__.py +++ b/src/retargeters/__init__.py @@ -16,6 +16,8 @@ - LocomotionRootCmdRetargeter: Locomotion from controller inputs - FootPedalRootCmdRetargeter: Root command from 3-axis foot pedal (horizontal/vertical + rudder) - GripperRetargeter: Pinch-based gripper control + - SharpaHandRetargeter: Pinocchio/Pink IK-based retargeting for Sharpa hand + - SharpaBiManualRetargeter: Bimanual version of SharpaHandRetargeter - Se3AbsRetargeter: Absolute EE pose control - Se3RelRetargeter: Relative EE delta control - TensorReorderer: Reorders and flattens multiple inputs into a single tensor @@ -100,6 +102,23 @@ "Se3RetargeterConfig", "retargeters-lite", ), + # .sharpa_hand_retargeter (requires grounding extra: robotic_grounding, + # which transitively pulls pinocchio, pink, etc.) + "SharpaHandRetargeter": ( + ".sharpa_hand_retargeter", + "SharpaHandRetargeter", + "grounding", + ), + "SharpaBiManualRetargeter": ( + ".sharpa_hand_retargeter", + "SharpaBiManualRetargeter", + "grounding", + ), + "SharpaHandRetargeterConfig": ( + ".sharpa_hand_retargeter", + "SharpaHandRetargeterConfig", + "grounding", + ), # .tensor_reorderer "TensorReorderer": (".tensor_reorderer", "TensorReorderer", None), } @@ -147,6 +166,10 @@ def __getattr__(name: str): "Se3AbsRetargeter", "Se3RelRetargeter", "Se3RetargeterConfig", + # Sharpa hand retargeters (require grounding extra: robotic_grounding) + "SharpaHandRetargeter", + "SharpaBiManualRetargeter", + "SharpaHandRetargeterConfig", # Utility retargeters "TensorReorderer", ] diff --git a/src/retargeters/sharpa_hand_retargeter.py b/src/retargeters/sharpa_hand_retargeter.py new file mode 100644 index 000000000..b7f64e3ad --- /dev/null +++ b/src/retargeters/sharpa_hand_retargeter.py @@ -0,0 +1,332 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +""" +Sharpa Hand Retargeter Module. + +Thin wrapper around `robotic_grounding.retarget.hand_kinematics.SharpaHandKinematics` +that adapts Teleop's OpenXR hand-tracking input format (26-joint HandInput, +xyzw quats) to the MANO 21-joint / wxyz layout the kinematics class expects, +runs IK, and writes the resulting finger DOFs into Teleop's TensorGroup +output. + +The IK loop, MANO joint ordering, Sharpa frame mappings, rotation +corrections, and Pinocchio/Pink configuration all live in +`robotic_grounding`; this module deliberately contains no IK math. + +Requires `isaacteleop[grounding]` and a separately-installed +`robotic_grounding` wheel. See src/retargeters/README.md. +""" + +import logging +from dataclasses import dataclass + +import numpy as np +from robotic_grounding.retarget.hand_kinematics import SharpaHandKinematics + +from isaacteleop.retargeting_engine.interface import ( + BaseRetargeter, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + TensorGroupType, + OptionalType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + HandInput, + FloatType, + HandInputIndex, + HandJointIndex, +) + +logger = logging.getLogger(__name__) + +# OpenXR (26 joints) -> MANO (21 joints) index mapping. +# Skips OpenXR palm(0) and metacarpal joints for non-thumb fingers. +# Order matches MANO_JOINTS_ORDER in robotic_grounding.retarget.params. +_OPENXR_TO_MANO_INDICES: list[int] = [ + HandJointIndex.WRIST, # wrist + HandJointIndex.THUMB_METACARPAL, # thumb1 + HandJointIndex.THUMB_PROXIMAL, # thumb2 + HandJointIndex.THUMB_DISTAL, # thumb3 + HandJointIndex.THUMB_TIP, # thumb4 + HandJointIndex.INDEX_PROXIMAL, # index1 + HandJointIndex.INDEX_INTERMEDIATE, # index2 + HandJointIndex.INDEX_DISTAL, # index3 + HandJointIndex.INDEX_TIP, # index4 + HandJointIndex.MIDDLE_PROXIMAL, # middle1 + HandJointIndex.MIDDLE_INTERMEDIATE, # middle2 + HandJointIndex.MIDDLE_DISTAL, # middle3 + HandJointIndex.MIDDLE_TIP, # middle4 + HandJointIndex.RING_PROXIMAL, # ring1 + HandJointIndex.RING_INTERMEDIATE, # ring2 + HandJointIndex.RING_DISTAL, # ring3 + HandJointIndex.RING_TIP, # ring4 + HandJointIndex.LITTLE_PROXIMAL, # pinky1 + HandJointIndex.LITTLE_INTERMEDIATE, # pinky2 + HandJointIndex.LITTLE_DISTAL, # pinky3 + HandJointIndex.LITTLE_TIP, # pinky4 +] + +# Number of FreeFlyer DOFs Pinocchio prepends to qpos when SharpaHandKinematics +# loads the MJCF with a JointModelFreeFlyer root joint. +_FREEFLYER_NQ = 7 + + +@dataclass +class SharpaHandRetargeterConfig: + """Configuration for the Sharpa hand retargeter. + + Attributes: + robot_asset_path: Path to the Sharpa MJCF (e.g. right_sharpawave.xml, + or right_sharpawave_nomesh.xml for tests). Resolve via + `importlib.resources.files("robotic_grounding") / "assets" / "xmls" / "sharpawave"`. + hand_side: "left" or "right". + hand_joint_names: Output joint ordering override. If None, uses the + finger joint names discovered from the MJCF model. + source_to_robot_scale: Scale factor from MANO to robot coordinates. + solver: QP solver backend for Pink IK. + max_iter: Maximum IK iterations per frame. + frequency: IK rate-limiter frequency [Hz], used to compute dt. + frame_tasks_converged_threshold: Per-task position-error convergence + threshold [m] for early termination. + parameter_config_path: Optional path to a JSON file for saving/loading + tunable parameters. + """ + + robot_asset_path: str + hand_side: str = "right" + hand_joint_names: list[str] | None = None + source_to_robot_scale: float = 1.0 + solver: str = "daqp" + max_iter: int = 200 + frequency: float = 200.0 + frame_tasks_converged_threshold: float = 1e-6 + parameter_config_path: str | None = None + + +class SharpaHandRetargeter(BaseRetargeter): + """Retargets OpenXR hand tracking to Sharpa hand joint angles via Pink IK. + + Inputs: + - "hand_{side}": OpenXR hand tracking data (26 joints), optional. + + Outputs: + - "hand_joints": Sharpa finger joint angles. + """ + + def __init__(self, config: SharpaHandRetargeterConfig, name: str) -> None: + self._config = config + self._hand_side = config.hand_side.lower() + if self._hand_side not in ("left", "right"): + raise ValueError( + f"hand_side must be 'left' or 'right', got: {self._hand_side}" + ) + + self._kinematics = SharpaHandKinematics( + side=self._hand_side, + robot_asset_path=config.robot_asset_path, + source_model="mano", + use_relative_frames=False, + solver=config.solver, + max_iter=config.max_iter, + frequency=config.frequency, + frame_tasks_converged_threshold=config.frame_tasks_converged_threshold, + ) + + # Finger joint names = everything Pinocchio reports past the FreeFlyer. + self._finger_joint_names: list[str] = list( + self._kinematics.robot_finger_joint_names.values() + ) + + if config.hand_joint_names is None: + self._hand_joint_names = list(self._finger_joint_names) + else: + override = list(config.hand_joint_names) + if len(override) != len(set(override)): + seen: set[str] = set() + dupes = [n for n in override if n in seen or seen.add(n)] # type: ignore[func-returns-value] + raise ValueError(f"hand_joint_names contains duplicates: {dupes}") + finger_set = set(self._finger_joint_names) + unknown = [n for n in override if n not in finger_set] + if unknown: + raise ValueError( + f"hand_joint_names contains names not found in the MJCF " + f"model's finger joints: {unknown}. " + f"Valid names: {self._finger_joint_names}" + ) + self._hand_joint_names = override + + self._hand_joint_name_to_idx = { + name: idx for idx, name in enumerate(self._hand_joint_names) + } + self._source_to_robot_scale = config.source_to_robot_scale + + # Warm-start qpos persists across frames. + self._qpos_prev: np.ndarray | None = None + + super().__init__(name=name) + + def input_spec(self) -> RetargeterIOType: + """Define input: optional hand tracking for the configured side.""" + key = f"hand_{self._hand_side}" + return {key: OptionalType(HandInput())} + + def output_spec(self) -> RetargeterIOType: + """Define output: Sharpa finger joint angles.""" + return { + "hand_joints": TensorGroupType( + f"hand_joints_{self._hand_side}", + [FloatType(name) for name in self._hand_joint_names], + ) + } + + def _emit_zeros(self, output_group) -> None: + for i in range(len(self._hand_joint_names)): + output_group[i] = 0.0 + self._qpos_prev = None + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + output_group = outputs["hand_joints"] + hand_group = inputs[f"hand_{self._hand_side}"] + + if hand_group.is_none: + self._emit_zeros(output_group) + return + + joint_positions = np.from_dlpack( + hand_group[HandInputIndex.JOINT_POSITIONS] + ) # (26, 3) + joint_orientations = np.from_dlpack( + hand_group[HandInputIndex.JOINT_ORIENTATIONS] + ) # (26, 4) xyzw + joint_valid = np.from_dlpack(hand_group[HandInputIndex.JOINT_VALID]) # (26,) + + # SharpaHandKinematics has no per-task validity gate; if any joint + # we depend on is invalid, fall back to zeros + reset warm-start + # rather than feed the IK bogus targets. + if not all(joint_valid[xr_idx] for xr_idx in _OPENXR_TO_MANO_INDICES): + self._emit_zeros(output_group) + return + + # Repack OpenXR (26 joints, xyzw quats) -> MANO (21 joints, wxyz quats). + mano_positions = joint_positions[_OPENXR_TO_MANO_INDICES].astype( + np.float64, copy=False + ) + xyzw = joint_orientations[_OPENXR_TO_MANO_INDICES] + mano_quats_wxyz = np.empty((21, 4), dtype=np.float64) + mano_quats_wxyz[:, 0] = xyzw[:, 3] + mano_quats_wxyz[:, 1:4] = xyzw[:, 0:3] + + # Initial qpos: warm-start from previous frame (with wrist re-anchored + # to the new tracker reading) if available, else from q0. + wrist_pos = mano_positions[0] + wrist_wxyz = mano_quats_wxyz[0] + wrist_xyzw = np.array( + [wrist_wxyz[1], wrist_wxyz[2], wrist_wxyz[3], wrist_wxyz[0]] + ) + + if self._qpos_prev is None: + qpos = self._kinematics.robot.q0.copy() + else: + qpos = self._qpos_prev.copy() + qpos[:3] = wrist_pos + qpos[3:7] = wrist_xyzw + + result = self._kinematics.compute( + mano_positions, + mano_quats_wxyz, + source_to_robot_scale=self._source_to_robot_scale, + qpos=qpos, + ) + new_qpos = result["q"] + self._qpos_prev = new_qpos.copy() + + # Slice finger DOFs out of the FreeFlyer-prefixed qpos and write + # them through the (potentially reordered) hand_joint_names mapping. + finger_angles = new_qpos[_FREEFLYER_NQ:] + for i, jname in enumerate(self._finger_joint_names): + out_idx = self._hand_joint_name_to_idx.get(jname) + if out_idx is not None: + output_group[out_idx] = float(finger_angles[i]) + + +class SharpaBiManualRetargeter(BaseRetargeter): + """Combines left and right Sharpa hand joint angles into a single vector. + + Inputs: + - "left_hand_joints": Joint angles from a left SharpaHandRetargeter + - "right_hand_joints": Joint angles from a right SharpaHandRetargeter + + Outputs: + - "hand_joints": Combined joint angles ordered by target_joint_names + """ + + def __init__( + self, + left_joint_names: list[str], + right_joint_names: list[str], + target_joint_names: list[str], + name: str, + ) -> None: + self._target_joint_names = target_joint_names + self._left_joint_names = left_joint_names + self._right_joint_names = right_joint_names + + super().__init__(name=name) + + self._left_indices: list[int] = [] + self._right_indices: list[int] = [] + self._output_indices_left: list[int] = [] + self._output_indices_right: list[int] = [] + + for i, jname in enumerate(target_joint_names): + if jname in left_joint_names: + self._output_indices_left.append(i) + self._left_indices.append(left_joint_names.index(jname)) + elif jname in right_joint_names: + self._output_indices_right.append(i) + self._right_indices.append(right_joint_names.index(jname)) + + mapped = len(self._output_indices_left) + len(self._output_indices_right) + if mapped != len(target_joint_names): + known = set(left_joint_names) | set(right_joint_names) + missing = [n for n in target_joint_names if n not in known] + raise ValueError( + f"target_joint_names contains {len(missing)} name(s) not found " + f"in left or right joint lists: {missing}" + ) + + def input_spec(self) -> RetargeterIOType: + """Define input collections for both hands.""" + return { + "left_hand_joints": TensorGroupType( + "left_hand_joints", + [FloatType(name) for name in self._left_joint_names], + ), + "right_hand_joints": TensorGroupType( + "right_hand_joints", + [FloatType(name) for name in self._right_joint_names], + ), + } + + def output_spec(self) -> RetargeterIOType: + """Define output collections for combined hand joints.""" + return { + "hand_joints": TensorGroupType( + "hand_joints_bimanual", + [FloatType(name) for name in self._target_joint_names], + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + left_input = inputs["left_hand_joints"] + right_input = inputs["right_hand_joints"] + combined = outputs["hand_joints"] + + for src, dst in zip(self._left_indices, self._output_indices_left): + combined[dst] = float(left_input[src]) + + for src, dst in zip(self._right_indices, self._output_indices_right): + combined[dst] = float(right_input[src])