From c19bc91b895f60b62ad60a36f0c10101d3105545 Mon Sep 17 00:00:00 2001 From: shared-twolabs Date: Wed, 27 May 2026 20:13:07 -0700 Subject: [PATCH 1/3] lima-1: VRFrame.motion_scale field, default 1.0 (additive, backwards compatible) --- almond_axol/vr/models.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/almond_axol/vr/models.py b/almond_axol/vr/models.py index b8f9a47..2053056 100644 --- a/almond_axol/vr/models.py +++ b/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 From 1623d1e0dd5d56653f86750f0b73e679e3e7a214 Mon Sep 17 00:00:00 2001 From: shared-twolabs Date: Wed, 27 May 2026 20:22:58 -0700 Subject: [PATCH 2/3] =?UTF-8?q?lima-1:=20clutch-and-scale=20in=20teleop=20?= =?UTF-8?q?worker=20=E2=80=94=20scale=20VR=20motion=20on=20grip-rising-edg?= =?UTF-8?q?e?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds an optional motion_scale parameter to _relative_target_np that multiplies the controller-frame position delta. Default 1.0 preserves prior identity mapping. Values <1.0 magnify VR→arm motion so the operator can drive the full arm workspace while their controllers stay inside the headset's tracking volume (compensates for tracking drift at extreme reach). IKWorker.step() resolves an effective scale per frame: client-sent frame.motion_scale wins unless it's exactly 1.0, in which case the new VRTeleopConfig.motion_scale CLI fallback applies. Position scaling is applied uniformly to both EE delta and elbow delta so their targets stay self-consistent for IK posture. Orientation is intentionally NOT scaled — rotation passes through 1:1 so wrist twist always matches. The existing clutch (rising-edge grip → engage_snap captures VR pose and arm FK) is already a clutch; this change scales the delta drawn from those snaps. --- almond_axol/teleop/config.py | 9 +++++++++ almond_axol/teleop/worker.py | 38 ++++++++++++++++++++++++++++++++---- 2 files changed, 43 insertions(+), 4 deletions(-) diff --git a/almond_axol/teleop/config.py b/almond_axol/teleop/config.py index a677269..500c265 100644 --- a/almond_axol/teleop/config.py +++ b/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 diff --git a/almond_axol/teleop/worker.py b/almond_axol/teleop/worker.py index 842288f..b43b5b1 100644 --- a/almond_axol/teleop/worker.py +++ b/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"] ) From fb421fb055032cc06c9eb0b7695e66051333d87c Mon Sep 17 00:00:00 2001 From: shared-twolabs Date: Wed, 27 May 2026 20:23:38 -0700 Subject: [PATCH 3/3] lima-1: --motion-scale CLI flag for axol teleop MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adds a --motion-scale FLOAT flag (default 1.0) to the axol teleop subcommand. Plumbs it into VRTeleopConfig.motion_scale, which the IK worker uses as a server-side fallback when the incoming VRFrame leaves motion_scale at its 1.0 default. Lets operators run with a magnified VR→arm mapping without a client-side change. --- almond_axol/cli/teleop.py | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/almond_axol/cli/teleop.py b/almond_axol/cli/teleop.py index 6b40ede..c8ff24b 100644 --- a/almond_axol/cli/teleop.py +++ b/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()