Expose SO-101 clutch position scale as a constructor parameter

src/core/retargeting_engine_tests/python/test_so101_retargeters.py

@@ -519,3 +519,71 @@ def test_invalid_grip_pose_holds_last_without_latching(self):
         r.compute(inputs2, outputs2, _make_context())
         np.testing.assert_allclose(_read_pose(outputs2)[:3], home, atol=1e-6)
         assert r._origin is not None
+
+    def test_position_scale_scales_delta(self):
+        """A non-unity ``position_scale`` scales the clutch delta without teleporting on engage."""
+        scale = 0.5
+        r = SO101ClutchRetargeter(name="ee_pose", position_scale=scale)
+        home = np.array(_CLUTCH_HOME_EE_POS)
+        p0 = np.array([0.5, -0.3, 0.7])
+
+        # Engage frame: p_ctrl == origin, so the EE sits exactly at home even at non-unity scale.
+        inputs, outputs = _build_io(r)
+        inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0)
+        r.compute(inputs, outputs, _make_context())
+        np.testing.assert_allclose(_read_pose(outputs)[:3], home, atol=1e-6)
+
+        # A controller delta after engage advances the EE by scale * delta from home.
+        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())
+        np.testing.assert_allclose(
+            _read_pose(outputs2)[:3], home + scale * delta, atol=1e-5
+        )
+
+    @pytest.mark.parametrize("bad_scale", [0.0, -1.0, float("nan"), float("inf")])
+    def test_invalid_position_scale_rejected(self, bad_scale):
+        """A non-positive or non-finite ``position_scale`` is rejected at construction."""
+        with pytest.raises(ValueError):
+            SO101ClutchRetargeter(name="ee_pose", position_scale=bad_scale)
+
+    def test_reengage_ratchets_scaled_delta_per_cycle(self):
+        """N re-clutch cycles accumulate ``scale * delta`` each, ratcheting by ``N * scale * delta``.
+
+        This is the issue's motivating regression: at 1:1 motion (``scale == 1``) each re-engage
+        ratchets a full delta onto the running home, driving the target out of reach; a sub-unity
+        scale attenuates each cycle's advance so the same operator motion stays in the workspace.
+        """
+        home = np.array(_CLUTCH_HOME_EE_POS)
+        delta = np.array([0.05, -0.02, 0.10])
+        n_cycles = 3
+
+        def _run_cycles(scale):
+            r = SO101ClutchRetargeter(name="ee_pose", position_scale=scale)
+            outputs = None
+            for i in range(n_cycles):
+                p0 = np.array(
+                    [float(i), float(i), float(i)]
+                )  # fresh origin each engage
+                # Engage: latch the origin at p0 and resume from the running home.
+                inputs, outputs = _build_io(r)
+                inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0)
+                r.compute(inputs, outputs, _make_context())
+                # Move +delta: the running home advances by scale * delta.
+                inputs, outputs = _build_io(r)
+                inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0 + delta)
+                r.compute(inputs, outputs, _make_context())
+                # Disengage (STOPPED): freeze the running home, re-arm the origin.
+                inputs, outputs = _build_io(r)
+                inputs[ControllersSource.RIGHT] = _make_controller(grip_pos=p0)
+                r.compute(inputs, outputs, _make_context(state=ExecutionState.STOPPED))
+            return _read_pose(outputs)[:3]
+
+        # Sub-unity scale ratchets N * scale * delta over the N cycles.
+        scale = 0.5
+        np.testing.assert_allclose(
+            _run_cycles(scale), home + n_cycles * scale * delta, atol=1e-5
+        )
+        # Unit-scale contrast: each cycle ratchets a full delta -> N * delta.
+        np.testing.assert_allclose(_run_cycles(1.0), home + n_cycles * delta, atol=1e-5)

src/retargeters/SO101/clutch_retargeter.py

@@ -44,7 +44,8 @@
   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
+  ``s = position_scale`` (the constructor parameter, defaulting to
+  :data:`_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
@@ -84,7 +85,8 @@
 # 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)
+# Default controller-to-EE translation gain (1.0 = 1:1 motion) for the ``position_scale``
+# constructor parameter. 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)
@@ -183,6 +185,7 @@ def __init__(
         name: str,
         input_device: str = ControllersSource.RIGHT,
         home_base_T_ee: np.ndarray | None = None,
+        position_scale: float = _CLUTCH_POSITION_SCALE,
         orientation_offset: np.ndarray | None = None,
         debug_log_engage: bool = False,
     ) -> None:
@@ -198,6 +201,16 @@ def __init__(
                 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`.
+            position_scale: Dimensionless controller-to-EE translation gain applied to the clutch
+                delta: ``output_pos = home + position_scale * (p_ctrl - p0)``. ``1.0`` is 1:1
+                motion. A value ``< 1`` shrinks robot travel relative to hand travel: the SO-101's
+                ~0.35 m reach is roughly half a comfortable ~0.7 m arm sweep, so 1:1 motion plus
+                clutch re-engage cycles can ratchet the commanded target out of reach -- e.g.
+                ``0.5`` keeps a full operator sweep inside reach in a single engaged segment.
+                Applies to **translation only**; the orientation is always 1:1. A value ``> 1``
+                amplifies the commanded EE velocity and jitter toward the joint-rate limits (no
+                upper clamp is enforced). Must be positive and finite. Defaults to
+                :data:`_CLUTCH_POSITION_SCALE` (1:1 motion).
             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
@@ -219,6 +232,15 @@ def __init__(
                 orientation_offset, dtype=np.float64
             ).copy()
         self._debug_log_engage = bool(debug_log_engage)
+        # Controller-to-EE translation gain applied to the clutch delta. Strictly positive and
+        # finite: scale == 0 freezes the EE, scale < 0 inverts the hand->EE mapping, and the finite
+        # check closes the ``inf * 0 = nan`` path before it can reach the SE3 IK.
+        position_scale = float(position_scale)
+        if not np.isfinite(position_scale) or position_scale <= 0.0:
+            raise ValueError(
+                f"position_scale must be positive and finite, got: {position_scale!r}"
+            )
+        self._position_scale = position_scale
         # 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:
@@ -340,7 +362,7 @@ def _compute_fn(self, inputs: RetargeterIO, outputs: RetargeterIO, context) -> N
                 )
 
         pos = _rebased_position(
-            grip_pos, self._origin, self._home, _CLUTCH_POSITION_SCALE
+            grip_pos, self._origin, self._home, self._position_scale
         )
         self._last_pose = np.concatenate([pos, cmd_ori]).astype(np.float32)
         ee_pose[0] = self._last_pose