diff --git a/dimos/teleop/hosted/arm_command.py b/dimos/teleop/hosted/arm_command.py index a947fcd3e4..e460557c83 100644 --- a/dimos/teleop/hosted/arm_command.py +++ b/dimos/teleop/hosted/arm_command.py @@ -45,6 +45,7 @@ class ArmCommandConfig(QuestTeleopConfig): cmd_stale_after_sec: float = 0.5 + enable_ui_scaling: bool = False class ArmCommandModule(ArmTeleopModule): @@ -166,9 +167,15 @@ def _on_twist_bytes(self, data: bytes) -> None: if ts <= self._last_twist_ts: # out-of-order return self._last_twist_ts = ts + with self._lock: + scale = self._translation_scale self.ee_twist_command.publish( TwistStamped( - linear=[msg.linear.x, msg.linear.y, msg.linear.z], + linear=[ + msg.linear.x * scale, + msg.linear.y * scale, + msg.linear.z * scale, + ], angular=[msg.angular.x, msg.angular.y, msg.angular.z], ts=msg.ts, ) @@ -193,6 +200,8 @@ def _on_state_json(self, data: Any) -> None: self._handle_estop_clear(msg.get("nonce")) elif kind == "operator_lost": # synthetic, injected by the provider self._on_operator_lost() + elif kind == "teleop_scale": + self._handle_teleop_scale(msg) elif kind == "gripper" and not self._estopped: self.gripper_command.publish(Bool(data=bool(msg.get("closed", False)))) @@ -202,6 +211,19 @@ def _send_ack(self, nonce: Any, ok: bool) -> None: except Exception: logger.warning("cmd_ack publish failed", exc_info=True) + def _handle_teleop_scale(self, msg: dict[str, Any]) -> None: + """Apply an opt-in UI scale to pose deltas and keyboard twists.""" + nonce = msg.get("nonce") + if not self.config.enable_ui_scaling: + self._send_ack(nonce, False) + return + try: + self._set_translation_scale(float(msg["scale"])) + except (KeyError, TypeError, ValueError): + self._send_ack(nonce, False) + return + self._send_ack(nonce, True) + # ─── E-STOP gating over the inherited control loop ──────────────── def _handle_engage(self) -> None: @@ -265,6 +287,7 @@ def _publish_robot_state(self) -> None: "left": self._is_engaged[Hand.LEFT], "right": self._is_engaged[Hand.RIGHT], }, + "teleop_scale": self._translation_scale, } try: self.robot_state.publish(json.dumps(state).encode()) diff --git a/dimos/teleop/hosted/test_arm_command.py b/dimos/teleop/hosted/test_arm_command.py index 553895de31..14b71c4070 100644 --- a/dimos/teleop/hosted/test_arm_command.py +++ b/dimos/teleop/hosted/test_arm_command.py @@ -51,6 +51,7 @@ def _fake_init(self: Any, **kwargs: Any) -> None: self.config = SimpleNamespace( control_loop_hz=50.0, cmd_stale_after_sec=0.5, + enable_ui_scaling=False, ) monkeypatch.setattr(Module, "__init__", _fake_init) @@ -75,10 +76,12 @@ def _pose_bytes(frame_id: str, ts: float | None = None) -> bytes: return PoseStamped(ts=time.time() if ts is None else ts, frame_id=frame_id).lcm_encode() -def _twist_bytes(x: float = 0.1, ts: float | None = None) -> bytes: +def _twist_bytes(x: float = 0.1, angular_x: float = 0.0, ts: float | None = None) -> bytes: # ts=None keeps TwistStamped's default stamp (now) — a fresh command. kwargs = {} if ts is None else {"ts": ts} - return TwistStamped(frame_id="eef_twist_arm", linear=[x, 0.0, 0.0], **kwargs).lcm_encode() + return TwistStamped( + frame_id="eef_twist_arm", linear=[x, 0.0, 0.0], angular=[angular_x, 0.0, 0.0], **kwargs + ).lcm_encode() def _tick(module: ArmCommandModule) -> None: @@ -162,6 +165,25 @@ def test_twist_republished_without_task_address(module: ArmCommandModule) -> Non assert out.linear.x == pytest.approx(0.2) +def test_ui_scale_disabled_is_rejected(module: ArmCommandModule) -> None: + module._on_state_json(b'{"type": "teleop_scale", "scale": 0.5, "nonce": 3}') + + assert _sent_acks(module) == [{"type": "cmd_ack", "nonce": 3, "ok": False}] + assert module._translation_scale == 1.0 + + +def test_ui_scale_updates_pose_and_keyboard_twist(module: ArmCommandModule) -> None: + module.config.enable_ui_scaling = True + module._on_state_json(b'{"type": "teleop_scale", "scale": 0.5, "nonce": 4}') + module._on_cmd_raw(_twist_bytes(0.2, angular_x=0.3)) + + assert _sent_acks(module) == [{"type": "cmd_ack", "nonce": 4, "ok": True}] + assert module._translation_scale == 0.5 + out = module.ee_twist_command.publish.call_args.args[0] + assert out.linear.x == pytest.approx(0.1) + assert out.angular.x == pytest.approx(0.3) + + def test_twist_dropped_while_estopped(module: ArmCommandModule) -> None: module._estopped = True module._on_cmd_raw(_twist_bytes(0.2)) diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index 1fcc055ae5..389d25eca9 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -23,6 +23,7 @@ import asyncio from dataclasses import dataclass +import math from pathlib import Path import threading import time @@ -105,6 +106,7 @@ def __init__(self, **kwargs: Any) -> None: Hand.RIGHT: None, } self._lock = threading.RLock() + self._translation_scale = 1.0 # Control loop self._control_loop_thread: threading.Thread | None = None @@ -365,12 +367,19 @@ def _get_output_pose(self, hand: Hand) -> PoseStamped | None: delta = current_pose - initial_pose return PoseStamped( - position=delta.position, + position=delta.position * self._translation_scale, orientation=delta.orientation, ts=current_pose.ts, frame_id=current_pose.frame_id, ) + def _set_translation_scale(self, translation_scale: float) -> None: + """Set the positive multiplier applied to controller position deltas.""" + if not math.isfinite(translation_scale) or translation_scale <= 0.0: + raise ValueError("translation_scale must be finite and positive") + with self._lock: + self._translation_scale = translation_scale + def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: """Publish message for a controller. diff --git a/dimos/teleop/quest/test_quest_teleop_module.py b/dimos/teleop/quest/test_quest_teleop_module.py index 478b4306f2..dda35c7786 100644 --- a/dimos/teleop/quest/test_quest_teleop_module.py +++ b/dimos/teleop/quest/test_quest_teleop_module.py @@ -16,6 +16,7 @@ import pytest +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.teleop.quest.quest_extensions import HandTeleopModule from dimos.teleop.quest.quest_teleop_module import QuestTeleopModule from dimos.teleop.quest.quest_types import Hand, QuestControllerState @@ -44,6 +45,29 @@ def test_quest_web_server_is_initialized_during_start(module: QuestTeleopModule, start_control_loop.assert_called_once_with() +def test_translation_scale_changes_pose_delta(module: QuestTeleopModule) -> None: + module._initial_poses[Hand.RIGHT] = PoseStamped(position=[1.0, 2.0, 3.0]) + module._current_poses[Hand.RIGHT] = PoseStamped(position=[1.2, 1.5, 4.0]) + + module._set_translation_scale(2.0) + + output = module._get_output_pose(Hand.RIGHT) + assert output is not None + assert output.position.x == pytest.approx(0.4) + assert output.position.y == pytest.approx(-1.0) + assert output.position.z == pytest.approx(2.0) + + +@pytest.mark.parametrize("translation_scale", [0.0, -1.0, float("inf")]) +def test_translation_scale_must_be_positive_and_finite( + module: QuestTeleopModule, translation_scale: float +) -> None: + with pytest.raises(ValueError): + module._set_translation_scale(translation_scale) + + assert module._translation_scale == 1.0 + + def test_hand_teleop_pinch_toggles_engagement(mocker) -> None: module = HandTeleopModule() try: