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() 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"] ) 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