feat(teleop): motion_scale on VRFrame + clutch-and-scale in worker + --motion-scale CLI flag

almond_axol/cli/teleop.py

@@ -66,6 +66,21 @@ def add_parser(subparsers: argparse._SubParsersAction) -> None:  # type: ignore[
         metavar=stiffness_metavar,
         help=stiffness_help.format(side="right", attr="right_stiffness"),
     )
+    p.add_argument(
+        "--motion-scale",
+        type=float,
+        default=1.0,
+        metavar="S",
+        help=(
+            "Server-side fallback for the VR→arm position multiplier (default: 1.0). "
+            "1.0 is identity (VR motion == arm motion). Values <1.0 magnify "
+            "VR motion (small controller move → larger arm move), useful when "
+            "the operator's controllers drift toward the edge of headset "
+            "tracking. Applied only when the incoming VRFrame leaves "
+            "motion_scale at its 1.0 default — a client that sends an "
+            "explicit motion_scale always wins. Orientation is never scaled."
+        ),
+    )
     p.add_argument(
         "--log-level",
         default="INFO",
@@ -99,6 +114,7 @@ def run(args: argparse.Namespace) -> None:
             right_gripper_torque_limit=args.right_gripper_torque_limit,
             left_stiffness=args.left_stiffness,
             right_stiffness=args.right_stiffness,
+            motion_scale=args.motion_scale,
         )
     )
 
@@ -112,10 +128,12 @@ async def _run(
     right_gripper_torque_limit: float = 0.5,
     left_stiffness: float | tuple[float, ...] = 0.5,
     right_stiffness: float | tuple[float, ...] = 0.5,
+    motion_scale: float = 1.0,
 ) -> None:
     from ..robot import Axol, Sim
     from ..robot.config import AxolConfig
     from ..teleop import VRTeleop
+    from ..teleop.config import VRTeleopConfig
 
     if robot_type == "sim":
         robot = Sim()
@@ -132,5 +150,6 @@ async def _run(
         axol_config.left.gripper.torque_limit = left_gripper_torque_limit
         axol_config.right.gripper.torque_limit = right_gripper_torque_limit
         robot = Axol(config=axol_config, **kwargs)
-    async with VRTeleop(robot) as teleop:
+    teleop_config = VRTeleopConfig(motion_scale=motion_scale)
+    async with VRTeleop(robot, config=teleop_config) as teleop:
         await teleop.run()

almond_axol/teleop/config.py

@@ -72,6 +72,14 @@ class VRTeleopConfig:
             keeping the filter transparent during fast intentional moves.  For
             meter-space positions at 120 Hz a value of ~20 works well; increase
             if fast moves feel sticky.  Defaults to ``20.0``.
+        motion_scale: Server-side fallback for the per-frame teleop position
+            multiplier. Used when a VRFrame arrives with ``motion_scale`` set
+            to its identity default ``1.0`` (i.e. a client unaware of the
+            field). Lets ``axol teleop --motion-scale 0.5`` drive scaled
+            behaviour without a client-side change. ``1.0`` (default)
+            preserves prior identity mapping (VR motion → equal arm motion).
+            Values ``< 1.0`` magnify VR→arm motion. Orientation is never
+            scaled.
     """
 
     rest_pose_left: np.ndarray = field(
@@ -117,3 +125,4 @@ class VRTeleopConfig:
     ik_alpha: float = 0.5
     pose_min_cutoff: float = 1.5
     pose_beta: float = 5.0
+    motion_scale: float = 1.0

almond_axol/teleop/worker.py

@@ -69,9 +69,19 @@ def _relative_target_np(
     rot_snap_ctrl: np.ndarray,
     pos_snap_fk: np.ndarray,
     rot_snap_fk: np.ndarray,
+    motion_scale: float = 1.0,
 ) -> tuple[np.ndarray, np.ndarray]:
-    """Compute absolute EE target from controller delta. Returns (pos_3, rot_3x3)."""
-    d = rot_snap_ctrl.T @ (pos_curr - pos_snap_ctrl)
+    """Compute absolute EE target from controller delta. Returns (pos_3, rot_3x3).
+
+    ``motion_scale`` multiplies the position delta in the controller's local
+    frame BEFORE it is mapped into the arm's FK frame, so a single scalar
+    setting works regardless of controller orientation at engage time. The
+    default ``1.0`` preserves prior identity behaviour. Values ``< 1.0``
+    magnify VR motion → arm motion (small controller move → larger arm
+    move). Orientation is intentionally not scaled — rotation passes through
+    1:1 so the operator's wrist twist always matches the arm wrist twist.
+    """
+    d = motion_scale * (rot_snap_ctrl.T @ (pos_curr - pos_snap_ctrl))
     new_t = (
         pos_snap_fk
         + rot_snap_fk[:, 0] * d[2]
@@ -111,6 +121,11 @@ def __init__(
             kinematics_config: IK solver cost weights forwarded to :class:`KinematicsSolver`.
         """
         self._config = config
+        # Server-side fallback scale used when an incoming VRFrame's
+        # ``motion_scale`` field is exactly ``1.0`` (i.e. the client did not
+        # send an explicit scale). Lets ``axol teleop --motion-scale`` work
+        # without a client-side change.
+        self._cli_motion_scale = float(getattr(config, "motion_scale", 1.0))
         self._solver = KinematicsSolver(kinematics_config)
 
         self._rest_pose_left = np.asarray(config.rest_pose_left, dtype=np.float32)
@@ -243,22 +258,37 @@ def step(self, frame: VRFrame, q_current: np.ndarray) -> np.ndarray:
             )
             return q_current
 
