diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 81da6c89b..4e594f46a 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -1,3 +1,6 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + Retargeting Interface ===================== @@ -41,6 +44,15 @@ Available Retargeters ``hand_side`` (``"left"`` or ``"right"``), ``gripper_close_meters``, ``gripper_open_meters``, and ``controller_threshold`` for trigger-based closing. +.. dropdown:: SO101ClutchRetargeter / SO101GripperRetargeter + + Retargeters for the SO-101 5-DOF arm under full-pose SE3 IK. ``SO101ClutchRetargeter`` + outputs a 7-D ``ee_pose`` like ``Se3AbsRetargeter`` but clutch-rebases controller position + around an origin captured on engage (no teleport) and composes a fixed orientation + calibration offset onto the grip orientation so the gripper pose follows the controller pose. + ``SO101GripperRetargeter`` maps the trigger to a proportional jaw closedness in ``[0, 1]``. + See :doc:`so101` for the full setup. + .. dropdown:: DexHandRetargeter / DexBiManualRetargeter Accurate hand tracking retargeter using the ``dex-retargeting`` library. It maps full hand @@ -315,3 +327,4 @@ and :doc:`Contributing Guide <../../getting_started/contributing>` for details. :caption: Retargeter setup guides sharpa + so101 diff --git a/docs/source/references/retargeting/so101.rst b/docs/source/references/retargeting/so101.rst new file mode 100644 index 000000000..a57a5cc32 --- /dev/null +++ b/docs/source/references/retargeting/so101.rst @@ -0,0 +1,156 @@ +.. SPDX-FileCopyrightText: Copyright (c) 2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +.. SPDX-License-Identifier: Apache-2.0 + +Retargeters: SO-101 (5-DOF arm) +=============================== + +The SO-101 is a low-cost 5-DOF arm with a single-jaw gripper. The controller is meant to feel +like holding the **leader arm**: the gripper pose follows the controller pose directly. A +full SE3 pose is commanded and a single differential IK solves all 5 arm joints, tracking +position exactly and orientation best-effort (a 5-DOF arm is over-determined by one DOF on a +6-DOF pose). These two retargeters provide the pieces for comfortable XR controller +teleoperation of the arm (used by the Isaac Lab ``Isaac-Stack-Cube-SO101-IK-Abs-v0`` +cube-stacking task): + +* :class:`~isaacteleop.retargeters.SO101ClutchRetargeter` -- absolute EE **pose** (position + + orientation) with clutch-style position rebasing (no teleport on engage). +* :class:`~isaacteleop.retargeters.SO101GripperRetargeter` -- proportional (analog) jaw + closedness from the controller trigger. + +Together they flatten (via :class:`~isaacteleop.retargeters.TensorReorderer`) into an 8-D action +``[pos_x, pos_y, pos_z, quat_x, quat_y, quat_z, quat_w, gripper]``. + +At a glance +----------- + +.. list-table:: + :header-rows: 1 + :widths: 28 22 50 + + * - Retargeter + - Output + - What it does + * - ``SO101ClutchRetargeter`` + - 7-D ``ee_pose`` (xyz + xyzw quat) + - Same output contract as ``Se3AbsRetargeter``, but rebases controller motion around an + origin captured on engage: ``pos = home + scale * (p_ctrl - p0)``. The orientation is + the controller grip orientation composed with a fixed calibration offset; it drives the + SE3 IK. + * - ``SO101GripperRetargeter`` + - 1 float ``gripper_command`` ``c`` in ``[0, 1]`` + - Trigger -> jaw closedness (``0`` = open, ``1`` = closed), with a released-end deadzone. + +Why a clutch +------------ + +``Se3AbsRetargeter`` maps the controller's absolute position straight to the EE target, so +engaging teleop teleports the arm to wherever the controller happens to be. The clutch instead +**re-arms** whenever teleop is not ``RUNNING`` and latches a controller origin ``p0`` on the +first ``RUNNING`` frame (the headset "Play"). From then on the EE position is driven by the +*delta* relative to ``p0``, so engaging with a steady controller does not move the arm. On the +latching frame ``p_ctrl == p0``, so the emitted position is exactly the home (no jump). The last +pose is held on a dropped frame. + +The clutch keeps position-control IK (``use_relative_mode=False``): it emits an **absolute** +target, just rebased. + +Frames and the home +^^^^^^^^^^^^^^^^^^^^ + +The controller stream reaching the clutch is already expressed in the robot **base** frame: the +Isaac Lab ``IsaacTeleopDevice`` rebases it upstream via its ``target_frame_prim_path`` (set to +the robot base), composing ``base_T_world`` onto the XR anchor before the controllers are +transformed. The clutch therefore applies the controller delta to the home directly, with no +world->base rotation of its own, and needs **no** live end-effector or base feed. + +The ``home`` is the clutch's own running home: it is seeded on reset / first engage from the +static ``home_base_T_ee`` reset-origin (the gripper's pose in the base frame at the reset pose, +only its translation is used), and thereafter holds the last commanded pose so a mid-task +re-clutch resumes from where the arm was left. + +.. note:: + + The fallback home, the position sign/scale knobs, and the orientation calibration offset + carry ``TODO(tune-in-sim)`` markers: the rebasing math is exact and unit-tested, but the + end-to-end controller->EE handedness and the neutral-controller -> neutral-gripper offset + should be confirmed in simulation. + +Orientation calibration +----------------------- + +The clutch emits the controller grip orientation composed with a fixed calibration offset: +``q_cmd = orientation_offset (x) q_grip`` (base-frame left multiply, renormalized). The offset +defaults to identity (passthrough); a non-identity offset maps a neutrally-held controller to a +sensible neutral gripper orientation for the SE3 IK. This single rotational offset replaces the +per-DOF roll/pitch calibration hacks of earlier revisions. + +Gripper +------- + +``SO101GripperRetargeter`` maps the analog trigger to a jaw **closedness** ``c`` in ``[0, 1]`` +(``0`` = open, ``1`` = closed) with a small released-end deadzone, so a half-pressed trigger +leaves the jaw half-closed. Downstream, an order-locked ``JointPositionActionCfg`` applies the +affine ``joint = offset + scale * c`` mapping ``c`` onto the open/close joint angles. This is +deliberately independent of the shared :class:`~isaacteleop.retargeters.GripperRetargeter`'s +binary ``+1 = open`` / ``-1 = closed`` sign. + +Use it from Python +------------------ + +.. code-block:: python + + from isaacteleop.retargeters import ( + SO101ClutchRetargeter, + SO101GripperRetargeter, + TensorReorderer, + ) + from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaacteleop.retargeting_engine.interface import OutputCombiner, ValueInput + from isaacteleop.retargeting_engine.tensor_types import TransformMatrix + + def build_so101_stack_pipeline(): + controllers = ControllersSource(name="controllers") + world_T_anchor = ValueInput("world_T_anchor", TransformMatrix()) + # The device rebases controller poses into the robot base frame upstream via + # target_frame_prim_path, so the clutch needs no live EE / base feed. + xformed = controllers.transformed(world_T_anchor.output(ValueInput.VALUE)) + + clutch = SO101ClutchRetargeter(name="ee_pose", input_device=ControllersSource.RIGHT) + connected_clutch = clutch.connect({ + ControllersSource.RIGHT: xformed.output(ControllersSource.RIGHT), + }) + + gripper = SO101GripperRetargeter(name="gripper", input_device=ControllersSource.RIGHT) + connected_gripper = gripper.connect( + {ControllersSource.RIGHT: xformed.output(ControllersSource.RIGHT)} + ) + + # Keep all 7 pose names and pass the full pose (xyz + quat) plus gripper through. + ee_elements = ["pos_x", "pos_y", "pos_z", "quat_x", "quat_y", "quat_z", "quat_w"] + reorderer = TensorReorderer( + input_config={ + "ee_pose": ee_elements, + "gripper_command": ["gripper_value"], + }, + output_order=ee_elements + ["gripper_value"], + name="action_reorderer", + input_types={"ee_pose": "array", "gripper_command": "scalar"}, + ) + connected = reorderer.connect({ + "ee_pose": connected_clutch.output("ee_pose"), + "gripper_command": connected_gripper.output("gripper_command"), + }) + return OutputCombiner({"action": connected.output("output")}) + +See :ref:`isaac-teleop-pipeline-builder` for the general pipeline-builder pattern and +:doc:`index` for the full retargeting interface. + +Validate +-------- + +The retargeters ship with sim-free unit tests (trigger/clutch math plus per-frame +``compute`` behavior): + +.. code-block:: console + + $ ctest --test-dir build -R retargeting_test_so101_retargeters --output-on-failure diff --git a/src/core/python/pyproject.toml.in b/src/core/python/pyproject.toml.in index f87db8f17..f02184efc 100644 --- a/src/core/python/pyproject.toml.in +++ b/src/core/python/pyproject.toml.in @@ -50,6 +50,7 @@ packages = [ "isaacteleop.retargeting_engine.utilities", "isaacteleop.retargeters", "isaacteleop.retargeters.G1", + "isaacteleop.retargeters.SO101", "isaacteleop.retargeting_engine_ui", "isaacteleop.teleop_session_manager", "isaacteleop.cloudxr", diff --git a/src/core/retargeting_engine_tests/python/test_so101_retargeters.py b/src/core/retargeting_engine_tests/python/test_so101_retargeters.py new file mode 100644 index 000000000..7b69946b4 --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_so101_retargeters.py @@ -0,0 +1,458 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""Sim-free unit tests for the SO-101 XR teleop retargeters. + +Covers the SO-101 retargeters that drive the full-pose SE3 IK stacking pipeline: + +* :class:`~isaacteleop.retargeters.SO101GripperRetargeter` -- analog trigger -> jaw closedness. +* :class:`~isaacteleop.retargeters.SO101ClutchRetargeter` -- clutch-rebased absolute EE pose + (position + calibration-composed grip orientation) for the 5-joint pose IK. + +Each retargeter is exercised both at the pure-math level (the module-private helper functions) +and at the ``BaseRetargeter.compute`` level (build inputs/outputs, drive a frame, read the +emitted tensor), with no ``gym.make``, USD, GPU, or XR device. +""" + +import math + +import numpy as np +import pytest + +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +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 ( + ControllerInput, + ControllerInputIndex, +) +from isaacteleop.retargeters import ( + SO101ClutchRetargeter, + SO101GripperRetargeter, +) +from isaacteleop.retargeters.SO101.clutch_retargeter import ( + _CLUTCH_HOME_EE_POS, + _quat_mul, + _rebased_position, +) +from isaacteleop.retargeters.SO101.gripper_retargeter import ( + GRIPPER_COMMAND_KEY, + _TRIGGER_DEADZONE, + _trigger_to_closedness, +) + +# --------------------------------------------------------------------------- +# Helpers (mirror the patterns in test_sharpa_hand_retargeter.py) +# --------------------------------------------------------------------------- + +_ID_QUAT = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) + + +def _make_context( + *, reset: bool = False, state: ExecutionState = ExecutionState.RUNNING +) -> ComputeContext: + """Build a ComputeContext with the given reset flag and execution state.""" + return ComputeContext( + graph_time=GraphTime(sim_time_ns=0, real_time_ns=0), + execution_events=ExecutionEvents(reset=reset, execution_state=state), + ) + + +def _build_io(retargeter): + """Construct empty input/output containers for a retargeter (optionals start absent).""" + inputs = {} + for k, v in retargeter.input_spec().items(): + inputs[k] = ( + OptionalTensorGroup(v) + if isinstance(v, OptionalTensorGroupType) + else TensorGroup(v) + ) + outputs = {} + for k, v in retargeter.output_spec().items(): + outputs[k] = ( + OptionalTensorGroup(v) + if isinstance(v, OptionalTensorGroupType) + else TensorGroup(v) + ) + return inputs, outputs + + +def _make_controller( + *, + grip_pos=(0.0, 0.0, 0.0), + grip_ori=_ID_QUAT, + aim_ori=_ID_QUAT, + trigger: float = 0.0, +) -> TensorGroup: + """Build a present (valid) ControllerInput TensorGroup with the given grip/aim pose / trigger. + + The grip pose drives roll; the aim pose (pointer ray) drives pitch. + """ + tg = TensorGroup(ControllerInput()) + tg[ControllerInputIndex.GRIP_POSITION] = np.asarray(grip_pos, dtype=np.float32) + tg[ControllerInputIndex.GRIP_ORIENTATION] = np.asarray(grip_ori, dtype=np.float32) + tg[ControllerInputIndex.GRIP_IS_VALID] = True + tg[ControllerInputIndex.AIM_ORIENTATION] = np.asarray(aim_ori, dtype=np.float32) + tg[ControllerInputIndex.AIM_IS_VALID] = True + tg[ControllerInputIndex.TRIGGER_VALUE] = float(trigger) + return tg + + +def _make_home_transform(translation) -> np.ndarray: + """Build a (4, 4) ``base_T_ee`` home transform with identity rotation and the given origin.""" + m = np.eye(4, dtype=np.float64) + m[:3, 3] = np.asarray(translation, dtype=np.float64) + return m + + +def _quat_xyzw(axis, angle_rad: float) -> np.ndarray: + """Build an [x, y, z, w] quaternion for a rotation of ``angle_rad`` about a unit ``axis``.""" + axis = np.asarray(axis, dtype=np.float64) + axis = axis / np.linalg.norm(axis) + half = 0.5 * angle_rad + xyz = axis * math.sin(half) + return np.array([xyz[0], xyz[1], xyz[2], math.cos(half)], dtype=np.float64) + + +def _read_pose(outputs) -> np.ndarray: + """Read the 7D ee_pose output as a numpy array.""" + return np.asarray(np.from_dlpack(outputs["ee_pose"][0]), dtype=np.float64) + + +# =========================================================================== +# SO101GripperRetargeter +# =========================================================================== + + +class TestSO101GripperTriggerMath: + """The pure ``_trigger_to_closedness`` mapping (deadzone + rescale + clamp).""" + + def test_released_is_open(self): + """A fully released trigger maps to closedness 0 (jaw open).""" + assert _trigger_to_closedness(0.0) == pytest.approx(0.0) + + def test_full_press_is_closed(self): + """A fully pressed trigger maps to closedness 1 (jaw closed).""" + assert _trigger_to_closedness(1.0) == pytest.approx(1.0) + + def test_deadzone_stays_open(self): + """A trigger within the released-end deadzone stays at closedness 0.""" + assert _trigger_to_closedness(_TRIGGER_DEADZONE) == pytest.approx(0.0) + assert _trigger_to_closedness(_TRIGGER_DEADZONE - 0.01) == pytest.approx(0.0) + + def test_half_press_is_mid(self): + """A half-pressed trigger maps to roughly half-closed (monotonic, mid-range).""" + c = _trigger_to_closedness(0.5) + assert 0.4 < c < 0.6 + assert _trigger_to_closedness(0.0) < c < _trigger_to_closedness(1.0) + + def test_clamps_out_of_range(self): + """Trigger values outside [0, 1] clamp to the closedness endpoints.""" + assert _trigger_to_closedness(-0.5) == pytest.approx(0.0) + assert _trigger_to_closedness(1.5) == pytest.approx(1.0) + + +class TestSO101GripperRetargeter: + """End-to-end ``compute`` behavior of the analog gripper retargeter.""" + + def test_output_spec_is_single_scalar(self): + """Outputs exactly one scalar under the gripper command key.""" + r = SO101GripperRetargeter(name="gripper") + spec = r.output_spec() + assert list(spec) == [GRIPPER_COMMAND_KEY] + + def test_full_press_closes(self): + """A fully pressed trigger drives the jaw closed (c == 1).""" + r = SO101GripperRetargeter(name="gripper") + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(trigger=1.0) + r.compute(inputs, outputs, _make_context()) + assert float(outputs[GRIPPER_COMMAND_KEY][0]) == pytest.approx(1.0) + + def test_release_opens(self): + """A released trigger drives the jaw open (c == 0).""" + r = SO101GripperRetargeter(name="gripper") + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(trigger=0.0) + r.compute(inputs, outputs, _make_context()) + assert float(outputs[GRIPPER_COMMAND_KEY][0]) == pytest.approx(0.0) + + def test_dropped_frame_holds_last(self): + """An absent controller frame holds the last commanded closedness.""" + r = SO101GripperRetargeter(name="gripper") + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(trigger=1.0) + r.compute(inputs, outputs, _make_context()) + + # Next frame: controller absent -> hold the previous closedness (1.0). + inputs2, outputs2 = _build_io(r) + r.compute(inputs2, outputs2, _make_context()) + assert float(outputs2[GRIPPER_COMMAND_KEY][0]) == pytest.approx(1.0) + + def test_reset_reopens(self): + """A reset returns the jaw to fully open even after a closed frame.""" + r = SO101GripperRetargeter(name="gripper") + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(trigger=1.0) + r.compute(inputs, outputs, _make_context()) + + # Reset with an absent controller -> the held value is forced back to open. + inputs2, outputs2 = _build_io(r) + r.compute(inputs2, outputs2, _make_context(reset=True)) + assert float(outputs2[GRIPPER_COMMAND_KEY][0]) == pytest.approx(0.0) + + +# =========================================================================== +# SO101ClutchRetargeter +# =========================================================================== + + +class TestRebasedPositionMath: + """The pure clutch rebasing math (origin + scale). Controller poses arrive already in the + robot base frame (the Lab device rebases them via ``target_frame_prim_path``), so no + world->base rotation is applied here.""" + + HOME = np.array(_CLUTCH_HOME_EE_POS, dtype=np.float64) + ORIGIN = np.array([0.31, -0.12, 0.44], dtype=np.float64) + + def test_first_frame_is_home(self): + """With ``p_ctrl == origin`` (the latching frame), the rebased position is exactly home.""" + out = _rebased_position(self.ORIGIN.copy(), self.ORIGIN, self.HOME, 1.0) + np.testing.assert_allclose(out, self.HOME, atol=1e-9) + + def test_applies_delta(self): + """A +delta controller move shifts the EE by +delta from home (scale 1).""" + delta = np.array([0.05, -0.02, 0.10], dtype=np.float64) + out = _rebased_position(self.ORIGIN + delta, self.ORIGIN, self.HOME, 1.0) + np.testing.assert_allclose(out, self.HOME + delta, atol=1e-9) + + def test_honors_scale(self): + """The translation scale multiplies the controller delta before adding it to home.""" + delta = np.array([0.05, -0.02, 0.10], dtype=np.float64) + out = _rebased_position(self.ORIGIN + delta, self.ORIGIN, self.HOME, 2.0) + np.testing.assert_allclose(out, self.HOME + 2.0 * delta, atol=1e-9) + + +class TestSO101ClutchRetargeter: + """End-to-end ``compute`` behavior of the clutch EE-pose retargeter.""" + + def test_output_spec_is_7d_pose(self): + """Outputs a single 7D ee_pose (position + quaternion).""" + r = SO101ClutchRetargeter(name="ee_pose") + pose_type = r.output_spec()["ee_pose"].types[0] + assert pose_type.shape == (7,) + + def test_input_spec_is_controller_only(self): + """The clutch consumes only the controller; no live EE or base transform inputs. + + The world->base rebase happens upstream (the device's ``target_frame_prim_path``) and the + reset-origin home is the static ``home_base_T_ee`` config, so the clutch needs neither a + per-frame ``robot_ee_pos`` nor a ``robot_base_pos`` input. + """ + r = SO101ClutchRetargeter(name="ee_pose") + assert list(r.input_spec()) == [ControllersSource.RIGHT] + assert not hasattr(SO101ClutchRetargeter, "ROBOT_EE_POS_INPUT") + assert not hasattr(SO101ClutchRetargeter, "ROBOT_BASE_POS_INPUT") + + def test_not_running_holds_home_without_latching(self): + """While not RUNNING the clutch holds the configured home and does not latch an origin.""" + r = SO101ClutchRetargeter(name="ee_pose") + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=(0.5, 0.5, 0.5)) + r.compute(inputs, outputs, _make_context(state=ExecutionState.STOPPED)) + pose = _read_pose(outputs) + np.testing.assert_allclose(pose[:3], np.array(_CLUTCH_HOME_EE_POS), atol=1e-6) + assert r._origin is None # not latched while stopped + + def test_engage_latches_origin_at_home(self): + """The first RUNNING frame latches the origin so the EE sits exactly at the configured home. + + On the latching frame ``p_ctrl == origin`` so the emitted position equals home (no + teleport on engage). An identity offset is used here so the grip orientation passes + through unchanged (the default ``Rz(pi)`` offset is covered by + :meth:`test_calibration_offset_composed_into_orientation`). + """ + r = SO101ClutchRetargeter( + name="ee_pose", orientation_offset=np.array([0.0, 0.0, 0.0, 1.0]) + ) + inputs, outputs = _build_io(r) + grip_ori = _quat_xyzw([0, 0, 1], 0.3).astype(np.float32) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=(0.5, -0.3, 0.7), grip_ori=grip_ori + ) + r.compute(inputs, outputs, _make_context()) + pose = _read_pose(outputs) + np.testing.assert_allclose(pose[:3], np.array(_CLUTCH_HOME_EE_POS), atol=1e-6) + np.testing.assert_allclose(pose[3:], grip_ori, atol=1e-6) + + def test_motion_after_engage_applies_delta(self): + """A controller delta after engage shifts the EE by that delta from home (base frame).""" + r = SO101ClutchRetargeter(name="ee_pose") + p0 = np.array([0.5, -0.3, 0.7]) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0) + r.compute(inputs, outputs, _make_context()) # engage frame: latch origin == p0 + + delta = np.array([0.05, -0.02, 0.10]) + inputs2, outputs2 = _build_io(r) + inputs2[ControllersSource.RIGHT] = _make_controller(grip_pos=p0 + delta) + r.compute(inputs2, outputs2, _make_context()) + pose = _read_pose(outputs2) + expected = np.array(_CLUTCH_HOME_EE_POS) + delta + np.testing.assert_allclose(pose[:3], expected, atol=1e-5) + + def test_configured_home_overrides_default(self): + """A ``home_base_T_ee`` transform sets the engage home from its translation block.""" + home_xyz = (0.30, 0.10, 0.25) + r = SO101ClutchRetargeter( + name="ee_pose", home_base_T_ee=_make_home_transform(home_xyz) + ) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=(0.5, -0.3, 0.7)) + r.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose( + _read_pose(outputs)[:3], np.array(home_xyz), atol=1e-6 + ) + + def test_reengage_resumes_from_last_commanded_pose(self): + """A mid-task re-clutch resumes from the last commanded pose (no snap), then accumulates. + + The clutch keeps its own running home internally. After a plain Stop, the next Play + latches the origin to the new controller pose but keeps the home at the last commanded + pose, so the arm stays put and tracks fresh delta from there. + """ + r = SO101ClutchRetargeter(name="ee_pose") + home = np.array(_CLUTCH_HOME_EE_POS) + p0 = np.array([0.5, -0.3, 0.7]) + + # Engage (seeds home from config) and move +d1 -> last commanded is home + d1. + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0) + r.compute(inputs, outputs, _make_context()) + d1 = np.array([0.05, -0.02, 0.10]) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0 + d1) + r.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_pose(outputs)[:3], home + d1, atol=1e-5) + + # Disengage (STOPPED) -> hold last commanded pose, re-arm the origin. + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=(9.0, 9.0, 9.0)) + r.compute(inputs, outputs, _make_context(state=ExecutionState.STOPPED)) + np.testing.assert_allclose(_read_pose(outputs)[:3], home + d1, atol=1e-5) + assert r._origin is None + + # Re-engage at a new controller pose -> resume from the running home (home + d1): no jump. + p1 = np.array([1.0, 1.0, 1.0]) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p1) + r.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_pose(outputs)[:3], home + d1, atol=1e-5) + + # Fresh delta from the re-engage origin accumulates on top of the resumed pose. + d2 = np.array([0.0, 0.10, -0.03]) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p1 + d2) + r.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_pose(outputs)[:3], home + d1 + d2, atol=1e-5) + + def test_reset_returns_home_to_config(self): + """An explicit reset returns the running home to the configured home. + + Unlike a plain Stop (which freezes the running home so re-engage resumes), a reset + re-zeros the clutch to the configured home for a fresh episode. + """ + r = SO101ClutchRetargeter(name="ee_pose") + home = np.array(_CLUTCH_HOME_EE_POS) + p0 = np.array([0.5, -0.3, 0.7]) + + # Engage and move so the running home is no longer the configured home. + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0) + r.compute(inputs, outputs, _make_context()) + d1 = np.array([0.05, -0.02, 0.10]) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0 + d1) + r.compute(inputs, outputs, _make_context()) + + # Reset (controller steady at p0) -> re-zeros the home to config and re-latches the + # origin at p0, so the emitted pose is the configured home, not the pre-reset pose. + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0) + r.compute(inputs, outputs, _make_context(reset=True)) + np.testing.assert_allclose(_read_pose(outputs)[:3], home, atol=1e-6) + + # Subsequent motion is measured from the config home (not the pre-reset commanded pose). + d2 = np.array([0.0, 0.10, -0.03]) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0 + d2) + r.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_pose(outputs)[:3], home + d2, atol=1e-6) + + def test_dropped_frame_holds_last_pose(self): + """An absent controller frame holds the last commanded pose.""" + r = SO101ClutchRetargeter(name="ee_pose") + p0 = np.array([0.5, -0.3, 0.7]) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0) + r.compute(inputs, outputs, _make_context()) + first = _read_pose(outputs) + + inputs2, outputs2 = _build_io(r) # controller absent + r.compute(inputs2, outputs2, _make_context()) + np.testing.assert_allclose(_read_pose(outputs2), first, atol=1e-9) + + def test_calibration_offset_composed_into_orientation(self): + """The fixed orientation offset right-multiplies (body frame) the grip orientation. + + The default offset is ``Rz(pi)`` (a roll about the shared approach axis), composed as a + body-frame right multiply (``grip (x) offset``) and renormalized to unit. An explicit + identity offset passes the grip orientation through unchanged. + """ + grip = _quat_xyzw([0.0, 1.0, 0.0], 0.4).astype(np.float32) + + # Explicit identity offset: passthrough. + r = SO101ClutchRetargeter( + name="ee_pose", orientation_offset=np.array([0.0, 0.0, 0.0, 1.0]) + ) + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_pos=(0.5, -0.3, 0.7), grip_ori=grip + ) + r.compute(inputs, outputs, _make_context()) + np.testing.assert_allclose(_read_pose(outputs)[3:], grip, atol=1e-6) + + # Default offset: emitted == grip (x) Rz(pi) (body-frame right multiply), unit. + r_def = SO101ClutchRetargeter(name="ee_pose") + inputs_def, outputs_def = _build_io(r_def) + inputs_def[ControllersSource.RIGHT] = _make_controller( + grip_pos=(0.5, -0.3, 0.7), grip_ori=grip + ) + r_def.compute(inputs_def, outputs_def, _make_context()) + rz_pi = np.array([0.0, 0.0, 1.0, 0.0]) # Rz(pi), xyzw + expected_def = _quat_mul(grip.astype(np.float64), rz_pi) + expected_def = expected_def / np.linalg.norm(expected_def) + np.testing.assert_allclose(_read_pose(outputs_def)[3:], expected_def, atol=1e-6) + + # Non-identity offset: emitted == grip (x) offset (right multiply), renormalized to unit. + offset = _quat_xyzw([0.0, 0.0, 1.0], 0.9) + r2 = SO101ClutchRetargeter(name="ee_pose", orientation_offset=offset) + inputs2, outputs2 = _build_io(r2) + inputs2[ControllersSource.RIGHT] = _make_controller( + grip_pos=(0.5, -0.3, 0.7), grip_ori=grip + ) + r2.compute(inputs2, outputs2, _make_context()) + emitted = _read_pose(outputs2)[3:] + expected = _quat_mul(grip.astype(np.float64), offset) + expected = expected / np.linalg.norm(expected) + np.testing.assert_allclose(emitted, expected, atol=1e-6) + assert np.linalg.norm(emitted) == pytest.approx(1.0, abs=1e-6) diff --git a/src/retargeters/CMakeLists.txt b/src/retargeters/CMakeLists.txt index 381c23c9f..45c8737e9 100644 --- a/src/retargeters/CMakeLists.txt +++ b/src/retargeters/CMakeLists.txt @@ -10,6 +10,8 @@ if(BUILD_PYTHON_BINDINGS) "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/retargeters/__pycache__" COMMAND ${CMAKE_COMMAND} -E rm -rf "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/retargeters/G1/__pycache__" + COMMAND ${CMAKE_COMMAND} -E rm -rf + "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/retargeters/SO101/__pycache__" COMMAND ${CMAKE_COMMAND} -E rm -f "${CMAKE_BINARY_DIR}/python_package/$/isaacteleop/retargeters/CMakeLists.txt" COMMAND ${CMAKE_COMMAND} -E rm -f diff --git a/src/retargeters/SO101/__init__.py b/src/retargeters/SO101/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/retargeters/SO101/clutch_retargeter.py b/src/retargeters/SO101/clutch_retargeter.py new file mode 100644 index 000000000..240ab5fc9 --- /dev/null +++ b/src/retargeters/SO101/clutch_retargeter.py @@ -0,0 +1,321 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""SO-101 clutch (relative-origin) EE-pose retargeter for the absolute-pose XR teleop pipeline. + +The shared :class:`~isaacteleop.retargeters.Se3AbsRetargeter` maps the controller's absolute +position directly to the EE target, so engaging teleop teleports the arm to wherever the +controller happens to be in the (anchor-transformed) world frame. For comfortable bring-up we +want clutch-style rebasing: capture the controller position at engage and drive the EE from +the *delta* relative to that captured origin, while keeping position-control IK +(``use_relative_mode=False``). + +:class:`SO101ClutchRetargeter` therefore emits an **absolute** 7D ``ee_pose`` (position +``[x, y, z]`` [m] + orientation quaternion ``[x, y, z, w]``) with the exact same output +contract as :class:`~isaacteleop.retargeters.Se3AbsRetargeter` (node ``name="ee_pose"``, output +key ``"ee_pose"``, ``NDArrayType("pose", shape=(7,))``), so the downstream reorderer and the +8D action contract are untouched. The orientation is the controller grip orientation composed +with a fixed calibration offset; it **drives the SE3 IK** that solves all 5 SO-101 arm joints. + +Frame contract: the controller stream reaching this retargeter is already expressed in the +robot **base** frame -- the Isaac Lab device rebases it upstream via its +``target_frame_prim_path`` (set to the robot base), composing ``base_T_world`` onto the XR +anchor before the controllers are transformed. The clutch therefore applies the controller +delta to the home directly, with no world->base rotation of its own. The emitted position is +in the same base frame as the downstream absolute-position IK command. + +Clutch behavior: + +- The origin is **re-armed** (``self._origin = None``) whenever teleop is not ``RUNNING`` + (i.e. on Stop, and on an explicit reset); the first ``RUNNING`` frame thereafter (the moment + the headset "Play" engages) latches the controller origin ``p0`` in the (base-frame) + controller stream. Connecting the client does **not** latch the origin, and every Play + re-zeroes it to the controller's current pose. +- The clutch keeps its own **running home** internally. Because it commands the EE, it knows + the last target it emitted; on a mid-task re-clutch (disengage to reposition the hand, + re-engage to continue) it latches the home to that last commanded pose, so the EE stays put + and tracks fresh delta from where it was left -- the live end-effector is *not* re-read on a + re-clutch. +- The home is **seeded from the configured reset-origin** on reset / first engage: the + :paramref:`home_base_T_ee` ``base_T_ee`` transform -- the gripper's pose relative to the robot + base at the reset pose. No live end-effector is read; the owning task resets the arm to this + pose, so the engage after a reset latches the home there and the arm sits exactly where it + already is (no snap on Play). The clutch never needs to read the robot back: the base rebase is + done upstream by ``target_frame_prim_path``, and the EE state going forward is the running home + (what the clutch itself commanded). +- Each frame the emitted position is ``output_pos = home + s * (p_ctrl - p0)`` with scale + ``s = _CLUTCH_POSITION_SCALE`` (see :func:`_rebased_position`). On the latching frame + ``p_ctrl == p0`` so ``output_pos == home`` exactly (no teleport on engage). +- A plain Stop **freezes** the running home, so the next Play resumes from the last commanded + pose. An explicit **reset** re-seeds the home from the configured reset-origin for a fresh + episode. +- The reset-origin home is supplied as a ``base_T_ee`` 4x4 transform at construction; only its + translation drives the position command (the orientation is the live controller grip + orientation composed with the fixed calibration offset, not read from this home transform). It + is **never** ``(0, 0, 0)`` (that would command the EE into the robot base / floor). +- The last pose is held on a dropped frame. +""" + +import numpy as np +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +from isaacteleop.retargeting_engine.interface import ( + BaseRetargeter, + RetargeterIOType, +) +from isaacteleop.retargeting_engine.interface.execution_events import ExecutionState +from isaacteleop.retargeting_engine.interface.retargeter_core_types import RetargeterIO +from isaacteleop.retargeting_engine.interface.tensor_group_type import ( + OptionalType, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerInput, + ControllerInputIndex, + DLDataType, + NDArrayType, +) + +# Default home EE position [m] in the IK command (base) frame, used to build the fallback +# ``base_T_ee`` home when no ``home_base_T_ee`` transform is supplied (e.g. sim-free tests). A +# generic, non-degenerate bring-up value (approximately where a small tabletop arm's gripper +# sits at a neutral pose); it is intentionally arm-scale geometry, not a task-specific +# placement. Must never be the base origin (that would command the EE into the robot base / +# floor). TODO(verify-in-sim) +_CLUTCH_HOME_EE_POS: tuple[float, float, float] = (0.22, 0.0, 0.12) +# Controller-to-EE translation gain (1.0 = 1:1 motion). TODO(verify-in-sim) +_CLUTCH_POSITION_SCALE = 1.0 +# Identity orientation quaternion [x, y, z, w], emitted before any valid frame / on reset. +_IDENTITY_QUAT = np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32) +# Default fixed orientation calibration offset [x, y, z, w], right-multiplied (body frame) onto +# the controller grip orientation so a neutrally-held controller maps to a sensible neutral +# gripper orientation. The OpenXR grip frame and the SO-101 gripper_link frame share their tool +# axis (both point/approach along their own -Z), so the only fixed convention offset between them +# is a roll about that shared approach (Z) axis; the default is Rz(pi) == [0, 0, 1, 0] (xyzw), the +# body-frame equivalent of the ~pi wrist-roll calibration the deleted SO101WristRetargeter applied. +# TODO(tune-in-sim): confirm the angle/sign (pi vs pi +/- pi/2) -- it sets which way the single +# moving jaw faces relative to the hand. +_DEFAULT_ORIENTATION_OFFSET = np.array( + [0.0, 0.0, 1.0, 0.0], dtype=np.float64 +) # Rz(pi), xyzw + + +def _quat_mul(a: np.ndarray, b: np.ndarray) -> np.ndarray: + """Hamilton product ``a (x) b`` of two ``[x, y, z, w]`` quaternions (scalar-last). + + Follows the SciPy ``Rotation`` composition convention (``(Ra * Rb).as_quat()`` equals + ``_quat_mul(Ra.as_quat(), Rb.as_quat())``), but is inlined here to avoid a SciPy / + cross-module dependency for this single product. + """ + ax, ay, az, aw = a + bx, by, bz, bw = b + return np.array( + [ + aw * bx + ax * bw + ay * bz - az * by, + aw * by - ax * bz + ay * bw + az * bx, + aw * bz + ax * by - ay * bx + az * bw, + aw * bw - ax * bx - ay * by - az * bz, + ], + dtype=np.float64, + ) + + +def _rebased_position( + p_ctrl: np.ndarray, + origin: np.ndarray, + home: np.ndarray, + scale: float, +) -> np.ndarray: + """Rebase a controller position onto the EE home via the clutch origin and scale. + + Returns ``home + scale * (p_ctrl - origin)`` [m]. With ``p_ctrl == origin`` (the latching + frame) this is exactly ``home``; subsequent controller motion ``delta = p_ctrl - origin`` + is scaled by ``scale`` and applied to the home. + + The controller stream is already in the robot base frame (the Lab device rebases it + upstream), so ``home`` and the downstream absolute-position IK command share that frame and + the delta is applied without any further rotation. + + Args: + p_ctrl: Current controller position [m], shape ``(3,)``, in the (base-frame) controller + stream. + origin: Latched controller origin ``p0`` [m], shape ``(3,)``, same frame. + home: EE home position [m], shape ``(3,)``, in the IK command (base) frame. + scale: Dimensionless translation gain applied to the controller delta. + + Returns: + The rebased EE target position [m], shape ``(3,)``. + """ + return home + scale * (p_ctrl - origin) + + +class SO101ClutchRetargeter(BaseRetargeter): + """Retargets an XR controller to an absolute SO-101 EE pose with clutch-style rebasing. + + Emits an absolute 7D ``ee_pose`` (position [m] + grip orientation quaternion ``[x,y,z,w]``) + identical in contract to :class:`~isaacteleop.retargeters.Se3AbsRetargeter`, but rebases the + controller motion around a captured origin: the EE position is + ``home + scale * (p_ctrl - p0)`` where ``p0`` is latched on the first valid frame after each + engage. The controller stream is already in the robot base frame (rebased upstream by the + Lab device's ``target_frame_prim_path``), so no world->base rotation is applied here. This + keeps position-control IK (``use_relative_mode=False``) while avoiding an engage-time + teleport (see the module docstring). The orientation drives the SE3 IK: it is the controller + grip orientation right-multiplied (body frame) by the fixed :paramref:`orientation_offset` + calibration rotation, then renormalized -- a rigid "tool mount" between the controller grip + frame and the gripper, so a controller rotation maps 1:1 onto the gripper. + + The clutch keeps its own running home: on a mid-task re-clutch it latches the home to the + last EE pose it commanded, so it resumes from where the arm was left (no jump). On reset / + first engage it seeds the home from the configured :paramref:`home_base_T_ee` reset-origin + (the gripper's pose relative to the base at the reset pose), so the arm sits where it already + is without reading the robot back each frame. + """ + + def __init__( + self, + name: str, + input_device: str = ControllersSource.RIGHT, + home_base_T_ee: np.ndarray | None = None, + orientation_offset: np.ndarray | None = None, + debug_log_engage: bool = False, + ) -> None: + """Initialize the clutch EE-pose retargeter. + + Args: + name: Name identifier for this retargeter node. + input_device: Controller source key to read the grip pose from. + home_base_T_ee: Optional ``base_T_ee`` 4x4 reset-origin home transform [m] giving the + EE pose in the robot base frame at the reset pose. The clutch seeds its home from + this on reset / first engage, so the owning task must reset the arm to this pose to + avoid a jump on engage. Only its translation block drives the position command (the + orientation is the live controller grip orientation composed with + :paramref:`orientation_offset`, not read from this transform). When ``None``, falls + back to :data:`_CLUTCH_HOME_EE_POS`. + orientation_offset: Optional fixed calibration quaternion ``[x, y, z, w]`` + right-multiplied (body frame) onto the controller grip orientation + (``q_cmd = q_grip (x) offset``) so a neutrally-held controller maps to a neutral + gripper orientation for the SE3 IK; a controller rotation then maps 1:1 onto the + gripper. When ``None``, defaults to a roll of ``pi`` about the approach axis + (``Rz(pi)``); see :data:`_DEFAULT_ORIENTATION_OFFSET`. + debug_log_engage: When ``True``, prints the controller grip orientation (xyzw, base + frame) and the active offset on each engage, to capture the calibration offset + ``R_off = quat_inv(q_grip) (x) q_G0``. Leave ``False`` outside tuning sessions. + """ + self._input_device = input_device + super().__init__(name=name) + # Fixed calibration offset [x, y, z, w], right-multiplied (body frame) onto the grip + # orientation before it is emitted. Defaults to Rz(pi) about the approach axis. + if orientation_offset is None: + self._orientation_offset = _DEFAULT_ORIENTATION_OFFSET.copy() + else: + self._orientation_offset = np.asarray( + orientation_offset, dtype=np.float64 + ).copy() + self._debug_log_engage = bool(debug_log_engage) + # Reset-origin home [m] in the base frame: the translation of the ``base_T_ee`` transform, + # or the constant. Seeds the home on reset / first engage. + if home_base_T_ee is None: + self._home_config = np.array(_CLUTCH_HOME_EE_POS, dtype=np.float64) + else: + self._home_config = np.asarray(home_base_T_ee, dtype=np.float64)[ + :3, 3 + ].copy() + # Running home [m] in the base frame. ``None`` means "re-seed from the reset-origin on the + # next engage"; set at construction and on reset. Otherwise it holds the last commanded + # pose so a mid-task re-clutch resumes there. + self._home: np.ndarray | None = None + self._origin: np.ndarray | None = None + # Pose held while not running and on dropped frames: the reset-origin home at identity + # orientation until the first command refreshes it. + self._last_pose = np.concatenate([self._home_config, _IDENTITY_QUAT]).astype( + np.float32 + ) + + def input_spec(self) -> RetargeterIOType: + """Requires only the controller grip pose; the stream is already in the base frame.""" + return { + self._input_device: OptionalType(ControllerInput()), + } + + def output_spec(self) -> RetargeterIOType: + """Outputs an absolute 7D ee pose (position [m] + quaternion [x, y, z, w]).""" + return { + "ee_pose": TensorGroupType( + "ee_pose", + [ + NDArrayType( + "pose", shape=(7,), dtype=DLDataType.FLOAT, dtype_bits=32 + ) + ], + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Computes the clutch-rebased absolute EE pose; holds last on a dropped frame.""" + # Teleop is only "engaged" when the session is RUNNING (the headset "Play"). + running = context.execution_events.execution_state == ExecutionState.RUNNING + + if context.execution_events.reset: + # A reset starts a fresh episode: re-arm the origin and mark the home for re-seeding + # from the configured reset-origin (the arm is reset to that pose) on the next engage. + self._origin = None + self._home = None + self._last_pose = np.concatenate( + [self._home_config, _IDENTITY_QUAT] + ).astype(np.float32) + elif not running: + # A plain Stop re-arms the origin but FREEZES the running home and last pose, so the + # next Play resumes from the last commanded pose instead of snapping to home. + self._origin = None + + ee_pose = outputs["ee_pose"] + inp = inputs[self._input_device] + if inp.is_none: + # Dropped frame: hold the last commanded pose. + ee_pose[0] = self._last_pose + return + + grip_pos = np.from_dlpack(inp[ControllerInputIndex.GRIP_POSITION]).astype( + np.float64 + ) + grip_ori = np.from_dlpack(inp[ControllerInputIndex.GRIP_ORIENTATION]).astype( + np.float64 + ) # XYZW + # Compose the fixed calibration offset as a body-frame right multiply (q_grip (x) offset) + # and renormalize, so a controller rotation maps 1:1 onto the commanded gripper orientation + # that drives the SE3 IK. + cmd_ori = _quat_mul(grip_ori, self._orientation_offset) + cmd_ori = cmd_ori / np.linalg.norm(cmd_ori) + cmd_ori = cmd_ori.astype(np.float32) + + if self._origin is None: + if not running: + # Connected but not playing yet: hold the last pose and do not latch. The origin + # latches on the first RUNNING frame (the moment Play starts). + ee_pose[0] = self._last_pose + return + # First RUNNING frame (Play just started): latch the controller origin. Seed the home + # from the configured reset-origin on a reset / first engage, else resume the running + # home (the last commanded pose) so a mid-task re-clutch stays put. + self._origin = grip_pos.copy() + if self._home is None: + self._home = self._home_config.copy() + else: + self._home = self._last_pose[:3].astype(np.float64) + if self._debug_log_engage: + # Tuning aid: capture the controller orientation at engage so the calibration + # offset can be set to R_off = quat_inv(q_grip) (x) q_G0. q_G0 is the gripper_link + # orientation in the base frame at the reset pose -- read it once (it is constant) + # from the env's "ee_frame" FrameTransformer: scene["ee_frame"].data + # .target_quat_source[0, 0] (wxyz -> reorder to xyzw before composing R_off). + print( + "[SO101ClutchRetargeter] engage capture (xyzw):\n" + f" q_grip (controller, base frame) = {grip_ori.tolist()}\n" + f" orientation_offset = {self._orientation_offset.tolist()}\n" + f" emitted cmd_ori = {cmd_ori.tolist()}" + ) + + pos = _rebased_position( + grip_pos, self._origin, self._home, _CLUTCH_POSITION_SCALE + ) + self._last_pose = np.concatenate([pos, cmd_ori]).astype(np.float32) + ee_pose[0] = self._last_pose diff --git a/src/retargeters/SO101/gripper_retargeter.py b/src/retargeters/SO101/gripper_retargeter.py new file mode 100644 index 000000000..367df7c38 --- /dev/null +++ b/src/retargeters/SO101/gripper_retargeter.py @@ -0,0 +1,136 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""SO-101 analog gripper retargeter for the absolute-pose XR teleop pipeline. + +The shared :class:`~isaacteleop.retargeters.GripperRetargeter` thresholds the controller +trigger into a binary open/close command (and carries hand-pinch fallback logic that the +SO-101 stacking setup does not use). For proportional grasping we instead want the jaw to +track the trigger continuously: a half-pressed trigger should leave the jaw half-closed. + +:class:`SO101GripperRetargeter` reads one controller's analog trigger value +(:attr:`~isaacteleop.retargeting_engine.tensor_types.ControllerInputIndex.TRIGGER_VALUE`, +nominally ``[0, 1]``) and emits a single *closedness* scalar ``c`` in ``[0, 1]`` under +:data:`GRIPPER_COMMAND_KEY` / :data:`GRIPPER_ELEMENT_LABEL`. + +Polarity / mapping (this module owns its convention): + +- ``c = 0`` means the jaw is fully **OPEN**; ``c = 1`` means fully **CLOSED**. +- Pressing the trigger increases ``c`` (squeeze to grasp): ``trigger -> 1`` drives ``c -> 1``. +- A small released-end deadzone (:data:`_TRIGGER_DEADZONE`) keeps a resting trigger fully + open; values are then rescaled and clamped to ``[0, 1]`` (see :func:`_trigger_to_closedness`). +- Downstream, the order-locked ``JointPositionActionCfg`` applies the affine + ``joint = offset + scale * c`` with ``offset = open angle`` and + ``scale = close angle - open angle``, so ``c = 0`` maps to the open joint angle and ``c = 1`` + to the closed joint angle; the endpoints are exactly the configured open / close angles. + +This convention is deliberately independent of the shared retargeter's ``+1 = open`` / +``-1 = closed`` sign: the affine that maps ``c`` to a joint target lives in the action term, +so this node only has to produce a clean ``[0, 1]`` closedness. +""" + +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource +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 ( + OptionalType, + TensorGroupType, +) +from isaacteleop.retargeting_engine.tensor_types import ( + ControllerInput, + ControllerInputIndex, + FloatType, +) + +# Single source of truth for the stringly-typed pipeline wiring. These *name* the +# already-pinned ``gripper_command`` / ``gripper_value`` strings (the reorderer/output_order +# contract); they do not rename them. +GRIPPER_COMMAND_KEY = "gripper_command" +"""Group / ``connect`` / ``input_config`` key for the gripper channel.""" +GRIPPER_ELEMENT_LABEL = "gripper_value" +"""Flattened element label, used in the reorderer ``input_config`` value and ``output_order``.""" + +# Released-end deadzone on the raw trigger value. A resting trigger reads slightly above 0 on +# some controllers; this keeps the jaw fully open until the operator deliberately squeezes. +_TRIGGER_DEADZONE = 0.05 +# Closedness emitted on a pipeline reset (jaw fully open). +_OPEN_CLOSEDNESS = 0.0 + + +def _trigger_to_closedness(trigger: float) -> float: + """Map a raw controller trigger value to a jaw *closedness* ``c`` in ``[0, 1]``. + + Applies a released-end deadzone then rescales so the usable trigger travel spans the full + closedness range: ``c = clamp((trigger - dz) / (1 - dz), 0, 1)`` with + ``dz = _TRIGGER_DEADZONE``. A resting trigger (``trigger <= dz``) yields ``c = 0`` (open); + a fully pressed trigger (``trigger >= 1``) yields ``c = 1`` (closed). + + Args: + trigger: Raw controller trigger value, nominally in ``[0, 1]`` (clamped if outside). + + Returns: + The jaw closedness ``c`` in ``[0, 1]``; ``0`` = fully open, ``1`` = fully closed. + """ + span = 1.0 - _TRIGGER_DEADZONE + c = (trigger - _TRIGGER_DEADZONE) / span + if c < 0.0: + return 0.0 + if c > 1.0: + return 1.0 + return c + + +class SO101GripperRetargeter(BaseRetargeter): + """Retargets an XR controller's analog trigger to a proportional SO-101 jaw closedness. + + Reads one controller's trigger value and emits a single closedness scalar ``c`` in + ``[0, 1]`` (``0`` = open, ``1`` = closed) under :data:`GRIPPER_COMMAND_KEY` / + :data:`GRIPPER_ELEMENT_LABEL`. The mapping is :func:`_trigger_to_closedness` (released-end + deadzone then clamp). The last value is held on a dropped frame, and reset back to open + (``c = 0``) on a pipeline reset. + + See the module docstring for the full polarity/mapping convention and the downstream affine + that converts ``c`` to a ``gripper`` joint target [rad]. + """ + + def __init__(self, name: str, input_device: str = ControllersSource.RIGHT) -> None: + """Initialize the analog gripper retargeter. + + Args: + name: Name identifier for this retargeter node. + input_device: Controller source key to read the trigger value from. + """ + self._input_device = input_device + super().__init__(name=name) + self._last_closedness = _OPEN_CLOSEDNESS + + def input_spec(self) -> RetargeterIOType: + """Requires the analog trigger of the configured controller (Optional).""" + return {self._input_device: OptionalType(ControllerInput())} + + def output_spec(self) -> RetargeterIOType: + """Outputs a single float jaw closedness ``c`` in ``[0, 1]``.""" + return { + GRIPPER_COMMAND_KEY: TensorGroupType( + GRIPPER_COMMAND_KEY, [FloatType(GRIPPER_ELEMENT_LABEL)] + ) + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Computes the analog jaw closedness from the trigger; holds last on a dropped frame.""" + if context.execution_events.reset: + self._last_closedness = _OPEN_CLOSEDNESS + + gripper_out = outputs[GRIPPER_COMMAND_KEY] + inp = inputs[self._input_device] + if inp.is_none: + # Dropped frame: hold the last commanded closedness. + gripper_out[0] = self._last_closedness + return + + trigger_value = float(inp[ControllerInputIndex.TRIGGER_VALUE]) + self._last_closedness = _trigger_to_closedness(trigger_value) + gripper_out[0] = self._last_closedness diff --git a/src/retargeters/__init__.py b/src/retargeters/__init__.py index afd470c5b..c485e69f2 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 + - SO101ClutchRetargeter: Clutch-rebased absolute EE pose for the SO-101 5-DOF arm + - SO101GripperRetargeter: Proportional (analog) jaw closedness for the SO-101 gripper - SharpaHandRetargeter: Pinocchio/Pink IK-based retargeting for Sharpa hand - SharpaBiManualRetargeter: Bimanual version of SharpaHandRetargeter - Se3AbsRetargeter: Absolute EE pose control @@ -98,6 +100,17 @@ # .gripper_retargeter "GripperRetargeter": (".gripper_retargeter", "GripperRetargeter", None), "GripperRetargeterConfig": (".gripper_retargeter", "GripperRetargeterConfig", None), + # .SO101 (SO-101 5-DOF arm: full-pose clutch EE pose, analog gripper) + "SO101ClutchRetargeter": ( + ".SO101.clutch_retargeter", + "SO101ClutchRetargeter", + None, + ), + "SO101GripperRetargeter": ( + ".SO101.gripper_retargeter", + "SO101GripperRetargeter", + None, + ), # .se3_retargeter (requires retargeters-lite extra: scipy) "Se3AbsRetargeter": (".se3_retargeter", "Se3AbsRetargeter", "retargeters-lite"), "Se3RelRetargeter": (".se3_retargeter", "Se3RelRetargeter", "retargeters-lite"), @@ -189,6 +202,9 @@ def __getattr__(name: str): # Manipulator retargeters "GripperRetargeter", "GripperRetargeterConfig", + # SO-101 5-DOF arm retargeters + "SO101ClutchRetargeter", + "SO101GripperRetargeter", "Se3AbsRetargeter", "Se3RelRetargeter", "Se3RetargeterConfig",