From 7231563ed8b0fa12e798aa9d73aedb157744b105 Mon Sep 17 00:00:00 2001 From: Jiwen Cai Date: Thu, 11 Jun 2026 07:22:21 +0000 Subject: [PATCH 1/2] feat(retargeters): add SO-101 5-DOF arm XR teleop retargeters Add the SO-101 retargeters used by the cube-stack IK-Abs teleop pipeline: - SO101ClutchRetargeter: clutch-rebased absolute end-effector pose, seeded from a configured reset-origin home so engaging does not move the arm and a mid-task re-clutch resumes from the last commanded pose. - SO101WristRetargeter: emits an engage-relative wrist roll (swing-twist about the controller's local Z axis) and an absolute world-elevation wrist pitch taken from the controller's AIM (pointer-ray) pose. Keeps a back-compat SO101RollRetargeter alias. - SO101GripperRetargeter: analog trigger mapped to a jaw closedness in [0, 1] (released-end deadzone, then rescale and clamp). Register the retargeters in the lazy-import registry and add sim-free unit tests covering the swing-twist math, clutch rebasing, pitch elevation and the gripper mapping. --- docs/source/references/retargeting/index.rst | 13 + docs/source/references/retargeting/so101.rst | 179 +++++ src/core/python/pyproject.toml.in | 1 + .../python/test_so101_retargeters.py | 754 ++++++++++++++++++ src/retargeters/CMakeLists.txt | 2 + src/retargeters/SO101/__init__.py | 0 src/retargeters/SO101/clutch_retargeter.py | 249 ++++++ src/retargeters/SO101/gripper_retargeter.py | 136 ++++ src/retargeters/SO101/wrist_retargeter.py | 311 ++++++++ src/retargeters/__init__.py | 22 + 10 files changed, 1667 insertions(+) create mode 100644 docs/source/references/retargeting/so101.rst create mode 100644 src/core/retargeting_engine_tests/python/test_so101_retargeters.py create mode 100644 src/retargeters/SO101/__init__.py create mode 100644 src/retargeters/SO101/clutch_retargeter.py create mode 100644 src/retargeters/SO101/gripper_retargeter.py create mode 100644 src/retargeters/SO101/wrist_retargeter.py diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 81da6c89b..8c8eee35e 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 / SO101RollRetargeter / SO101GripperRetargeter + + Retargeters for the SO-101 5-DOF arm under position-only IK. ``SO101ClutchRetargeter`` + outputs a 7-D ``ee_pose`` like ``Se3AbsRetargeter`` but clutch-rebases controller motion + around an origin captured on engage (no teleport). ``SO101RollRetargeter`` recovers the + controller's roll as a dedicated ``wrist_roll`` channel via swing-twist decomposition, and + ``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..c83d23e61 --- /dev/null +++ b/docs/source/references/retargeting/so101.rst @@ -0,0 +1,179 @@ +.. 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. Its task-space IK is +**position-only** -- a 5-DOF arm cannot track a full 6-DOF end-effector pose -- so the standard +:class:`~isaacteleop.retargeters.Se3AbsRetargeter` + :class:`~isaacteleop.retargeters.GripperRetargeter` +pairing does not fit. These three retargeters provide the missing 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 **position** with + clutch-style rebasing (no teleport on engage). +* :class:`~isaacteleop.retargeters.SO101RollRetargeter` -- recovers the controller's roll as a + dedicated ``wrist_roll`` joint channel that position-only IK would otherwise drop. +* :class:`~isaacteleop.retargeters.SO101GripperRetargeter` -- proportional (analog) jaw + closedness from the controller trigger. + +Together they flatten (via :class:`~isaacteleop.retargeters.TensorReorderer`) into a 5-D action +``[pos_x, pos_y, pos_z, roll, 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 * R_base_from_world @ (p_ctrl - p0)``. + The quaternion is a passthrough and is dropped downstream. + * - ``SO101RollRetargeter`` + - 1 float ``roll_command`` [rad] + - Absolute swing-twist twist of the grip about fixed world Z, driving ``wrist_roll``. + * - ``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 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 live-EE home +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The controller delta ``(p_ctrl - p0)`` is in the (anchor-transformed) **world** frame, while the +IK position command is in the robot **base** frame. When the base is not aligned with the world +frame (e.g. a seated, yawed base) the world-frame delta is rotated into the base frame before +being added to the home. The base placement is **not** hardcoded: the retargeter learns it at +runtime from the optional ``robot_base_pos`` input (the live ``world_T_base`` 4x4 transform, +supplied by the Isaac Lab ``IsaacTeleopDevice`` from its ``base_frame_prim_path``), latched on +engage. When that input is absent the base frame defaults to **identity** -- world and base +coincide and the delta is applied unrotated. + +The ``home`` itself is latched on engage: + +* If the optional ``robot_ee_pos`` input (the live ``world_T_ee`` 4x4 transform, supplied by the + Isaac Lab ``IsaacTeleopDevice`` from its ``ee_frame_prim_path``) is connected, the home is the + robot's **current** end-effector position converted into the latched base frame -- so the arm + stays put on engage. +* Otherwise it falls back to a fixed constant approximating the EE at the init joint pose. + +.. note:: + + The fallback home and the sign/scale knobs carry ``TODO(verify-in-sim)`` markers: the rebasing + math is exact and unit-tested, but the end-to-end controller->EE handedness should be confirmed + in simulation. Because the base placement now arrives at runtime via ``robot_base_pos``, this + library no longer embeds any specific task's robot seat pose. + +Wrist roll +---------- + +Because position-only IK drops the controller's orientation, ``SO101RollRetargeter`` recovers +just the **roll** as a separate joint channel. It takes the swing-twist twist angle of the grip +orientation about fixed world Z via quaternion projection (``2 * atan2(d, w)``), which stays +well-conditioned near a 180-degree rotation where Euler/rotvec extraction is unstable. + +The twist responsiveness is exact and pose-independent, but the absolute zero absorbs the +controller's world *yaw* -- acceptable for the seated, forward-facing stacking setup. Switching +the twist axis to the grip body Z is the fix if yaw leak becomes objectionable. + +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, + SO101RollRetargeter, + 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()) + ee_pos = ValueInput(SO101ClutchRetargeter.ROBOT_EE_POS_INPUT, TransformMatrix()) + base_pos = ValueInput(SO101ClutchRetargeter.ROBOT_BASE_POS_INPUT, TransformMatrix()) + 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), + SO101ClutchRetargeter.ROBOT_EE_POS_INPUT: ee_pos.output(ValueInput.VALUE), + SO101ClutchRetargeter.ROBOT_BASE_POS_INPUT: base_pos.output(ValueInput.VALUE), + }) + + roll = SO101RollRetargeter(name="roll", input_device=ControllersSource.RIGHT) + connected_roll = roll.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 in input_config (must match the retargeter's (7,) output), but + # drop the quaternion by listing only xyz + roll + gripper in output_order. + ee_elements = ["pos_x", "pos_y", "pos_z", "quat_x", "quat_y", "quat_z", "quat_w"] + reorderer = TensorReorderer( + input_config={ + "ee_pose": ee_elements, + "roll_command": ["roll_value"], + "gripper_command": ["gripper_value"], + }, + output_order=["pos_x", "pos_y", "pos_z", "roll_value", "gripper_value"], + name="action_reorderer", + input_types={"ee_pose": "array", "roll_command": "scalar", "gripper_command": "scalar"}, + ) + connected = reorderer.connect({ + "ee_pose": connected_clutch.output("ee_pose"), + "roll_command": connected_roll.output("roll_command"), + "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/roll/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..2e5af09c3 --- /dev/null +++ b/src/core/retargeting_engine_tests/python/test_so101_retargeters.py @@ -0,0 +1,754 @@ +# 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 position-only IK stacking pipeline: + +* :class:`~isaacteleop.retargeters.SO101GripperRetargeter` -- analog trigger -> jaw closedness. +* :class:`~isaacteleop.retargeters.SO101WristRetargeter` -- grip roll -> absolute wrist-roll + angle via swing-twist decomposition, plus absolute world-elevation pitch. +* :class:`~isaacteleop.retargeters.SO101ClutchRetargeter` -- clutch-rebased absolute EE pose. + +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, + SO101RollRetargeter, +) +from isaacteleop.retargeters import SO101WristRetargeter +from isaacteleop.retargeters.SO101.clutch_retargeter import ( + _CLUTCH_HOME_EE_POS, + _rebased_position, +) +from isaacteleop.retargeters.SO101.gripper_retargeter import ( + GRIPPER_COMMAND_KEY, + _TRIGGER_DEADZONE, + _trigger_to_closedness, +) +from isaacteleop.retargeters.SO101.wrist_retargeter import ( + PITCH_COMMAND_KEY, + ROLL_COMMAND_KEY, + _APPROACH_AXIS, + _PITCH_APPROACH_AXIS, + _approach_elevation, + _roll_twist, + _swing_twist_angle, + _WRIST_ROLL_OFFSET_RAD, +) + +# --------------------------------------------------------------------------- +# 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) + + +# =========================================================================== +# SO101RollRetargeter +# =========================================================================== + + +class TestSwingTwistAngle: + """The pure swing-twist decomposition used by the roll retargeter.""" + + def test_identity_is_zero(self): + """The identity quaternion has zero twist about any axis.""" + assert _swing_twist_angle( + np.array([0.0, 0.0, 0.0, 1.0]), _APPROACH_AXIS + ) == pytest.approx(0.0) + + @pytest.mark.parametrize("phi", [0.3, 1.0, -0.7, 2.5, -2.5]) + def test_pure_roll_about_z(self, phi): + """A pure roll of phi about Z recovers phi.""" + q = _quat_xyzw([0.0, 0.0, 1.0], phi) + assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx(phi, abs=1e-9) + + @pytest.mark.parametrize("axis", [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) + def test_pure_swing_is_zero(self, axis): + """A pure swing (tilt) about X or Y has zero twist about Z.""" + q = _quat_xyzw(axis, 0.9) + assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx(0.0, abs=1e-9) + + def test_sign(self): + """Twist sign follows the rotation sign about the axis.""" + assert ( + _swing_twist_angle(_quat_xyzw([0.0, 0.0, 1.0], 0.6), _APPROACH_AXIS) > 0.0 + ) + assert ( + _swing_twist_angle(_quat_xyzw([0.0, 0.0, 1.0], -0.6), _APPROACH_AXIS) < 0.0 + ) + + def test_near_180_swing_guard(self): + """A ~180-degree swing about X (no twist component) hits the degeneracy guard.""" + q = _quat_xyzw([1.0, 0.0, 0.0], math.pi) + assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx(0.0, abs=1e-9) + + def test_scipy_cross_check_orthogonal_swing(self): + """Cross-check the twist against scipy for a roll composed with an orthogonal swing.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + phi = 0.8 + r_swing = Rotation.from_rotvec([0.5, 0.0, 0.0]) + r_twist = Rotation.from_rotvec([0.0, 0.0, phi]) + q = (r_twist * r_swing).as_quat() # scipy returns [x, y, z, w] + assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx(phi, abs=1e-9) + + def test_scipy_cross_check_non_orthogonal_swing(self): + """Cross-check against scipy for a NON-orthogonal swing (swing axis has a Z component). + + The orthogonal-X-swing case passes under either twist convention, so it does not pin the + decomposition. A swing about an axis with a Z component genuinely exercises the + swing-twist split: the recovered twist is no longer simply the planted ``phi``. + """ + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + phi = 0.8 + swing_axis = np.array([1.0, 0.0, 0.4]) + swing_axis = swing_axis / np.linalg.norm(swing_axis) + r_swing = Rotation.from_rotvec(0.6 * swing_axis) + r_twist = Rotation.from_rotvec([0.0, 0.0, phi]) + q = (r_twist * r_swing).as_quat() + + # Independent quaternion-projection twist about Z (oracle). + qn = np.asarray(q, dtype=np.float64) + if qn[3] < 0.0: + qn = -qn + twist = np.array([0.0, 0.0, qn[2], qn[3]]) + twist /= np.linalg.norm(twist) + expected = 2.0 * math.atan2(twist[2], twist[3]) + + assert ( + abs(expected - phi) > 0.05 + ) # the non-orthogonal swing genuinely perturbs the twist + assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx( + expected, abs=1e-9 + ) + + +class TestRollTwistMath: + """The pure relative-twist helper: twist of ``ref^-1 . cur`` about the controller local axis.""" + + def test_pure_local_roll_recovered(self): + """A pure roll about the local axis since the reference is recovered 1:1.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + R0 = Rotation.from_euler( + "y", 90, degrees=True + ) # tilted grasp (approach horizontal) + phi = 0.5 + cur = (R0 * Rotation.from_rotvec([0.0, 0.0, phi])).as_quat() + assert _roll_twist(R0.as_quat(), cur, _APPROACH_AXIS) == pytest.approx( + phi, abs=1e-9 + ) + + @pytest.mark.parametrize("axis", [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) + def test_off_axis_rotation_rejected(self, axis): + """A rotation about a local axis perpendicular to the roll axis yields ~zero twist.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + R0 = Rotation.from_euler("y", 90, degrees=True) + cur = (R0 * Rotation.from_rotvec(np.array(axis) * 0.5)).as_quat() + assert _roll_twist(R0.as_quat(), cur, _APPROACH_AXIS) == pytest.approx( + 0.0, abs=1e-9 + ) + + def test_identity_reference_matches_swing_twist(self): + """With an identity reference the relative twist equals the absolute swing-twist.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + cur = Rotation.from_rotvec([0.2, -0.1, 0.4]).as_quat() + ident = np.array([0.0, 0.0, 0.0, 1.0]) + assert _roll_twist(ident, cur, _APPROACH_AXIS) == pytest.approx( + _swing_twist_angle(cur, _APPROACH_AXIS), abs=1e-12 + ) + + +class TestSO101RollRetargeter: + """End-to-end ``compute`` behavior of the wrist-roll retargeter (engage-relative).""" + + @staticmethod + def _roll(r, grip_ori, **ctx): + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller( + grip_ori=np.asarray(grip_ori, dtype=np.float32) + ) + r.compute(inputs, outputs, _make_context(**ctx)) + return float(outputs[ROLL_COMMAND_KEY][0]) + + def test_output_spec_has_roll_command_key(self): + """Output spec contains the roll command key (among other channels).""" + r = SO101RollRetargeter(name="roll") + assert ROLL_COMMAND_KEY in r.output_spec() + + def test_engage_emits_offset(self): + """The first RUNNING frame latches the reference orientation and emits the offset seed.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + r = SO101RollRetargeter(name="roll") + out = self._roll(r, Rotation.from_euler("y", 90, degrees=True).as_quat()) + assert out == pytest.approx(_WRIST_ROLL_OFFSET_RAD, abs=1e-6) + + @pytest.mark.parametrize("phi", [0.4, -1.1, 2.0]) + def test_relative_roll_about_local_z(self, phi): + """A roll of phi about the controller local Z since engage emits roll == offset + phi.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + r = SO101RollRetargeter(name="roll") + R0 = Rotation.from_euler( + "xy", [20, 35], degrees=True + ) # arbitrary engage orientation + self._roll(r, R0.as_quat()) # engage + out = self._roll(r, (R0 * Rotation.from_rotvec([0.0, 0.0, phi])).as_quat()) + assert out == pytest.approx(_WRIST_ROLL_OFFSET_RAD + phi, abs=1e-6) + + def test_off_axis_rotations_do_not_leak(self): + """Pitch/yaw about local axes perpendicular to the roll axis do not move the roll output. + + This is the regression for the cross-coupling bug: with the old absolute twist about a + fixed world axis, a hand pitch or yaw at a non-vertical grasp leaked fully into wrist_roll. + """ + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + r = SO101RollRetargeter(name="roll") + R0 = Rotation.from_euler( + "y", 90, degrees=True + ) # approach horizontal: worst case for fixed-Z + self._roll(r, R0.as_quat()) # engage + # Rotations about local X and local Y (perpendicular to the local-Z roll axis) -> stay at + # the engage seed (offset). + assert self._roll( + r, (R0 * Rotation.from_rotvec([0.5, 0.0, 0.0])).as_quat() + ) == pytest.approx(_WRIST_ROLL_OFFSET_RAD, abs=1e-6) + assert self._roll( + r, (R0 * Rotation.from_rotvec([0.0, 0.5, 0.0])).as_quat() + ) == pytest.approx(_WRIST_ROLL_OFFSET_RAD, abs=1e-6) + # A roll about local Z still maps 1:1 on top of the offset. + assert self._roll( + r, (R0 * Rotation.from_rotvec([0.0, 0.0, 0.5])).as_quat() + ) == pytest.approx(_WRIST_ROLL_OFFSET_RAD + 0.5, abs=1e-6) + + def test_reengage_resumes_running_roll(self): + """A re-clutch resumes from the last commanded roll (no snap), then accumulates.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + r = SO101RollRetargeter(name="roll") + self._roll(r, Rotation.identity().as_quat()) # engage at identity + phi1 = 0.6 + base = _WRIST_ROLL_OFFSET_RAD + assert self._roll( + r, Rotation.from_rotvec([0.0, 0.0, phi1]).as_quat() + ) == pytest.approx(base + phi1, abs=1e-6) + + # Disengage (STOPPED) -> hold, re-arm reference. + assert self._roll( + r, Rotation.identity().as_quat(), state=ExecutionState.STOPPED + ) == pytest.approx(base + phi1, abs=1e-6) + assert r._ref_quat is None + + # Re-engage at a NEW orientation -> resume from base + phi1 (no jump), then a further + # local-Z roll of phi2 accumulates on top. + R1 = Rotation.from_euler("z", 80, degrees=True) + assert self._roll(r, R1.as_quat()) == pytest.approx(base + phi1, abs=1e-6) + phi2 = -0.3 + assert self._roll( + r, (R1 * Rotation.from_rotvec([0.0, 0.0, phi2])).as_quat() + ) == pytest.approx(base + phi1 + phi2, abs=1e-6) + + def test_reset_returns_to_offset(self): + """A reset re-zeros the running roll back to the offset seed.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + r = SO101RollRetargeter(name="roll") + self._roll(r, Rotation.identity().as_quat()) # engage + self._roll(r, Rotation.from_rotvec([0.0, 0.0, 0.9]).as_quat()) # roll away + out = self._roll(r, Rotation.identity().as_quat(), reset=True) + assert out == pytest.approx(_WRIST_ROLL_OFFSET_RAD, abs=1e-6) + + def test_not_running_holds_without_latching(self): + """While not RUNNING the roll holds the last value and does not latch a reference.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + r = SO101RollRetargeter(name="roll") + out = self._roll( + r, + Rotation.from_rotvec([0.0, 0.0, 0.7]).as_quat(), + state=ExecutionState.STOPPED, + ) + assert out == pytest.approx( + _WRIST_ROLL_OFFSET_RAD, abs=1e-6 + ) # offset, never engaged + assert r._ref_quat is None + + def test_dropped_frame_holds_last(self): + """An absent controller frame holds the last commanded roll.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + r = SO101RollRetargeter(name="roll") + self._roll(r, Rotation.identity().as_quat()) # engage + self._roll( + r, Rotation.from_rotvec([0.0, 0.0, 0.7]).as_quat() + ) # roll to offset + 0.7 + + inputs2, outputs2 = _build_io(r) # controller absent + r.compute(inputs2, outputs2, _make_context()) + assert float(outputs2[ROLL_COMMAND_KEY][0]) == pytest.approx( + _WRIST_ROLL_OFFSET_RAD + 0.7, abs=1e-6 + ) + + def test_roll_alias_points_at_wrist_retargeter(self): + """The legacy SO101RollRetargeter name still resolves (back-compat alias).""" + from isaacteleop.retargeters import SO101RollRetargeter as Legacy + from isaacteleop.retargeters import SO101WristRetargeter + + assert Legacy is SO101WristRetargeter + + +# =========================================================================== +# SO101WristRetargeter (pitch channel) +# =========================================================================== + + +class TestApproachElevationMath: + """The pure approach-axis elevation helper used by the wrist retargeter's pitch channel.""" + + def test_horizontal_is_zero(self): + """An orientation that rotates the approach axis into the horizontal plane gives 0.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + # Rotate the approach axis (+Z) to horizontal via +90deg about Y -> [1, 0, 0]. + q = Rotation.from_euler("y", 90, degrees=True).as_quat() + assert _approach_elevation(q) == pytest.approx(0.0, abs=1e-9) + + def test_identity_points_along_approach_axis(self): + """At identity the approach axis is _PITCH_APPROACH_AXIS, so elevation is its own.""" + expected = math.atan2( + _PITCH_APPROACH_AXIS[2], + math.hypot(_PITCH_APPROACH_AXIS[0], _PITCH_APPROACH_AXIS[1]), + ) + assert _approach_elevation(_ID_QUAT) == pytest.approx(expected, abs=1e-9) + + @pytest.mark.parametrize( + "euler", [(0, 30, 0), (0, -25, 0), (45, 0, 0), (20, 35, 10), (0, 90, 0)] + ) + def test_matches_scipy_for_arbitrary_orientation(self, euler): + """Cross-check the helper's quaternion math against scipy for arbitrary orientations. + + Rotating the approach axis by the orientation and taking atan2(z, ||xy||) is the oracle; + the helper must match it (guards the quaternion sandwich/convention, not a planted angle). + """ + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + R = Rotation.from_euler("xyz", euler, degrees=True) + axis = R.apply(_PITCH_APPROACH_AXIS) + expected = math.atan2(axis[2], math.hypot(axis[0], axis[1])) + assert _approach_elevation(R.as_quat()) == pytest.approx(expected, abs=1e-9) + + +class TestSO101WristRetargeterPitch: + """The wrist retargeter emits an ABSOLUTE pitch channel (no engage latching).""" + + @staticmethod + def _pitch(r, aim_ori, **ctx): + # Pitch is driven by the AIM pose (pointer ray), not the grip pose. + inputs, outputs = _build_io(r) + inputs[ControllersSource.RIGHT] = _make_controller( + aim_ori=np.asarray(aim_ori, dtype=np.float32) + ) + r.compute(inputs, outputs, _make_context(**ctx)) + return float(outputs[PITCH_COMMAND_KEY][0]) + + def test_output_spec_has_pitch_and_roll(self): + """The wrist retargeter exposes both the roll and pitch command channels.""" + from isaacteleop.retargeters.SO101.wrist_retargeter import ROLL_COMMAND_KEY + + r = SO101WristRetargeter(name="wrist") + assert set(r.output_spec()) == {ROLL_COMMAND_KEY, PITCH_COMMAND_KEY} + + def test_pitch_is_absolute_not_engage_relative(self): + """The same aim pose yields the same pitch regardless of when teleop engaged.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + pose = Rotation.from_euler("y", 30, degrees=True).as_quat() + # Two frames suffice: pitch has no accumulated/engage state, so this test would catch + # a future regression that added engage-relative pitch state. + r1 = SO101WristRetargeter(name="wrist") + first = self._pitch(r1, pose) # engage frame + r2 = SO101WristRetargeter(name="wrist") + self._pitch( + r2, Rotation.from_euler("y", -50, degrees=True).as_quat() + ) # different engage + second = self._pitch(r2, pose) + assert first == pytest.approx(second, abs=1e-6) + + def test_pitch_dropped_frame_holds_last(self): + """An absent controller frame holds the last commanded pitch.""" + Rotation = pytest.importorskip("scipy.spatial.transform").Rotation + r = SO101WristRetargeter(name="wrist") + self._pitch(r, Rotation.from_euler("y", 20, degrees=True).as_quat()) + inputs2, outputs2 = _build_io(r) # controller absent + r.compute(inputs2, outputs2, _make_context()) + held = float(outputs2[PITCH_COMMAND_KEY][0]) + again = self._pitch(r, Rotation.from_euler("y", 20, degrees=True).as_quat()) + assert held == pytest.approx(again, abs=1e-6) + + +# =========================================================================== +# 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). The grip orientation passes through unchanged. + """ + r = SO101ClutchRetargeter(name="ee_pose") + 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) 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..cbbe1a385 --- /dev/null +++ b/src/retargeters/SO101/clutch_retargeter.py @@ -0,0 +1,249 @@ +# 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 5D +action contract are untouched. The orientation is a passthrough of the controller grip +orientation and is dropped downstream (position-only IK). + +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 channel is a controller passthrough, + dropped by position-only IK). 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) + + +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 is a passthrough and dropped + downstream. + + 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, + ) -> 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 + (orientation is a controller passthrough, dropped by position-only IK). When + ``None``, falls back to :data:`_CLUTCH_HOME_EE_POS`. + """ + self._input_device = input_device + super().__init__(name=name) + # 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.float32 + ) # XYZW + + 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) + + pos = _rebased_position( + grip_pos, self._origin, self._home, _CLUTCH_POSITION_SCALE + ) + self._last_pose = np.concatenate([pos, grip_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/SO101/wrist_retargeter.py b/src/retargeters/SO101/wrist_retargeter.py new file mode 100644 index 000000000..1954042c4 --- /dev/null +++ b/src/retargeters/SO101/wrist_retargeter.py @@ -0,0 +1,311 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 + +"""SO-101 wrist retargeter for the absolute-pose XR teleop pipeline. + +The SO-101's task-space IK is position-only (it cannot track a full 6-DOF pose with a 5-DOF +arm), so the controller's roll and pitch about its grip axis are otherwise lost. This module +recovers both as dedicated joint channels: :class:`SO101WristRetargeter` reads the XR controller +grip orientation and emits: + +* An **engage-relative** roll [rad] about the controller's own **local Z** axis + (:data:`_APPROACH_AXIS`), forwarded to the ``wrist_roll`` joint. +* An **absolute world-elevation** pitch [rad] of the controller's **aim ray** (the AIM pose + OpenXR provides, not the GRIP pose) above the horizontal plane + (:func:`_approach_elevation`), forwarded to the position+pitch IK pitch channel. + +Roll frame rationale: the roll is the swing-twist twist of the orientation delta *since engage* +(``R_engage^-1 . R_current``) about the grip's local Z axis (see :func:`_roll_twist`). Measuring +the delta about the grip's own axis -- rather than an absolute twist about a fixed world axis -- +isolates wrist roll from hand pitch/yaw: rotations about axes perpendicular to local Z fall into +the (discarded) swing component, so ordinary heading/wobble does not leak into the channel. An +earlier revision took the absolute twist about fixed world Z, which cross-coupled controller yaw +(and, at non-vertical grasps, pitch) into the roll with gain ~1 -- hence over-sensitive control. + +Pitch rationale: the pitch is taken from the controller's AIM ray (where the operator is +pointing), not the GRIP pose, because the ray elevation is the more intuitive handle for the +gripper's in-plane tilt. It is deliberately absolute (not engage-relative) -- the same ray +elevation always commands the same wrist tilt, regardless of when teleop engaged or from where +(e.g. pointing 45° down always tilts the gripper to 45°). Engage-relative pitch would require +the operator to mentally track the engage orientation, which is not natural. + +Like the clutch, the roll channel keeps a running baseline: a plain Stop freezes the last +commanded roll and the next Play resumes from it (re-latching the reference orientation, so no +jump); an explicit reset re-zeros it to :data:`_WRIST_ROLL_OFFSET_RAD`. The pitch channel holds +its last value on a dropped frame and resets to :data:`_PITCH_OFFSET_RAD`. + +The twist is computed by quaternion-projection swing-twist decomposition (see +:func:`_swing_twist_angle`), which stays well-conditioned near a 180-degree rotation where +Euler/rotvec extraction would be unstable. +""" + +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, + FloatType, +) + +# Single source of truth for the stringly-typed pipeline wiring. These mirror the gripper +# node's ``gripper_command`` / ``gripper_value`` convention so the group/connect key and the +# flattened element label / ``output_order`` label stay structurally consistent. +ROLL_COMMAND_KEY = "roll_command" +"""Group / ``connect`` / ``input_config`` key for the roll channel.""" +ROLL_ELEMENT_LABEL = "roll_value" +"""Flattened element label, used in the reorderer ``input_config`` value and ``output_order``.""" + +PITCH_COMMAND_KEY = "pitch_command" +"""Group / ``connect`` / ``input_config`` key for the pitch channel.""" +PITCH_ELEMENT_LABEL = "pitch_value" +"""Flattened element label, used in the reorderer ``input_config`` value and ``output_order``.""" + +# The controller's local roll axis: wrist roll is the rotation about the grip's own Z axis +# (the OpenXR controller local Z). The roll is the twist of the orientation delta since engage +# about this axis, so it is expressed in the grip body frame -- not a fixed world axis -- which +# isolates roll from hand pitch/yaw. TODO(verify-in-sim): confirm local Z is the operator's +# intuitive roll axis for the grip pose convention in use. +_APPROACH_AXIS = np.array([0.0, 0.0, 1.0], dtype=np.float64) +# Controller-twist -> wrist_roll handedness. PhysX bounds the joint at its hard USD limit; a +# guessed node-side clamp would mask sign/scale bugs during bring-up, so none is applied. +_ROLL_SIGN = 1.0 # TODO(verify-in-sim) +# Calibration zero [rad]. Flipped 180 deg so the engaged wrist_roll seed is pi rather than 0 +# (the gripper's neutral roll faces the opposite way from the SO-101 joint zero). TODO(tune-in-sim) +_WRIST_ROLL_OFFSET_RAD = np.pi + +# Local axis whose world elevation defines the commanded pitch. Pitch is read from the +# controller's AIM pose -- the pointer ray that OpenXR already provides -- not the GRIP pose, +# so no grip->ray offset is computed here. The SO-101 pitch is an ABSOLUTE world elevation (not +# engage-relative). Distinct from _APPROACH_AXIS (the grip-local +Z twist axis used for roll). +# TODO(verify-in-sim): confirm the sign for the aim pose convention (flip _PITCH_SIGN if needed). +_PITCH_APPROACH_AXIS = np.array([0.0, 0.0, 1.0], dtype=np.float64) +_PITCH_SIGN = 1.0 # TODO(verify-in-sim) +# TODO(tune-in-sim): absolute elevation that maps to the home tilt +_PITCH_OFFSET_RAD = 0.0 + + +def _swing_twist_angle(quat_xyzw: np.ndarray, axis: np.ndarray) -> float: + """Return the swing-twist twist angle [rad] of a quaternion about an axis. + + Uses quaternion-projection swing-twist decomposition: the twist component is the part of + the rotation about ``axis``, and its angle is ``2 * atan2(d, w)`` where ``d`` is the + projection of the vector part onto ``axis`` and ``w`` is the scalar part. This is robust + near a 180-degree rotation, unlike Euler/rotvec extraction. + + Args: + quat_xyzw: Unit quaternion in ``[x, y, z, w]`` (scipy) order, shape ``(4,)``. + axis: Unit twist axis in the quaternion's frame, shape ``(3,)``. + + Returns: + The twist angle [rad] in ``(-pi, pi]``; ``0.0`` when the twist is degenerate. + """ + q = np.asarray(quat_xyzw, dtype=np.float64) + # Hemisphere-canonicalize so the twist angle has a consistent sign. + if q[3] < 0.0: + q = -q + w = q[3] + d = float(np.dot(q[:3], axis)) + # Degeneracy guard: a pure 180-degree swing leaves no twist component to recover. + if w * w + d * d < 1e-12: + return 0.0 + return 2.0 * np.arctan2(d, w) + + +def _quat_conj(q: np.ndarray) -> np.ndarray: + """Conjugate (inverse, for a unit quaternion) of an ``[x, y, z, w]`` quaternion.""" + return np.array([-q[0], -q[1], -q[2], q[3]], dtype=np.float64) + + +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). + + Matches the SciPy ``Rotation`` composition convention: ``(Ra * Rb).as_quat()`` equals + ``_quat_mul(Ra.as_quat(), Rb.as_quat())``. + """ + 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 _roll_twist( + ref_quat_xyzw: np.ndarray, cur_quat_xyzw: np.ndarray, axis: np.ndarray +) -> float: + """Return the roll [rad] of ``cur`` relative to ``ref`` about a grip-local ``axis``. + + Computes the orientation delta since the reference (``ref^-1 . cur``, expressed in the + reference's body frame) and takes its swing-twist twist about ``axis`` (the grip local roll + axis). Rotations about axes perpendicular to ``axis`` fall into the discarded swing, so hand + pitch/yaw do not leak into the roll. With an identity ``ref`` this reduces to the absolute + swing-twist of ``cur`` about ``axis``. + + Args: + ref_quat_xyzw: Reference (engage) grip orientation, unit quaternion ``[x, y, z, w]``. + cur_quat_xyzw: Current grip orientation, unit quaternion ``[x, y, z, w]``. + axis: Grip-local roll axis, shape ``(3,)``. + + Returns: + The relative roll angle [rad] in ``(-pi, pi]``. + """ + ref = np.asarray(ref_quat_xyzw, dtype=np.float64) + cur = np.asarray(cur_quat_xyzw, dtype=np.float64) + delta = _quat_mul(_quat_conj(ref), cur) + return _swing_twist_angle(delta, axis) + + +def _approach_elevation(quat_xyzw: np.ndarray) -> float: + """Return the elevation [rad] of the pose approach axis above the world horizontal plane. + + Rotates :data:`_PITCH_APPROACH_AXIS` by the given orientation (the controller AIM pose) and + returns ``atan2(z, ||xy||)`` of the result -- the signed angle above horizontal, in + ``[-pi/2, pi/2]``. This is an absolute (not engage-relative) measure: the same orientation + always yields the same elevation, independent of azimuth. + + Args: + quat_xyzw: Unit AIM-pose quaternion in ``[x, y, z, w]`` (scipy) order, shape ``(4,)``. + + Returns: + The approach-axis elevation [rad] above world horizontal. + """ + q = np.asarray(quat_xyzw, dtype=np.float64) + a = _PITCH_APPROACH_AXIS + av = np.array([a[0], a[1], a[2], 0.0], dtype=np.float64) + rotated = _quat_mul(_quat_mul(q, av), _quat_conj(q))[:3] + horiz = float(np.hypot(rotated[0], rotated[1])) + return float(np.arctan2(rotated[2], horiz)) + + +class SO101WristRetargeter(BaseRetargeter): + """Retargets XR controller grip orientation to SO-101 wrist-roll and wrist-pitch commands. + + Reads the grip orientation of one controller and emits two channels: + + * **Roll** (engage-relative): the swing-twist twist [rad] of ``R_engage^-1 . R_current`` + about the grip's own local Z axis (:data:`_APPROACH_AXIS`, see :func:`_roll_twist`), + offset by :data:`_WRIST_ROLL_OFFSET_RAD` and signed by :data:`_ROLL_SIGN`. Measuring the + delta about the grip's own axis isolates wrist roll from hand pitch/yaw (those fall into + the discarded swing). Like the clutch, the channel keeps a running baseline: the reference + orientation is latched on the first RUNNING frame and re-armed whenever teleop is not + RUNNING, so each Play resumes from the last commanded roll without a jump. A reset + re-zeros the roll to the offset. + + * **Pitch** (absolute): the world elevation [rad] of the controller's AIM ray + (:func:`_approach_elevation`), signed by :data:`_PITCH_SIGN` and offset by + :data:`_PITCH_OFFSET_RAD`. Taken from the AIM pose (pointer), not the GRIP pose. The same + aim orientation always commands the same pitch, independent of when teleop engaged or the + azimuth of the controller. The last value is held on a dropped/aim-invalid frame, and + reset to the offset. + + Outputs floats under :data:`ROLL_COMMAND_KEY` / :data:`ROLL_ELEMENT_LABEL` and + :data:`PITCH_COMMAND_KEY` / :data:`PITCH_ELEMENT_LABEL`. + """ + + def __init__(self, name: str, input_device: str = ControllersSource.RIGHT) -> None: + """Initialize the wrist retargeter. + + Args: + name: Name identifier for this retargeter node. + input_device: Controller source key to read the grip orientation from. + """ + self._input_device = input_device + super().__init__(name=name) + # Reference grip orientation latched on engage (``[x, y, z, w]``), or ``None`` when + # re-armed (not yet engaged this episode / after a Stop or reset). + self._ref_quat: np.ndarray | None = None + # Running roll baseline [rad], latched on engage to the last commanded roll so a re-clutch + # resumes there. Re-zeroed to the offset on reset. + self._roll_baseline = _WRIST_ROLL_OFFSET_RAD + self._last_roll = _WRIST_ROLL_OFFSET_RAD + # Last commanded pitch [rad]; held on dropped frames and reset to offset. + self._last_pitch = _PITCH_OFFSET_RAD + + def input_spec(self) -> RetargeterIOType: + """Requires the grip orientation of the configured controller (Optional).""" + return {self._input_device: OptionalType(ControllerInput())} + + def output_spec(self) -> RetargeterIOType: + """Outputs engage-relative wrist-roll and absolute wrist-pitch [rad] under ``ROLL_COMMAND_KEY`` / ``PITCH_COMMAND_KEY``.""" + return { + ROLL_COMMAND_KEY: TensorGroupType( + ROLL_COMMAND_KEY, [FloatType(ROLL_ELEMENT_LABEL)] + ), + PITCH_COMMAND_KEY: TensorGroupType( + PITCH_COMMAND_KEY, [FloatType(PITCH_ELEMENT_LABEL)] + ), + } + + def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: + """Computes engage-relative roll and absolute pitch [rad]; holds last on a dropped frame.""" + running = context.execution_events.execution_state == ExecutionState.RUNNING + + if context.execution_events.reset: + # Fresh episode: re-arm the reference, re-zero roll to the offset, reset pitch. + self._ref_quat = None + self._last_roll = _WRIST_ROLL_OFFSET_RAD + self._last_pitch = _PITCH_OFFSET_RAD + elif not running: + # A plain Stop re-arms the reference but FREEZES the running roll, so the next Play + # resumes from the last commanded value. + self._ref_quat = None + + roll_out = outputs[ROLL_COMMAND_KEY] + pitch_out = outputs[PITCH_COMMAND_KEY] + inp = inputs[self._input_device] + + if inp.is_none: + # Dropped frame: hold the last commanded values for both channels. + roll_out[0] = self._last_roll + pitch_out[0] = self._last_pitch + return + + quat_xyzw = np.from_dlpack(inp[ControllerInputIndex.GRIP_ORIENTATION]).astype( + np.float64 + ) # XYZW + + if self._ref_quat is None: + if not running: + # Connected but not playing yet: hold the last values and do not latch the reference. + roll_out[0] = self._last_roll + pitch_out[0] = self._last_pitch + return + # First RUNNING frame (Play just started): latch the reference orientation and the + # running baseline (the last commanded roll) so the channel resumes without a jump. + self._ref_quat = quat_xyzw.copy() + self._roll_baseline = self._last_roll + + self._last_roll = self._roll_baseline + _ROLL_SIGN * _roll_twist( + self._ref_quat, quat_xyzw, _APPROACH_AXIS + ) + roll_out[0] = self._last_roll + + # Pitch is computed from the AIM pose (pointer ray), not the GRIP pose. Hold the last + # pitch if the aim pose is not tracked this frame. + if bool(inp[ControllerInputIndex.AIM_IS_VALID]): + aim_quat_xyzw = np.from_dlpack( + inp[ControllerInputIndex.AIM_ORIENTATION] + ).astype(np.float64) # XYZW + self._last_pitch = ( + _PITCH_SIGN * _approach_elevation(aim_quat_xyzw) + _PITCH_OFFSET_RAD + ) + pitch_out[0] = self._last_pitch + + +# Back-compat alias: the class was formerly named SO101RollRetargeter. +SO101RollRetargeter = SO101WristRetargeter diff --git a/src/retargeters/__init__.py b/src/retargeters/__init__.py index afd470c5b..317d5c221 100644 --- a/src/retargeters/__init__.py +++ b/src/retargeters/__init__.py @@ -16,6 +16,10 @@ - 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 + - SO101WristRetargeter: Engage-relative wrist roll + absolute pitch for the SO-101 5-DOF arm + - SO101RollRetargeter: Back-compat alias for SO101WristRetargeter + - 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 +102,19 @@ # .gripper_retargeter "GripperRetargeter": (".gripper_retargeter", "GripperRetargeter", None), "GripperRetargeterConfig": (".gripper_retargeter", "GripperRetargeterConfig", None), + # .SO101 (SO-101 5-DOF arm: clutch EE pose, wrist roll+pitch, analog gripper) + "SO101ClutchRetargeter": ( + ".SO101.clutch_retargeter", + "SO101ClutchRetargeter", + None, + ), + "SO101WristRetargeter": (".SO101.wrist_retargeter", "SO101WristRetargeter", None), + "SO101RollRetargeter": (".SO101.wrist_retargeter", "SO101RollRetargeter", 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 +206,11 @@ def __getattr__(name: str): # Manipulator retargeters "GripperRetargeter", "GripperRetargeterConfig", + # SO-101 5-DOF arm retargeters + "SO101ClutchRetargeter", + "SO101WristRetargeter", + "SO101RollRetargeter", + "SO101GripperRetargeter", "Se3AbsRetargeter", "Se3RelRetargeter", "Se3RetargeterConfig", From 86335072507b0848c53f460b6e16c3c80b24c799 Mon Sep 17 00:00:00 2001 From: Jiwen Cai Date: Fri, 12 Jun 2026 19:21:00 +0000 Subject: [PATCH 2/2] refactor(so101): emit full pose for SE3 IK; body-frame orientation offset - Emit full pose for SE3 IK and drop the separate wrist retargeter - Apply body-frame right-multiply orientation offset (Rz(pi) default) - Add engage-capture debug log to the clutch retargeter --- docs/source/references/retargeting/index.rst | 10 +- docs/source/references/retargeting/so101.rst | 113 ++--- .../python/test_so101_retargeters.py | 408 +++--------------- src/retargeters/SO101/clutch_retargeter.py | 98 ++++- src/retargeters/SO101/wrist_retargeter.py | 311 ------------- src/retargeters/__init__.py | 8 +- 6 files changed, 192 insertions(+), 756 deletions(-) delete mode 100644 src/retargeters/SO101/wrist_retargeter.py diff --git a/docs/source/references/retargeting/index.rst b/docs/source/references/retargeting/index.rst index 8c8eee35e..4e594f46a 100644 --- a/docs/source/references/retargeting/index.rst +++ b/docs/source/references/retargeting/index.rst @@ -44,12 +44,12 @@ Available Retargeters ``hand_side`` (``"left"`` or ``"right"``), ``gripper_close_meters``, ``gripper_open_meters``, and ``controller_threshold`` for trigger-based closing. -.. dropdown:: SO101ClutchRetargeter / SO101RollRetargeter / SO101GripperRetargeter +.. dropdown:: SO101ClutchRetargeter / SO101GripperRetargeter - Retargeters for the SO-101 5-DOF arm under position-only IK. ``SO101ClutchRetargeter`` - outputs a 7-D ``ee_pose`` like ``Se3AbsRetargeter`` but clutch-rebases controller motion - around an origin captured on engage (no teleport). ``SO101RollRetargeter`` recovers the - controller's roll as a dedicated ``wrist_roll`` channel via swing-twist decomposition, and + 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. diff --git a/docs/source/references/retargeting/so101.rst b/docs/source/references/retargeting/so101.rst index c83d23e61..a57a5cc32 100644 --- a/docs/source/references/retargeting/so101.rst +++ b/docs/source/references/retargeting/so101.rst @@ -4,22 +4,21 @@ Retargeters: SO-101 (5-DOF arm) =============================== -The SO-101 is a low-cost 5-DOF arm with a single-jaw gripper. Its task-space IK is -**position-only** -- a 5-DOF arm cannot track a full 6-DOF end-effector pose -- so the standard -:class:`~isaacteleop.retargeters.Se3AbsRetargeter` + :class:`~isaacteleop.retargeters.GripperRetargeter` -pairing does not fit. These three retargeters provide the missing pieces for comfortable XR -controller teleoperation of the arm (used by the Isaac Lab ``Isaac-Stack-Cube-SO101-IK-Abs-v0`` +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 **position** with - clutch-style rebasing (no teleport on engage). -* :class:`~isaacteleop.retargeters.SO101RollRetargeter` -- recovers the controller's roll as a - dedicated ``wrist_roll`` joint channel that position-only IK would otherwise drop. +* :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 a 5-D action -``[pos_x, pos_y, pos_z, roll, gripper]``. +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 ----------- @@ -34,11 +33,9 @@ At a glance * - ``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 * R_base_from_world @ (p_ctrl - p0)``. - The quaternion is a passthrough and is dropped downstream. - * - ``SO101RollRetargeter`` - - 1 float ``roll_command`` [rad] - - Absolute swing-twist twist of the grip about fixed world Z, driving ``wrist_roll``. + 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. @@ -49,52 +46,43 @@ 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 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. +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 live-EE home -^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Frames and the home +^^^^^^^^^^^^^^^^^^^^ -The controller delta ``(p_ctrl - p0)`` is in the (anchor-transformed) **world** frame, while the -IK position command is in the robot **base** frame. When the base is not aligned with the world -frame (e.g. a seated, yawed base) the world-frame delta is rotated into the base frame before -being added to the home. The base placement is **not** hardcoded: the retargeter learns it at -runtime from the optional ``robot_base_pos`` input (the live ``world_T_base`` 4x4 transform, -supplied by the Isaac Lab ``IsaacTeleopDevice`` from its ``base_frame_prim_path``), latched on -engage. When that input is absent the base frame defaults to **identity** -- world and base -coincide and the delta is applied unrotated. +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`` itself is latched on engage: - -* If the optional ``robot_ee_pos`` input (the live ``world_T_ee`` 4x4 transform, supplied by the - Isaac Lab ``IsaacTeleopDevice`` from its ``ee_frame_prim_path``) is connected, the home is the - robot's **current** end-effector position converted into the latched base frame -- so the arm - stays put on engage. -* Otherwise it falls back to a fixed constant approximating the EE at the init joint pose. +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 and the sign/scale knobs carry ``TODO(verify-in-sim)`` markers: the rebasing - math is exact and unit-tested, but the end-to-end controller->EE handedness should be confirmed - in simulation. Because the base placement now arrives at runtime via ``robot_base_pos``, this - library no longer embeds any specific task's robot seat pose. - -Wrist roll ----------- + 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. -Because position-only IK drops the controller's orientation, ``SO101RollRetargeter`` recovers -just the **roll** as a separate joint channel. It takes the swing-twist twist angle of the grip -orientation about fixed world Z via quaternion projection (``2 * atan2(d, w)``), which stays -well-conditioned near a 180-degree rotation where Euler/rotvec extraction is unstable. +Orientation calibration +----------------------- -The twist responsiveness is exact and pose-independent, but the absolute zero absorbs the -controller's world *yaw* -- acceptable for the seated, forward-facing stacking setup. Switching -the twist axis to the grip body Z is the fix if yaw leak becomes objectionable. +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 ------- @@ -114,7 +102,6 @@ Use it from Python from isaacteleop.retargeters import ( SO101ClutchRetargeter, SO101GripperRetargeter, - SO101RollRetargeter, TensorReorderer, ) from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource @@ -124,43 +111,33 @@ Use it from Python def build_so101_stack_pipeline(): controllers = ControllersSource(name="controllers") world_T_anchor = ValueInput("world_T_anchor", TransformMatrix()) - ee_pos = ValueInput(SO101ClutchRetargeter.ROBOT_EE_POS_INPUT, TransformMatrix()) - base_pos = ValueInput(SO101ClutchRetargeter.ROBOT_BASE_POS_INPUT, 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), - SO101ClutchRetargeter.ROBOT_EE_POS_INPUT: ee_pos.output(ValueInput.VALUE), - SO101ClutchRetargeter.ROBOT_BASE_POS_INPUT: base_pos.output(ValueInput.VALUE), }) - roll = SO101RollRetargeter(name="roll", input_device=ControllersSource.RIGHT) - connected_roll = roll.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 in input_config (must match the retargeter's (7,) output), but - # drop the quaternion by listing only xyz + roll + gripper in output_order. + # 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, - "roll_command": ["roll_value"], "gripper_command": ["gripper_value"], }, - output_order=["pos_x", "pos_y", "pos_z", "roll_value", "gripper_value"], + output_order=ee_elements + ["gripper_value"], name="action_reorderer", - input_types={"ee_pose": "array", "roll_command": "scalar", "gripper_command": "scalar"}, + input_types={"ee_pose": "array", "gripper_command": "scalar"}, ) connected = reorderer.connect({ "ee_pose": connected_clutch.output("ee_pose"), - "roll_command": connected_roll.output("roll_command"), "gripper_command": connected_gripper.output("gripper_command"), }) return OutputCombiner({"action": connected.output("output")}) @@ -171,7 +148,7 @@ See :ref:`isaac-teleop-pipeline-builder` for the general pipeline-builder patter Validate -------- -The retargeters ship with sim-free unit tests (trigger/roll/clutch math plus per-frame +The retargeters ship with sim-free unit tests (trigger/clutch math plus per-frame ``compute`` behavior): .. code-block:: console diff --git a/src/core/retargeting_engine_tests/python/test_so101_retargeters.py b/src/core/retargeting_engine_tests/python/test_so101_retargeters.py index 2e5af09c3..7b69946b4 100644 --- a/src/core/retargeting_engine_tests/python/test_so101_retargeters.py +++ b/src/core/retargeting_engine_tests/python/test_so101_retargeters.py @@ -3,12 +3,11 @@ """Sim-free unit tests for the SO-101 XR teleop retargeters. -Covers the SO-101 retargeters that drive the position-only IK stacking pipeline: +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.SO101WristRetargeter` -- grip roll -> absolute wrist-roll - angle via swing-twist decomposition, plus absolute world-elevation pitch. -* :class:`~isaacteleop.retargeters.SO101ClutchRetargeter` -- clutch-rebased absolute EE pose. +* :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 @@ -39,11 +38,10 @@ from isaacteleop.retargeters import ( SO101ClutchRetargeter, SO101GripperRetargeter, - SO101RollRetargeter, ) -from isaacteleop.retargeters import SO101WristRetargeter from isaacteleop.retargeters.SO101.clutch_retargeter import ( _CLUTCH_HOME_EE_POS, + _quat_mul, _rebased_position, ) from isaacteleop.retargeters.SO101.gripper_retargeter import ( @@ -51,16 +49,6 @@ _TRIGGER_DEADZONE, _trigger_to_closedness, ) -from isaacteleop.retargeters.SO101.wrist_retargeter import ( - PITCH_COMMAND_KEY, - ROLL_COMMAND_KEY, - _APPROACH_AXIS, - _PITCH_APPROACH_AXIS, - _approach_elevation, - _roll_twist, - _swing_twist_angle, - _WRIST_ROLL_OFFSET_RAD, -) # --------------------------------------------------------------------------- # Helpers (mirror the patterns in test_sharpa_hand_retargeter.py) @@ -223,340 +211,6 @@ def test_reset_reopens(self): assert float(outputs2[GRIPPER_COMMAND_KEY][0]) == pytest.approx(0.0) -# =========================================================================== -# SO101RollRetargeter -# =========================================================================== - - -class TestSwingTwistAngle: - """The pure swing-twist decomposition used by the roll retargeter.""" - - def test_identity_is_zero(self): - """The identity quaternion has zero twist about any axis.""" - assert _swing_twist_angle( - np.array([0.0, 0.0, 0.0, 1.0]), _APPROACH_AXIS - ) == pytest.approx(0.0) - - @pytest.mark.parametrize("phi", [0.3, 1.0, -0.7, 2.5, -2.5]) - def test_pure_roll_about_z(self, phi): - """A pure roll of phi about Z recovers phi.""" - q = _quat_xyzw([0.0, 0.0, 1.0], phi) - assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx(phi, abs=1e-9) - - @pytest.mark.parametrize("axis", [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) - def test_pure_swing_is_zero(self, axis): - """A pure swing (tilt) about X or Y has zero twist about Z.""" - q = _quat_xyzw(axis, 0.9) - assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx(0.0, abs=1e-9) - - def test_sign(self): - """Twist sign follows the rotation sign about the axis.""" - assert ( - _swing_twist_angle(_quat_xyzw([0.0, 0.0, 1.0], 0.6), _APPROACH_AXIS) > 0.0 - ) - assert ( - _swing_twist_angle(_quat_xyzw([0.0, 0.0, 1.0], -0.6), _APPROACH_AXIS) < 0.0 - ) - - def test_near_180_swing_guard(self): - """A ~180-degree swing about X (no twist component) hits the degeneracy guard.""" - q = _quat_xyzw([1.0, 0.0, 0.0], math.pi) - assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx(0.0, abs=1e-9) - - def test_scipy_cross_check_orthogonal_swing(self): - """Cross-check the twist against scipy for a roll composed with an orthogonal swing.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - phi = 0.8 - r_swing = Rotation.from_rotvec([0.5, 0.0, 0.0]) - r_twist = Rotation.from_rotvec([0.0, 0.0, phi]) - q = (r_twist * r_swing).as_quat() # scipy returns [x, y, z, w] - assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx(phi, abs=1e-9) - - def test_scipy_cross_check_non_orthogonal_swing(self): - """Cross-check against scipy for a NON-orthogonal swing (swing axis has a Z component). - - The orthogonal-X-swing case passes under either twist convention, so it does not pin the - decomposition. A swing about an axis with a Z component genuinely exercises the - swing-twist split: the recovered twist is no longer simply the planted ``phi``. - """ - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - phi = 0.8 - swing_axis = np.array([1.0, 0.0, 0.4]) - swing_axis = swing_axis / np.linalg.norm(swing_axis) - r_swing = Rotation.from_rotvec(0.6 * swing_axis) - r_twist = Rotation.from_rotvec([0.0, 0.0, phi]) - q = (r_twist * r_swing).as_quat() - - # Independent quaternion-projection twist about Z (oracle). - qn = np.asarray(q, dtype=np.float64) - if qn[3] < 0.0: - qn = -qn - twist = np.array([0.0, 0.0, qn[2], qn[3]]) - twist /= np.linalg.norm(twist) - expected = 2.0 * math.atan2(twist[2], twist[3]) - - assert ( - abs(expected - phi) > 0.05 - ) # the non-orthogonal swing genuinely perturbs the twist - assert _swing_twist_angle(q, _APPROACH_AXIS) == pytest.approx( - expected, abs=1e-9 - ) - - -class TestRollTwistMath: - """The pure relative-twist helper: twist of ``ref^-1 . cur`` about the controller local axis.""" - - def test_pure_local_roll_recovered(self): - """A pure roll about the local axis since the reference is recovered 1:1.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - R0 = Rotation.from_euler( - "y", 90, degrees=True - ) # tilted grasp (approach horizontal) - phi = 0.5 - cur = (R0 * Rotation.from_rotvec([0.0, 0.0, phi])).as_quat() - assert _roll_twist(R0.as_quat(), cur, _APPROACH_AXIS) == pytest.approx( - phi, abs=1e-9 - ) - - @pytest.mark.parametrize("axis", [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) - def test_off_axis_rotation_rejected(self, axis): - """A rotation about a local axis perpendicular to the roll axis yields ~zero twist.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - R0 = Rotation.from_euler("y", 90, degrees=True) - cur = (R0 * Rotation.from_rotvec(np.array(axis) * 0.5)).as_quat() - assert _roll_twist(R0.as_quat(), cur, _APPROACH_AXIS) == pytest.approx( - 0.0, abs=1e-9 - ) - - def test_identity_reference_matches_swing_twist(self): - """With an identity reference the relative twist equals the absolute swing-twist.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - cur = Rotation.from_rotvec([0.2, -0.1, 0.4]).as_quat() - ident = np.array([0.0, 0.0, 0.0, 1.0]) - assert _roll_twist(ident, cur, _APPROACH_AXIS) == pytest.approx( - _swing_twist_angle(cur, _APPROACH_AXIS), abs=1e-12 - ) - - -class TestSO101RollRetargeter: - """End-to-end ``compute`` behavior of the wrist-roll retargeter (engage-relative).""" - - @staticmethod - def _roll(r, grip_ori, **ctx): - inputs, outputs = _build_io(r) - inputs[ControllersSource.RIGHT] = _make_controller( - grip_ori=np.asarray(grip_ori, dtype=np.float32) - ) - r.compute(inputs, outputs, _make_context(**ctx)) - return float(outputs[ROLL_COMMAND_KEY][0]) - - def test_output_spec_has_roll_command_key(self): - """Output spec contains the roll command key (among other channels).""" - r = SO101RollRetargeter(name="roll") - assert ROLL_COMMAND_KEY in r.output_spec() - - def test_engage_emits_offset(self): - """The first RUNNING frame latches the reference orientation and emits the offset seed.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - r = SO101RollRetargeter(name="roll") - out = self._roll(r, Rotation.from_euler("y", 90, degrees=True).as_quat()) - assert out == pytest.approx(_WRIST_ROLL_OFFSET_RAD, abs=1e-6) - - @pytest.mark.parametrize("phi", [0.4, -1.1, 2.0]) - def test_relative_roll_about_local_z(self, phi): - """A roll of phi about the controller local Z since engage emits roll == offset + phi.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - r = SO101RollRetargeter(name="roll") - R0 = Rotation.from_euler( - "xy", [20, 35], degrees=True - ) # arbitrary engage orientation - self._roll(r, R0.as_quat()) # engage - out = self._roll(r, (R0 * Rotation.from_rotvec([0.0, 0.0, phi])).as_quat()) - assert out == pytest.approx(_WRIST_ROLL_OFFSET_RAD + phi, abs=1e-6) - - def test_off_axis_rotations_do_not_leak(self): - """Pitch/yaw about local axes perpendicular to the roll axis do not move the roll output. - - This is the regression for the cross-coupling bug: with the old absolute twist about a - fixed world axis, a hand pitch or yaw at a non-vertical grasp leaked fully into wrist_roll. - """ - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - r = SO101RollRetargeter(name="roll") - R0 = Rotation.from_euler( - "y", 90, degrees=True - ) # approach horizontal: worst case for fixed-Z - self._roll(r, R0.as_quat()) # engage - # Rotations about local X and local Y (perpendicular to the local-Z roll axis) -> stay at - # the engage seed (offset). - assert self._roll( - r, (R0 * Rotation.from_rotvec([0.5, 0.0, 0.0])).as_quat() - ) == pytest.approx(_WRIST_ROLL_OFFSET_RAD, abs=1e-6) - assert self._roll( - r, (R0 * Rotation.from_rotvec([0.0, 0.5, 0.0])).as_quat() - ) == pytest.approx(_WRIST_ROLL_OFFSET_RAD, abs=1e-6) - # A roll about local Z still maps 1:1 on top of the offset. - assert self._roll( - r, (R0 * Rotation.from_rotvec([0.0, 0.0, 0.5])).as_quat() - ) == pytest.approx(_WRIST_ROLL_OFFSET_RAD + 0.5, abs=1e-6) - - def test_reengage_resumes_running_roll(self): - """A re-clutch resumes from the last commanded roll (no snap), then accumulates.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - r = SO101RollRetargeter(name="roll") - self._roll(r, Rotation.identity().as_quat()) # engage at identity - phi1 = 0.6 - base = _WRIST_ROLL_OFFSET_RAD - assert self._roll( - r, Rotation.from_rotvec([0.0, 0.0, phi1]).as_quat() - ) == pytest.approx(base + phi1, abs=1e-6) - - # Disengage (STOPPED) -> hold, re-arm reference. - assert self._roll( - r, Rotation.identity().as_quat(), state=ExecutionState.STOPPED - ) == pytest.approx(base + phi1, abs=1e-6) - assert r._ref_quat is None - - # Re-engage at a NEW orientation -> resume from base + phi1 (no jump), then a further - # local-Z roll of phi2 accumulates on top. - R1 = Rotation.from_euler("z", 80, degrees=True) - assert self._roll(r, R1.as_quat()) == pytest.approx(base + phi1, abs=1e-6) - phi2 = -0.3 - assert self._roll( - r, (R1 * Rotation.from_rotvec([0.0, 0.0, phi2])).as_quat() - ) == pytest.approx(base + phi1 + phi2, abs=1e-6) - - def test_reset_returns_to_offset(self): - """A reset re-zeros the running roll back to the offset seed.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - r = SO101RollRetargeter(name="roll") - self._roll(r, Rotation.identity().as_quat()) # engage - self._roll(r, Rotation.from_rotvec([0.0, 0.0, 0.9]).as_quat()) # roll away - out = self._roll(r, Rotation.identity().as_quat(), reset=True) - assert out == pytest.approx(_WRIST_ROLL_OFFSET_RAD, abs=1e-6) - - def test_not_running_holds_without_latching(self): - """While not RUNNING the roll holds the last value and does not latch a reference.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - r = SO101RollRetargeter(name="roll") - out = self._roll( - r, - Rotation.from_rotvec([0.0, 0.0, 0.7]).as_quat(), - state=ExecutionState.STOPPED, - ) - assert out == pytest.approx( - _WRIST_ROLL_OFFSET_RAD, abs=1e-6 - ) # offset, never engaged - assert r._ref_quat is None - - def test_dropped_frame_holds_last(self): - """An absent controller frame holds the last commanded roll.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - r = SO101RollRetargeter(name="roll") - self._roll(r, Rotation.identity().as_quat()) # engage - self._roll( - r, Rotation.from_rotvec([0.0, 0.0, 0.7]).as_quat() - ) # roll to offset + 0.7 - - inputs2, outputs2 = _build_io(r) # controller absent - r.compute(inputs2, outputs2, _make_context()) - assert float(outputs2[ROLL_COMMAND_KEY][0]) == pytest.approx( - _WRIST_ROLL_OFFSET_RAD + 0.7, abs=1e-6 - ) - - def test_roll_alias_points_at_wrist_retargeter(self): - """The legacy SO101RollRetargeter name still resolves (back-compat alias).""" - from isaacteleop.retargeters import SO101RollRetargeter as Legacy - from isaacteleop.retargeters import SO101WristRetargeter - - assert Legacy is SO101WristRetargeter - - -# =========================================================================== -# SO101WristRetargeter (pitch channel) -# =========================================================================== - - -class TestApproachElevationMath: - """The pure approach-axis elevation helper used by the wrist retargeter's pitch channel.""" - - def test_horizontal_is_zero(self): - """An orientation that rotates the approach axis into the horizontal plane gives 0.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - # Rotate the approach axis (+Z) to horizontal via +90deg about Y -> [1, 0, 0]. - q = Rotation.from_euler("y", 90, degrees=True).as_quat() - assert _approach_elevation(q) == pytest.approx(0.0, abs=1e-9) - - def test_identity_points_along_approach_axis(self): - """At identity the approach axis is _PITCH_APPROACH_AXIS, so elevation is its own.""" - expected = math.atan2( - _PITCH_APPROACH_AXIS[2], - math.hypot(_PITCH_APPROACH_AXIS[0], _PITCH_APPROACH_AXIS[1]), - ) - assert _approach_elevation(_ID_QUAT) == pytest.approx(expected, abs=1e-9) - - @pytest.mark.parametrize( - "euler", [(0, 30, 0), (0, -25, 0), (45, 0, 0), (20, 35, 10), (0, 90, 0)] - ) - def test_matches_scipy_for_arbitrary_orientation(self, euler): - """Cross-check the helper's quaternion math against scipy for arbitrary orientations. - - Rotating the approach axis by the orientation and taking atan2(z, ||xy||) is the oracle; - the helper must match it (guards the quaternion sandwich/convention, not a planted angle). - """ - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - R = Rotation.from_euler("xyz", euler, degrees=True) - axis = R.apply(_PITCH_APPROACH_AXIS) - expected = math.atan2(axis[2], math.hypot(axis[0], axis[1])) - assert _approach_elevation(R.as_quat()) == pytest.approx(expected, abs=1e-9) - - -class TestSO101WristRetargeterPitch: - """The wrist retargeter emits an ABSOLUTE pitch channel (no engage latching).""" - - @staticmethod - def _pitch(r, aim_ori, **ctx): - # Pitch is driven by the AIM pose (pointer ray), not the grip pose. - inputs, outputs = _build_io(r) - inputs[ControllersSource.RIGHT] = _make_controller( - aim_ori=np.asarray(aim_ori, dtype=np.float32) - ) - r.compute(inputs, outputs, _make_context(**ctx)) - return float(outputs[PITCH_COMMAND_KEY][0]) - - def test_output_spec_has_pitch_and_roll(self): - """The wrist retargeter exposes both the roll and pitch command channels.""" - from isaacteleop.retargeters.SO101.wrist_retargeter import ROLL_COMMAND_KEY - - r = SO101WristRetargeter(name="wrist") - assert set(r.output_spec()) == {ROLL_COMMAND_KEY, PITCH_COMMAND_KEY} - - def test_pitch_is_absolute_not_engage_relative(self): - """The same aim pose yields the same pitch regardless of when teleop engaged.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - pose = Rotation.from_euler("y", 30, degrees=True).as_quat() - # Two frames suffice: pitch has no accumulated/engage state, so this test would catch - # a future regression that added engage-relative pitch state. - r1 = SO101WristRetargeter(name="wrist") - first = self._pitch(r1, pose) # engage frame - r2 = SO101WristRetargeter(name="wrist") - self._pitch( - r2, Rotation.from_euler("y", -50, degrees=True).as_quat() - ) # different engage - second = self._pitch(r2, pose) - assert first == pytest.approx(second, abs=1e-6) - - def test_pitch_dropped_frame_holds_last(self): - """An absent controller frame holds the last commanded pitch.""" - Rotation = pytest.importorskip("scipy.spatial.transform").Rotation - r = SO101WristRetargeter(name="wrist") - self._pitch(r, Rotation.from_euler("y", 20, degrees=True).as_quat()) - inputs2, outputs2 = _build_io(r) # controller absent - r.compute(inputs2, outputs2, _make_context()) - held = float(outputs2[PITCH_COMMAND_KEY][0]) - again = self._pitch(r, Rotation.from_euler("y", 20, degrees=True).as_quat()) - assert held == pytest.approx(again, abs=1e-6) - - # =========================================================================== # SO101ClutchRetargeter # =========================================================================== @@ -623,9 +277,13 @@ 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). The grip orientation passes through unchanged. + 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") + 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( @@ -752,3 +410,49 @@ def test_dropped_frame_holds_last_pose(self): 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/SO101/clutch_retargeter.py b/src/retargeters/SO101/clutch_retargeter.py index cbbe1a385..240ab5fc9 100644 --- a/src/retargeters/SO101/clutch_retargeter.py +++ b/src/retargeters/SO101/clutch_retargeter.py @@ -13,9 +13,9 @@ :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 5D -action contract are untouched. The orientation is a passthrough of the controller grip -orientation and is dropped downstream (position-only IK). +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 @@ -50,9 +50,9 @@ 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 channel is a controller passthrough, - dropped by position-only IK). It is **never** ``(0, 0, 0)`` (that would command the EE into the - robot base / floor). + 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. """ @@ -86,6 +86,37 @@ _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( @@ -127,8 +158,10 @@ class SO101ClutchRetargeter(BaseRetargeter): 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 is a passthrough and dropped - downstream. + 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 / @@ -142,6 +175,8 @@ def __init__( 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. @@ -151,12 +186,31 @@ def __init__( 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 - (orientation is a controller passthrough, dropped by position-only IK). When - ``None``, falls back to :data:`_CLUTCH_HOME_EE_POS`. + 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: @@ -224,8 +278,14 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N np.float64 ) grip_ori = np.from_dlpack(inp[ControllerInputIndex.GRIP_ORIENTATION]).astype( - np.float32 + 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: @@ -241,9 +301,21 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N 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, grip_ori]).astype(np.float32) + self._last_pose = np.concatenate([pos, cmd_ori]).astype(np.float32) ee_pose[0] = self._last_pose diff --git a/src/retargeters/SO101/wrist_retargeter.py b/src/retargeters/SO101/wrist_retargeter.py deleted file mode 100644 index 1954042c4..000000000 --- a/src/retargeters/SO101/wrist_retargeter.py +++ /dev/null @@ -1,311 +0,0 @@ -# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 - -"""SO-101 wrist retargeter for the absolute-pose XR teleop pipeline. - -The SO-101's task-space IK is position-only (it cannot track a full 6-DOF pose with a 5-DOF -arm), so the controller's roll and pitch about its grip axis are otherwise lost. This module -recovers both as dedicated joint channels: :class:`SO101WristRetargeter` reads the XR controller -grip orientation and emits: - -* An **engage-relative** roll [rad] about the controller's own **local Z** axis - (:data:`_APPROACH_AXIS`), forwarded to the ``wrist_roll`` joint. -* An **absolute world-elevation** pitch [rad] of the controller's **aim ray** (the AIM pose - OpenXR provides, not the GRIP pose) above the horizontal plane - (:func:`_approach_elevation`), forwarded to the position+pitch IK pitch channel. - -Roll frame rationale: the roll is the swing-twist twist of the orientation delta *since engage* -(``R_engage^-1 . R_current``) about the grip's local Z axis (see :func:`_roll_twist`). Measuring -the delta about the grip's own axis -- rather than an absolute twist about a fixed world axis -- -isolates wrist roll from hand pitch/yaw: rotations about axes perpendicular to local Z fall into -the (discarded) swing component, so ordinary heading/wobble does not leak into the channel. An -earlier revision took the absolute twist about fixed world Z, which cross-coupled controller yaw -(and, at non-vertical grasps, pitch) into the roll with gain ~1 -- hence over-sensitive control. - -Pitch rationale: the pitch is taken from the controller's AIM ray (where the operator is -pointing), not the GRIP pose, because the ray elevation is the more intuitive handle for the -gripper's in-plane tilt. It is deliberately absolute (not engage-relative) -- the same ray -elevation always commands the same wrist tilt, regardless of when teleop engaged or from where -(e.g. pointing 45° down always tilts the gripper to 45°). Engage-relative pitch would require -the operator to mentally track the engage orientation, which is not natural. - -Like the clutch, the roll channel keeps a running baseline: a plain Stop freezes the last -commanded roll and the next Play resumes from it (re-latching the reference orientation, so no -jump); an explicit reset re-zeros it to :data:`_WRIST_ROLL_OFFSET_RAD`. The pitch channel holds -its last value on a dropped frame and resets to :data:`_PITCH_OFFSET_RAD`. - -The twist is computed by quaternion-projection swing-twist decomposition (see -:func:`_swing_twist_angle`), which stays well-conditioned near a 180-degree rotation where -Euler/rotvec extraction would be unstable. -""" - -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, - FloatType, -) - -# Single source of truth for the stringly-typed pipeline wiring. These mirror the gripper -# node's ``gripper_command`` / ``gripper_value`` convention so the group/connect key and the -# flattened element label / ``output_order`` label stay structurally consistent. -ROLL_COMMAND_KEY = "roll_command" -"""Group / ``connect`` / ``input_config`` key for the roll channel.""" -ROLL_ELEMENT_LABEL = "roll_value" -"""Flattened element label, used in the reorderer ``input_config`` value and ``output_order``.""" - -PITCH_COMMAND_KEY = "pitch_command" -"""Group / ``connect`` / ``input_config`` key for the pitch channel.""" -PITCH_ELEMENT_LABEL = "pitch_value" -"""Flattened element label, used in the reorderer ``input_config`` value and ``output_order``.""" - -# The controller's local roll axis: wrist roll is the rotation about the grip's own Z axis -# (the OpenXR controller local Z). The roll is the twist of the orientation delta since engage -# about this axis, so it is expressed in the grip body frame -- not a fixed world axis -- which -# isolates roll from hand pitch/yaw. TODO(verify-in-sim): confirm local Z is the operator's -# intuitive roll axis for the grip pose convention in use. -_APPROACH_AXIS = np.array([0.0, 0.0, 1.0], dtype=np.float64) -# Controller-twist -> wrist_roll handedness. PhysX bounds the joint at its hard USD limit; a -# guessed node-side clamp would mask sign/scale bugs during bring-up, so none is applied. -_ROLL_SIGN = 1.0 # TODO(verify-in-sim) -# Calibration zero [rad]. Flipped 180 deg so the engaged wrist_roll seed is pi rather than 0 -# (the gripper's neutral roll faces the opposite way from the SO-101 joint zero). TODO(tune-in-sim) -_WRIST_ROLL_OFFSET_RAD = np.pi - -# Local axis whose world elevation defines the commanded pitch. Pitch is read from the -# controller's AIM pose -- the pointer ray that OpenXR already provides -- not the GRIP pose, -# so no grip->ray offset is computed here. The SO-101 pitch is an ABSOLUTE world elevation (not -# engage-relative). Distinct from _APPROACH_AXIS (the grip-local +Z twist axis used for roll). -# TODO(verify-in-sim): confirm the sign for the aim pose convention (flip _PITCH_SIGN if needed). -_PITCH_APPROACH_AXIS = np.array([0.0, 0.0, 1.0], dtype=np.float64) -_PITCH_SIGN = 1.0 # TODO(verify-in-sim) -# TODO(tune-in-sim): absolute elevation that maps to the home tilt -_PITCH_OFFSET_RAD = 0.0 - - -def _swing_twist_angle(quat_xyzw: np.ndarray, axis: np.ndarray) -> float: - """Return the swing-twist twist angle [rad] of a quaternion about an axis. - - Uses quaternion-projection swing-twist decomposition: the twist component is the part of - the rotation about ``axis``, and its angle is ``2 * atan2(d, w)`` where ``d`` is the - projection of the vector part onto ``axis`` and ``w`` is the scalar part. This is robust - near a 180-degree rotation, unlike Euler/rotvec extraction. - - Args: - quat_xyzw: Unit quaternion in ``[x, y, z, w]`` (scipy) order, shape ``(4,)``. - axis: Unit twist axis in the quaternion's frame, shape ``(3,)``. - - Returns: - The twist angle [rad] in ``(-pi, pi]``; ``0.0`` when the twist is degenerate. - """ - q = np.asarray(quat_xyzw, dtype=np.float64) - # Hemisphere-canonicalize so the twist angle has a consistent sign. - if q[3] < 0.0: - q = -q - w = q[3] - d = float(np.dot(q[:3], axis)) - # Degeneracy guard: a pure 180-degree swing leaves no twist component to recover. - if w * w + d * d < 1e-12: - return 0.0 - return 2.0 * np.arctan2(d, w) - - -def _quat_conj(q: np.ndarray) -> np.ndarray: - """Conjugate (inverse, for a unit quaternion) of an ``[x, y, z, w]`` quaternion.""" - return np.array([-q[0], -q[1], -q[2], q[3]], dtype=np.float64) - - -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). - - Matches the SciPy ``Rotation`` composition convention: ``(Ra * Rb).as_quat()`` equals - ``_quat_mul(Ra.as_quat(), Rb.as_quat())``. - """ - 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 _roll_twist( - ref_quat_xyzw: np.ndarray, cur_quat_xyzw: np.ndarray, axis: np.ndarray -) -> float: - """Return the roll [rad] of ``cur`` relative to ``ref`` about a grip-local ``axis``. - - Computes the orientation delta since the reference (``ref^-1 . cur``, expressed in the - reference's body frame) and takes its swing-twist twist about ``axis`` (the grip local roll - axis). Rotations about axes perpendicular to ``axis`` fall into the discarded swing, so hand - pitch/yaw do not leak into the roll. With an identity ``ref`` this reduces to the absolute - swing-twist of ``cur`` about ``axis``. - - Args: - ref_quat_xyzw: Reference (engage) grip orientation, unit quaternion ``[x, y, z, w]``. - cur_quat_xyzw: Current grip orientation, unit quaternion ``[x, y, z, w]``. - axis: Grip-local roll axis, shape ``(3,)``. - - Returns: - The relative roll angle [rad] in ``(-pi, pi]``. - """ - ref = np.asarray(ref_quat_xyzw, dtype=np.float64) - cur = np.asarray(cur_quat_xyzw, dtype=np.float64) - delta = _quat_mul(_quat_conj(ref), cur) - return _swing_twist_angle(delta, axis) - - -def _approach_elevation(quat_xyzw: np.ndarray) -> float: - """Return the elevation [rad] of the pose approach axis above the world horizontal plane. - - Rotates :data:`_PITCH_APPROACH_AXIS` by the given orientation (the controller AIM pose) and - returns ``atan2(z, ||xy||)`` of the result -- the signed angle above horizontal, in - ``[-pi/2, pi/2]``. This is an absolute (not engage-relative) measure: the same orientation - always yields the same elevation, independent of azimuth. - - Args: - quat_xyzw: Unit AIM-pose quaternion in ``[x, y, z, w]`` (scipy) order, shape ``(4,)``. - - Returns: - The approach-axis elevation [rad] above world horizontal. - """ - q = np.asarray(quat_xyzw, dtype=np.float64) - a = _PITCH_APPROACH_AXIS - av = np.array([a[0], a[1], a[2], 0.0], dtype=np.float64) - rotated = _quat_mul(_quat_mul(q, av), _quat_conj(q))[:3] - horiz = float(np.hypot(rotated[0], rotated[1])) - return float(np.arctan2(rotated[2], horiz)) - - -class SO101WristRetargeter(BaseRetargeter): - """Retargets XR controller grip orientation to SO-101 wrist-roll and wrist-pitch commands. - - Reads the grip orientation of one controller and emits two channels: - - * **Roll** (engage-relative): the swing-twist twist [rad] of ``R_engage^-1 . R_current`` - about the grip's own local Z axis (:data:`_APPROACH_AXIS`, see :func:`_roll_twist`), - offset by :data:`_WRIST_ROLL_OFFSET_RAD` and signed by :data:`_ROLL_SIGN`. Measuring the - delta about the grip's own axis isolates wrist roll from hand pitch/yaw (those fall into - the discarded swing). Like the clutch, the channel keeps a running baseline: the reference - orientation is latched on the first RUNNING frame and re-armed whenever teleop is not - RUNNING, so each Play resumes from the last commanded roll without a jump. A reset - re-zeros the roll to the offset. - - * **Pitch** (absolute): the world elevation [rad] of the controller's AIM ray - (:func:`_approach_elevation`), signed by :data:`_PITCH_SIGN` and offset by - :data:`_PITCH_OFFSET_RAD`. Taken from the AIM pose (pointer), not the GRIP pose. The same - aim orientation always commands the same pitch, independent of when teleop engaged or the - azimuth of the controller. The last value is held on a dropped/aim-invalid frame, and - reset to the offset. - - Outputs floats under :data:`ROLL_COMMAND_KEY` / :data:`ROLL_ELEMENT_LABEL` and - :data:`PITCH_COMMAND_KEY` / :data:`PITCH_ELEMENT_LABEL`. - """ - - def __init__(self, name: str, input_device: str = ControllersSource.RIGHT) -> None: - """Initialize the wrist retargeter. - - Args: - name: Name identifier for this retargeter node. - input_device: Controller source key to read the grip orientation from. - """ - self._input_device = input_device - super().__init__(name=name) - # Reference grip orientation latched on engage (``[x, y, z, w]``), or ``None`` when - # re-armed (not yet engaged this episode / after a Stop or reset). - self._ref_quat: np.ndarray | None = None - # Running roll baseline [rad], latched on engage to the last commanded roll so a re-clutch - # resumes there. Re-zeroed to the offset on reset. - self._roll_baseline = _WRIST_ROLL_OFFSET_RAD - self._last_roll = _WRIST_ROLL_OFFSET_RAD - # Last commanded pitch [rad]; held on dropped frames and reset to offset. - self._last_pitch = _PITCH_OFFSET_RAD - - def input_spec(self) -> RetargeterIOType: - """Requires the grip orientation of the configured controller (Optional).""" - return {self._input_device: OptionalType(ControllerInput())} - - def output_spec(self) -> RetargeterIOType: - """Outputs engage-relative wrist-roll and absolute wrist-pitch [rad] under ``ROLL_COMMAND_KEY`` / ``PITCH_COMMAND_KEY``.""" - return { - ROLL_COMMAND_KEY: TensorGroupType( - ROLL_COMMAND_KEY, [FloatType(ROLL_ELEMENT_LABEL)] - ), - PITCH_COMMAND_KEY: TensorGroupType( - PITCH_COMMAND_KEY, [FloatType(PITCH_ELEMENT_LABEL)] - ), - } - - def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> None: - """Computes engage-relative roll and absolute pitch [rad]; holds last on a dropped frame.""" - running = context.execution_events.execution_state == ExecutionState.RUNNING - - if context.execution_events.reset: - # Fresh episode: re-arm the reference, re-zero roll to the offset, reset pitch. - self._ref_quat = None - self._last_roll = _WRIST_ROLL_OFFSET_RAD - self._last_pitch = _PITCH_OFFSET_RAD - elif not running: - # A plain Stop re-arms the reference but FREEZES the running roll, so the next Play - # resumes from the last commanded value. - self._ref_quat = None - - roll_out = outputs[ROLL_COMMAND_KEY] - pitch_out = outputs[PITCH_COMMAND_KEY] - inp = inputs[self._input_device] - - if inp.is_none: - # Dropped frame: hold the last commanded values for both channels. - roll_out[0] = self._last_roll - pitch_out[0] = self._last_pitch - return - - quat_xyzw = np.from_dlpack(inp[ControllerInputIndex.GRIP_ORIENTATION]).astype( - np.float64 - ) # XYZW - - if self._ref_quat is None: - if not running: - # Connected but not playing yet: hold the last values and do not latch the reference. - roll_out[0] = self._last_roll - pitch_out[0] = self._last_pitch - return - # First RUNNING frame (Play just started): latch the reference orientation and the - # running baseline (the last commanded roll) so the channel resumes without a jump. - self._ref_quat = quat_xyzw.copy() - self._roll_baseline = self._last_roll - - self._last_roll = self._roll_baseline + _ROLL_SIGN * _roll_twist( - self._ref_quat, quat_xyzw, _APPROACH_AXIS - ) - roll_out[0] = self._last_roll - - # Pitch is computed from the AIM pose (pointer ray), not the GRIP pose. Hold the last - # pitch if the aim pose is not tracked this frame. - if bool(inp[ControllerInputIndex.AIM_IS_VALID]): - aim_quat_xyzw = np.from_dlpack( - inp[ControllerInputIndex.AIM_ORIENTATION] - ).astype(np.float64) # XYZW - self._last_pitch = ( - _PITCH_SIGN * _approach_elevation(aim_quat_xyzw) + _PITCH_OFFSET_RAD - ) - pitch_out[0] = self._last_pitch - - -# Back-compat alias: the class was formerly named SO101RollRetargeter. -SO101RollRetargeter = SO101WristRetargeter diff --git a/src/retargeters/__init__.py b/src/retargeters/__init__.py index 317d5c221..c485e69f2 100644 --- a/src/retargeters/__init__.py +++ b/src/retargeters/__init__.py @@ -17,8 +17,6 @@ - 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 - - SO101WristRetargeter: Engage-relative wrist roll + absolute pitch for the SO-101 5-DOF arm - - SO101RollRetargeter: Back-compat alias for SO101WristRetargeter - SO101GripperRetargeter: Proportional (analog) jaw closedness for the SO-101 gripper - SharpaHandRetargeter: Pinocchio/Pink IK-based retargeting for Sharpa hand - SharpaBiManualRetargeter: Bimanual version of SharpaHandRetargeter @@ -102,14 +100,12 @@ # .gripper_retargeter "GripperRetargeter": (".gripper_retargeter", "GripperRetargeter", None), "GripperRetargeterConfig": (".gripper_retargeter", "GripperRetargeterConfig", None), - # .SO101 (SO-101 5-DOF arm: clutch EE pose, wrist roll+pitch, analog gripper) + # .SO101 (SO-101 5-DOF arm: full-pose clutch EE pose, analog gripper) "SO101ClutchRetargeter": ( ".SO101.clutch_retargeter", "SO101ClutchRetargeter", None, ), - "SO101WristRetargeter": (".SO101.wrist_retargeter", "SO101WristRetargeter", None), - "SO101RollRetargeter": (".SO101.wrist_retargeter", "SO101RollRetargeter", None), "SO101GripperRetargeter": ( ".SO101.gripper_retargeter", "SO101GripperRetargeter", @@ -208,8 +204,6 @@ def __getattr__(name: str): "GripperRetargeterConfig", # SO-101 5-DOF arm retargeters "SO101ClutchRetargeter", - "SO101WristRetargeter", - "SO101RollRetargeter", "SO101GripperRetargeter", "Se3AbsRetargeter", "Se3RelRetargeter",