+        # Resolve per-frame motion scale. Client value wins unless it's
+        # exactly 1.0 (the default an unaware client sends), in which case
+        # the CLI fallback applies. Position is scaled; orientation is not.
+        # Elbow delta is scaled the same as EE delta so EE and elbow targets
+        # stay self-consistent (otherwise a magnified EE move with an
+        # unscaled elbow target produces poor IK posture).
+        client_scale = float(frame.motion_scale)
+        effective_scale = (
+            client_scale if client_scale != 1.0 else self._cli_motion_scale
+        )
+
         # Relative targets — pure numpy
         tl_pos, tl_rot = _relative_target_np(
             left_pos,
             left_rot,
             *self._snap_ctrl["left"],
             *self._snap_fk["left"],
+            motion_scale=effective_scale,
         )
         tr_pos, tr_rot = _relative_target_np(
             right_pos,
             right_rot,
             *self._snap_ctrl["right"],
             *self._snap_fk["right"],
+            motion_scale=effective_scale,
         )
 
-        elbow_l = self._snap_elbow_fk["left"] + (left_e - self._snap_elbow_ctrl["left"])
-        elbow_r = self._snap_elbow_fk["right"] + (
+        elbow_l = self._snap_elbow_fk["left"] + effective_scale * (
+            left_e - self._snap_elbow_ctrl["left"]
+        )
+        elbow_r = self._snap_elbow_fk["right"] + effective_scale * (
             right_e - self._snap_elbow_ctrl["right"]
         )
 

almond_axol/vr/models.py

@@ -71,6 +71,15 @@ class VRFrame(BaseModel):
         r_lock:  Right grip button state (True = pressed). See l_lock.
         reset:   Rising edge (False → True) triggers a reset to rest pose.
         state:   Current teleoperation session state (data_collection / teleop / recording).
+        motion_scale: Per-frame teleop position multiplier applied to the VR
+            controller delta after the engage-snap. ``1.0`` (default) is the
+            identity mapping (VR motion → equal arm motion, prior behaviour).
+            Values ``< 1.0`` magnify VR→arm motion (small controller move →
+            larger arm move) so the operator can drive the full arm workspace
+            while their hands stay inside the headset's tracking volume.
+            Only POSITION is scaled; orientation passes through 1:1 to avoid
+            confusing rotational behaviour. Optional — backwards compatible
+            with clients that don't send the field.
     """
 
     l_ee: VRPose
@@ -83,3 +92,4 @@ class VRFrame(BaseModel):
     r_lock: bool = False
     reset: bool = False
     state: VRState = VRState.TELEOP
+    motion_scale: float = 1.0