From f1848a27935987ee269717bea06ed0221ea21657 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Sat, 8 Aug 2026 12:57:36 -0700 Subject: [PATCH 1/7] feat(teleop): add translation scaling --- dimos/teleop/hosted/test_arm_command.py | 29 ++++++++++++++++++++++--- dimos/teleop/quest/quest_extensions.py | 25 ++++++++++++++++++++- 2 files changed, 50 insertions(+), 4 deletions(-) diff --git a/dimos/teleop/hosted/test_arm_command.py b/dimos/teleop/hosted/test_arm_command.py index 8d0aabe865..0c4007588b 100644 --- a/dimos/teleop/hosted/test_arm_command.py +++ b/dimos/teleop/hosted/test_arm_command.py @@ -26,17 +26,17 @@ from collections.abc import Iterator import json import time -from types import SimpleNamespace from typing import Any from unittest.mock import MagicMock, patch +from pydantic import ValidationError import pytest from dimos.core.module import Module from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.TwistStamped import TwistStamped from dimos.robot.manipulators.common.topics import EEF_TWIST_TASK_NAME -from dimos.teleop.hosted.arm_command import ArmCommandModule +from dimos.teleop.hosted.arm_command import ArmCommandConfig, ArmCommandModule from dimos.teleop.quest.quest_types import Hand, QuestControllerState from dimos.utils.testing.waiting import wait_until @@ -50,7 +50,7 @@ def module(monkeypatch: pytest.MonkeyPatch) -> Iterator[ArmCommandModule]: reads ``config.task_names`` while constructing.""" def _fake_init(self: Any, **kwargs: Any) -> None: - self.config = SimpleNamespace( + self.config = ArmCommandConfig( task_names={"right": "teleop_xarm"}, control_loop_hz=50.0, cmd_stale_after_sec=0.5, @@ -235,6 +235,29 @@ def test_release_disengages(module: ArmCommandModule) -> None: assert not module._is_engaged[Hand.RIGHT] +def test_translation_scale_changes_pose_delta(module: ArmCommandModule) -> 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: ArmCommandModule, translation_scale: float +) -> None: + with pytest.raises(ValidationError): + module.set_translation_scale(translation_scale) + + assert module.config.translation_scale == 1.0 + + # ─── E-STOP latch ────────────────────────────────────────────────────── diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index 2af66b1693..f90b066d2e 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -26,7 +26,7 @@ from typing import Any from fastapi import WebSocket -from pydantic import Field +from pydantic import Field, FiniteFloat from dimos.core.core import rpc from dimos.core.stream import In, Out @@ -132,9 +132,12 @@ class ArmTeleopConfig(QuestTeleopConfig): task_names: Mapping of Hand -> coordinator task name. Used to set frame_id on output PoseStamped so the coordinator routes each hand's commands to the correct TeleopIKTask. + translation_scale: Positive multiplier applied to controller position + deltas before they are sent to the coordinator. """ task_names: dict[str, str] = Field(default_factory=dict) + translation_scale: FiniteFloat = Field(default=1.0, gt=0.0) class ArmTeleopModule(QuestTeleopModule): @@ -170,6 +173,26 @@ def __init__(self, **kwargs: Any) -> None: Hand[k.upper()]: v for k, v in self.config.task_names.items() } + @rpc + def set_translation_scale(self, translation_scale: float) -> None: + """Set the positive multiplier applied to controller position deltas.""" + with self._lock: + self.config = type(self.config).model_validate( + {**self.config.model_dump(), "translation_scale": translation_scale} + ) + + def _get_output_pose(self, hand: Hand) -> PoseStamped | None: + """Return the controller delta with its translation scaled.""" + output_pose = super()._get_output_pose(hand) + if output_pose is None: + return None + return PoseStamped( + position=output_pose.position * self.config.translation_scale, + orientation=output_pose.orientation, + ts=output_pose.ts, + frame_id=output_pose.frame_id, + ) + def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: """Stamp frame_id with task name and publish.""" task_name = self._task_names.get(hand) From 38b1ab841e9c63ac829157f743089a3d1f14e217 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Sat, 8 Aug 2026 13:09:19 -0700 Subject: [PATCH 2/7] refactor(teleop): centralize translation scaling --- dimos/teleop/quest/quest_extensions.py | 25 +------------------ dimos/teleop/quest/quest_teleop_module.py | 12 ++++++++- .../teleop/quest/test_quest_teleop_module.py | 25 +++++++++++++++++++ 3 files changed, 37 insertions(+), 25 deletions(-) diff --git a/dimos/teleop/quest/quest_extensions.py b/dimos/teleop/quest/quest_extensions.py index f90b066d2e..2af66b1693 100644 --- a/dimos/teleop/quest/quest_extensions.py +++ b/dimos/teleop/quest/quest_extensions.py @@ -26,7 +26,7 @@ from typing import Any from fastapi import WebSocket -from pydantic import Field, FiniteFloat +from pydantic import Field from dimos.core.core import rpc from dimos.core.stream import In, Out @@ -132,12 +132,9 @@ class ArmTeleopConfig(QuestTeleopConfig): task_names: Mapping of Hand -> coordinator task name. Used to set frame_id on output PoseStamped so the coordinator routes each hand's commands to the correct TeleopIKTask. - translation_scale: Positive multiplier applied to controller position - deltas before they are sent to the coordinator. """ task_names: dict[str, str] = Field(default_factory=dict) - translation_scale: FiniteFloat = Field(default=1.0, gt=0.0) class ArmTeleopModule(QuestTeleopModule): @@ -173,26 +170,6 @@ def __init__(self, **kwargs: Any) -> None: Hand[k.upper()]: v for k, v in self.config.task_names.items() } - @rpc - def set_translation_scale(self, translation_scale: float) -> None: - """Set the positive multiplier applied to controller position deltas.""" - with self._lock: - self.config = type(self.config).model_validate( - {**self.config.model_dump(), "translation_scale": translation_scale} - ) - - def _get_output_pose(self, hand: Hand) -> PoseStamped | None: - """Return the controller delta with its translation scaled.""" - output_pose = super()._get_output_pose(hand) - if output_pose is None: - return None - return PoseStamped( - position=output_pose.position * self.config.translation_scale, - orientation=output_pose.orientation, - ts=output_pose.ts, - frame_id=output_pose.frame_id, - ) - def _publish_msg(self, hand: Hand, output_msg: PoseStamped) -> None: """Stamp frame_id with task name and publish.""" task_name = self._task_names.get(hand) diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index 1fcc055ae5..3795356431 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -33,6 +33,7 @@ from fastapi import WebSocket, WebSocketDisconnect from fastapi.responses import HTMLResponse from fastapi.staticfiles import StaticFiles +from pydantic import Field, FiniteFloat from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core.core import rpc @@ -68,6 +69,7 @@ class QuestTeleopConfig(ModuleConfig): control_loop_hz: float = 50.0 server_port: int = 8443 + translation_scale: FiniteFloat = Field(default=1.0, gt=0.0) _Config = TypeVar("_Config", bound=QuestTeleopConfig) @@ -365,12 +367,20 @@ def _get_output_pose(self, hand: Hand) -> PoseStamped | None: delta = current_pose - initial_pose return PoseStamped( - position=delta.position, + position=delta.position * self.config.translation_scale, orientation=delta.orientation, ts=current_pose.ts, frame_id=current_pose.frame_id, ) + @rpc + def set_translation_scale(self, translation_scale: float) -> None: + """Set the positive multiplier applied to controller position deltas.""" + with self._lock: + self.config = type(self.config).model_validate( + {**self.config.model_dump(), "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..e19aecdeaa 100644 --- a/dimos/teleop/quest/test_quest_teleop_module.py +++ b/dimos/teleop/quest/test_quest_teleop_module.py @@ -14,8 +14,10 @@ from collections.abc import Iterator +from pydantic import ValidationError 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 +46,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(ValidationError): + module.set_translation_scale(translation_scale) + + assert module.config.translation_scale == 1.0 + + def test_hand_teleop_pinch_toggles_engagement(mocker) -> None: module = HandTeleopModule() try: From 9309123ff6edb9317651f538635257ae3fea38b8 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Sat, 8 Aug 2026 13:19:31 -0700 Subject: [PATCH 3/7] test(teleop): remove duplicate scaling coverage --- dimos/teleop/hosted/test_arm_command.py | 24 ------------------------ 1 file changed, 24 deletions(-) diff --git a/dimos/teleop/hosted/test_arm_command.py b/dimos/teleop/hosted/test_arm_command.py index 0c4007588b..28ca08f3f1 100644 --- a/dimos/teleop/hosted/test_arm_command.py +++ b/dimos/teleop/hosted/test_arm_command.py @@ -29,7 +29,6 @@ from typing import Any from unittest.mock import MagicMock, patch -from pydantic import ValidationError import pytest from dimos.core.module import Module @@ -235,29 +234,6 @@ def test_release_disengages(module: ArmCommandModule) -> None: assert not module._is_engaged[Hand.RIGHT] -def test_translation_scale_changes_pose_delta(module: ArmCommandModule) -> 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: ArmCommandModule, translation_scale: float -) -> None: - with pytest.raises(ValidationError): - module.set_translation_scale(translation_scale) - - assert module.config.translation_scale == 1.0 - - # ─── E-STOP latch ────────────────────────────────────────────────────── From b02db1c919c6feab2583e75f7409b8a5acefee11 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Sat, 8 Aug 2026 14:03:22 -0700 Subject: [PATCH 4/7] feat(teleop): add hosted UI scaling controls --- dimos/teleop/hosted/arm_command.py | 32 ++++++++++++++++++++++-- dimos/teleop/hosted/go2_command.py | 33 +++++++++++++++++++++++++ dimos/teleop/hosted/test_arm_command.py | 18 ++++++++++++++ dimos/teleop/hosted/test_go2_command.py | 23 +++++++++++++++++ 4 files changed, 104 insertions(+), 2 deletions(-) diff --git a/dimos/teleop/hosted/arm_command.py b/dimos/teleop/hosted/arm_command.py index 166a2ecb34..d9f11beb45 100644 --- a/dimos/teleop/hosted/arm_command.py +++ b/dimos/teleop/hosted/arm_command.py @@ -25,6 +25,7 @@ from typing import Any from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped +from pydantic import ValidationError from reactivex.disposable import Disposable from dimos.control.coordinator import ControlCoordinator @@ -45,6 +46,7 @@ class ArmCommandConfig(ArmTeleopConfig): cmd_stale_after_sec: float = 0.5 + enable_ui_scaling: bool = False class ArmCommandModule(ArmTeleopModule): @@ -166,11 +168,21 @@ 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.config.translation_scale self.coordinator_ee_twist_command.publish( TwistStamped( frame_id=EEF_TWIST_TASK_NAME, - linear=[msg.linear.x, msg.linear.y, msg.linear.z], - angular=[msg.angular.x, msg.angular.y, msg.angular.z], + linear=[ + msg.linear.x * scale, + msg.linear.y * scale, + msg.linear.z * scale, + ], + angular=[ + msg.angular.x * scale, + msg.angular.y * scale, + msg.angular.z * scale, + ], ts=msg.ts, ) ) @@ -194,6 +206,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)))) @@ -203,6 +217,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, ValidationError): + self._send_ack(nonce, False) + return + self._send_ack(nonce, True) + # ─── E-STOP gating over the inherited control loop ──────────────── def _handle_engage(self) -> None: @@ -266,6 +293,7 @@ def _publish_robot_state(self) -> None: "left": self._is_engaged[Hand.LEFT], "right": self._is_engaged[Hand.RIGHT], }, + "teleop_scale": self.config.translation_scale, } try: self.robot_state.publish(json.dumps(state).encode()) diff --git a/dimos/teleop/hosted/go2_command.py b/dimos/teleop/hosted/go2_command.py index 73044aad52..ce2c817601 100644 --- a/dimos/teleop/hosted/go2_command.py +++ b/dimos/teleop/hosted/go2_command.py @@ -24,6 +24,7 @@ import json import math +import threading import time from typing import Any @@ -70,6 +71,7 @@ class Go2CommandConfig(ModuleConfig): allow_acrobatics: bool = False max_linear_mps: float = 1.5 max_angular_rps: float = 2.0 + enable_ui_scaling: bool = False class Go2CommandModule(Module): @@ -101,6 +103,8 @@ def __init__(self, **kwargs: Any) -> None: self._posture = "StandReady" self._last_cmd_ts = 0.0 self._last_cmd_nonzero = False + self._teleop_scale = 1.0 + self._teleop_scale_lock = threading.Lock() @rpc def start(self) -> None: @@ -154,6 +158,8 @@ def _on_state_json(self, data: Any) -> None: self._handle_nav_goal(msg) elif kind == "nav_cancel": self._handle_nav_cancel(msg.get("nonce")) + elif kind == "teleop_scale": + self._handle_teleop_scale(msg) # ─── E-STOP + operator-loss ─────────────────────────────────────── @@ -376,6 +382,24 @@ def _handle_nav_cancel(self, nonce: Any) -> None: logger.info("nav_cancel: plan cancelled by operator") self._send_ack(nonce, True) + def _handle_teleop_scale(self, msg: dict[str, Any]) -> None: + """Apply an opt-in UI scale to manual drive commands.""" + nonce = msg.get("nonce") + if not self.config.enable_ui_scaling: + self._send_ack(nonce, False) + return + try: + scale = float(msg["scale"]) + except (KeyError, TypeError, ValueError): + self._send_ack(nonce, False) + return + if not math.isfinite(scale) or scale <= 0.0: + self._send_ack(nonce, False) + return + with self._teleop_scale_lock: + self._teleop_scale = scale + self._send_ack(nonce, True) + def _cancel_nav(self) -> None: try: msg = Bool() @@ -406,6 +430,14 @@ def _on_cmd_vel_in(self, twist: TwistStamped) -> None: if not _all_finite(twist): logger.warning("dropping non-finite cmd_vel") return + with self._teleop_scale_lock: + scale = self._teleop_scale + twist.linear.x *= scale + twist.linear.y *= scale + twist.linear.z *= scale + twist.angular.x *= scale + twist.angular.y *= scale + twist.angular.z *= scale lin_max = self.config.max_linear_mps ang_max = self.config.max_angular_rps twist.linear.x = _clamp(twist.linear.x, -lin_max, lin_max) @@ -430,6 +462,7 @@ def _robot_state(self) -> dict[str, Any]: "obstacle_avoidance": self._obstacle_avoidance, "light": self._light, "estopped": self._estopped, + "teleop_scale": self._teleop_scale, } def _publish_robot_state(self) -> None: diff --git a/dimos/teleop/hosted/test_arm_command.py b/dimos/teleop/hosted/test_arm_command.py index 28ca08f3f1..9aab8caa72 100644 --- a/dimos/teleop/hosted/test_arm_command.py +++ b/dimos/teleop/hosted/test_arm_command.py @@ -164,6 +164,24 @@ def test_twist_routes_to_eef_twist_task(module: ArmCommandModule) -> None: 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.config.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)) + + assert _sent_acks(module) == [{"type": "cmd_ack", "nonce": 4, "ok": True}] + assert module.config.translation_scale == 0.5 + out = module.coordinator_ee_twist_command.publish.call_args.args[0] + assert out.linear.x == pytest.approx(0.1) + + 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/hosted/test_go2_command.py b/dimos/teleop/hosted/test_go2_command.py index a7046f4636..27c49c252a 100644 --- a/dimos/teleop/hosted/test_go2_command.py +++ b/dimos/teleop/hosted/test_go2_command.py @@ -24,6 +24,7 @@ from __future__ import annotations from collections.abc import Iterator +import json import time from types import SimpleNamespace from typing import Any @@ -52,6 +53,7 @@ def module(monkeypatch: pytest.MonkeyPatch) -> Iterator[Go2CommandModule]: allow_acrobatics=False, max_linear_mps=1.5, max_angular_rps=2.0, + enable_ui_scaling=False, ) for port in ("cmd_ack", "tele_cmd_vel", "robot_state", "goal_request", "stop_movement"): setattr(module, port, MagicMock()) @@ -67,6 +69,10 @@ def _twist(ts: float, *, vx: float = 0.3) -> TwistStamped: return t +def _sent_acks(module: Go2CommandModule) -> list[dict[str, Any]]: + return [json.loads(call.args[0]) for call in module.cmd_ack.publish.call_args_list] + + # ─── sport allow-list (RPC to driver) ──────────────────────────────── _NON_ACROBATIC = [n for n in ALLOWED_SPORT_CMDS if n not in ("FrontJump", "FrontPounce")] @@ -153,6 +159,23 @@ def test_drive_forwards_fresh(module: Go2CommandModule) -> None: assert module._last_cmd_ts == ts +def test_ui_scale_disabled_is_rejected(module: Go2CommandModule) -> 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}] + + +def test_ui_scale_scales_drive_before_clamping(module: Go2CommandModule) -> None: + module.config.enable_ui_scaling = True + module._on_state_json(b'{"type": "teleop_scale", "scale": 10, "nonce": 4}') + + module._on_cmd_vel_in(_twist(time.time(), vx=0.3)) + + assert _sent_acks(module) == [{"type": "cmd_ack", "nonce": 4, "ok": True}] + out = module.tele_cmd_vel.publish.call_args.args[0] + assert out.linear.x == 1.5 + + def test_drive_suppresses_idle_zero_stream(module: Go2CommandModule) -> None: # Idle-joystick zeros must NOT be forwarded — MovementManager treats any # tele_cmd_vel as active manual drive and would cancel the nav plan. From 96a60d1006d1aa0caf5ac1c84d5945e5bc7e10c9 Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Sat, 8 Aug 2026 14:13:15 -0700 Subject: [PATCH 5/7] refactor(teleop): limit UI scaling to arms --- dimos/teleop/hosted/go2_command.py | 33 ------------------------- dimos/teleop/hosted/test_go2_command.py | 23 ----------------- 2 files changed, 56 deletions(-) diff --git a/dimos/teleop/hosted/go2_command.py b/dimos/teleop/hosted/go2_command.py index ce2c817601..73044aad52 100644 --- a/dimos/teleop/hosted/go2_command.py +++ b/dimos/teleop/hosted/go2_command.py @@ -24,7 +24,6 @@ import json import math -import threading import time from typing import Any @@ -71,7 +70,6 @@ class Go2CommandConfig(ModuleConfig): allow_acrobatics: bool = False max_linear_mps: float = 1.5 max_angular_rps: float = 2.0 - enable_ui_scaling: bool = False class Go2CommandModule(Module): @@ -103,8 +101,6 @@ def __init__(self, **kwargs: Any) -> None: self._posture = "StandReady" self._last_cmd_ts = 0.0 self._last_cmd_nonzero = False - self._teleop_scale = 1.0 - self._teleop_scale_lock = threading.Lock() @rpc def start(self) -> None: @@ -158,8 +154,6 @@ def _on_state_json(self, data: Any) -> None: self._handle_nav_goal(msg) elif kind == "nav_cancel": self._handle_nav_cancel(msg.get("nonce")) - elif kind == "teleop_scale": - self._handle_teleop_scale(msg) # ─── E-STOP + operator-loss ─────────────────────────────────────── @@ -382,24 +376,6 @@ def _handle_nav_cancel(self, nonce: Any) -> None: logger.info("nav_cancel: plan cancelled by operator") self._send_ack(nonce, True) - def _handle_teleop_scale(self, msg: dict[str, Any]) -> None: - """Apply an opt-in UI scale to manual drive commands.""" - nonce = msg.get("nonce") - if not self.config.enable_ui_scaling: - self._send_ack(nonce, False) - return - try: - scale = float(msg["scale"]) - except (KeyError, TypeError, ValueError): - self._send_ack(nonce, False) - return - if not math.isfinite(scale) or scale <= 0.0: - self._send_ack(nonce, False) - return - with self._teleop_scale_lock: - self._teleop_scale = scale - self._send_ack(nonce, True) - def _cancel_nav(self) -> None: try: msg = Bool() @@ -430,14 +406,6 @@ def _on_cmd_vel_in(self, twist: TwistStamped) -> None: if not _all_finite(twist): logger.warning("dropping non-finite cmd_vel") return - with self._teleop_scale_lock: - scale = self._teleop_scale - twist.linear.x *= scale - twist.linear.y *= scale - twist.linear.z *= scale - twist.angular.x *= scale - twist.angular.y *= scale - twist.angular.z *= scale lin_max = self.config.max_linear_mps ang_max = self.config.max_angular_rps twist.linear.x = _clamp(twist.linear.x, -lin_max, lin_max) @@ -462,7 +430,6 @@ def _robot_state(self) -> dict[str, Any]: "obstacle_avoidance": self._obstacle_avoidance, "light": self._light, "estopped": self._estopped, - "teleop_scale": self._teleop_scale, } def _publish_robot_state(self) -> None: diff --git a/dimos/teleop/hosted/test_go2_command.py b/dimos/teleop/hosted/test_go2_command.py index 27c49c252a..a7046f4636 100644 --- a/dimos/teleop/hosted/test_go2_command.py +++ b/dimos/teleop/hosted/test_go2_command.py @@ -24,7 +24,6 @@ from __future__ import annotations from collections.abc import Iterator -import json import time from types import SimpleNamespace from typing import Any @@ -53,7 +52,6 @@ def module(monkeypatch: pytest.MonkeyPatch) -> Iterator[Go2CommandModule]: allow_acrobatics=False, max_linear_mps=1.5, max_angular_rps=2.0, - enable_ui_scaling=False, ) for port in ("cmd_ack", "tele_cmd_vel", "robot_state", "goal_request", "stop_movement"): setattr(module, port, MagicMock()) @@ -69,10 +67,6 @@ def _twist(ts: float, *, vx: float = 0.3) -> TwistStamped: return t -def _sent_acks(module: Go2CommandModule) -> list[dict[str, Any]]: - return [json.loads(call.args[0]) for call in module.cmd_ack.publish.call_args_list] - - # ─── sport allow-list (RPC to driver) ──────────────────────────────── _NON_ACROBATIC = [n for n in ALLOWED_SPORT_CMDS if n not in ("FrontJump", "FrontPounce")] @@ -159,23 +153,6 @@ def test_drive_forwards_fresh(module: Go2CommandModule) -> None: assert module._last_cmd_ts == ts -def test_ui_scale_disabled_is_rejected(module: Go2CommandModule) -> 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}] - - -def test_ui_scale_scales_drive_before_clamping(module: Go2CommandModule) -> None: - module.config.enable_ui_scaling = True - module._on_state_json(b'{"type": "teleop_scale", "scale": 10, "nonce": 4}') - - module._on_cmd_vel_in(_twist(time.time(), vx=0.3)) - - assert _sent_acks(module) == [{"type": "cmd_ack", "nonce": 4, "ok": True}] - out = module.tele_cmd_vel.publish.call_args.args[0] - assert out.linear.x == 1.5 - - def test_drive_suppresses_idle_zero_stream(module: Go2CommandModule) -> None: # Idle-joystick zeros must NOT be forwarded — MovementManager treats any # tele_cmd_vel as active manual drive and would cancel the nav plan. From 02d50d97e07fe87e876ae2aad6fe61694fcd025d Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Thu, 13 Aug 2026 17:30:23 -0700 Subject: [PATCH 6/7] refactor(teleop): keep scaling runtime-private --- dimos/teleop/hosted/arm_command.py | 15 +++++---------- dimos/teleop/hosted/test_arm_command.py | 13 ++++++++----- dimos/teleop/quest/quest_teleop_module.py | 15 +++++++-------- dimos/teleop/quest/test_quest_teleop_module.py | 9 ++++----- 4 files changed, 24 insertions(+), 28 deletions(-) diff --git a/dimos/teleop/hosted/arm_command.py b/dimos/teleop/hosted/arm_command.py index d9f11beb45..2aaacfaff8 100644 --- a/dimos/teleop/hosted/arm_command.py +++ b/dimos/teleop/hosted/arm_command.py @@ -25,7 +25,6 @@ from typing import Any from dimos_lcm.geometry_msgs import TwistStamped as LCMTwistStamped -from pydantic import ValidationError from reactivex.disposable import Disposable from dimos.control.coordinator import ControlCoordinator @@ -169,7 +168,7 @@ def _on_twist_bytes(self, data: bytes) -> None: return self._last_twist_ts = ts with self._lock: - scale = self.config.translation_scale + scale = self._translation_scale self.coordinator_ee_twist_command.publish( TwistStamped( frame_id=EEF_TWIST_TASK_NAME, @@ -178,11 +177,7 @@ def _on_twist_bytes(self, data: bytes) -> None: msg.linear.y * scale, msg.linear.z * scale, ], - angular=[ - msg.angular.x * scale, - msg.angular.y * scale, - msg.angular.z * scale, - ], + angular=[msg.angular.x, msg.angular.y, msg.angular.z], ts=msg.ts, ) ) @@ -224,8 +219,8 @@ def _handle_teleop_scale(self, msg: dict[str, Any]) -> None: self._send_ack(nonce, False) return try: - self.set_translation_scale(float(msg["scale"])) - except (KeyError, TypeError, ValueError, ValidationError): + self._set_translation_scale(float(msg["scale"])) + except (KeyError, TypeError, ValueError): self._send_ack(nonce, False) return self._send_ack(nonce, True) @@ -293,7 +288,7 @@ def _publish_robot_state(self) -> None: "left": self._is_engaged[Hand.LEFT], "right": self._is_engaged[Hand.RIGHT], }, - "teleop_scale": self.config.translation_scale, + "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 9aab8caa72..c7dfaddc2f 100644 --- a/dimos/teleop/hosted/test_arm_command.py +++ b/dimos/teleop/hosted/test_arm_command.py @@ -77,10 +77,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: @@ -168,18 +170,19 @@ 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.config.translation_scale == 1.0 + 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)) + 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.config.translation_scale == 0.5 + assert module._translation_scale == 0.5 out = module.coordinator_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: diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index 3795356431..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 @@ -33,7 +34,6 @@ from fastapi import WebSocket, WebSocketDisconnect from fastapi.responses import HTMLResponse from fastapi.staticfiles import StaticFiles -from pydantic import Field, FiniteFloat from dimos.constants import DIMOS_PROJECT_ROOT from dimos.core.core import rpc @@ -69,7 +69,6 @@ class QuestTeleopConfig(ModuleConfig): control_loop_hz: float = 50.0 server_port: int = 8443 - translation_scale: FiniteFloat = Field(default=1.0, gt=0.0) _Config = TypeVar("_Config", bound=QuestTeleopConfig) @@ -107,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 @@ -367,19 +367,18 @@ def _get_output_pose(self, hand: Hand) -> PoseStamped | None: delta = current_pose - initial_pose return PoseStamped( - position=delta.position * self.config.translation_scale, + position=delta.position * self._translation_scale, orientation=delta.orientation, ts=current_pose.ts, frame_id=current_pose.frame_id, ) - @rpc - def set_translation_scale(self, translation_scale: float) -> None: + 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.config = type(self.config).model_validate( - {**self.config.model_dump(), "translation_scale": translation_scale} - ) + 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 e19aecdeaa..dda35c7786 100644 --- a/dimos/teleop/quest/test_quest_teleop_module.py +++ b/dimos/teleop/quest/test_quest_teleop_module.py @@ -14,7 +14,6 @@ from collections.abc import Iterator -from pydantic import ValidationError import pytest from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped @@ -50,7 +49,7 @@ 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) + module._set_translation_scale(2.0) output = module._get_output_pose(Hand.RIGHT) assert output is not None @@ -63,10 +62,10 @@ def test_translation_scale_changes_pose_delta(module: QuestTeleopModule) -> None def test_translation_scale_must_be_positive_and_finite( module: QuestTeleopModule, translation_scale: float ) -> None: - with pytest.raises(ValidationError): - module.set_translation_scale(translation_scale) + with pytest.raises(ValueError): + module._set_translation_scale(translation_scale) - assert module.config.translation_scale == 1.0 + assert module._translation_scale == 1.0 def test_hand_teleop_pinch_toggles_engagement(mocker) -> None: From 916a9202122b2f66af65e90bddbd5aea378f847d Mon Sep 17 00:00:00 2001 From: Ruthwik Date: Thu, 13 Aug 2026 17:32:30 -0700 Subject: [PATCH 7/7] test(teleop): restore arm command fixture --- dimos/teleop/hosted/test_arm_command.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/teleop/hosted/test_arm_command.py b/dimos/teleop/hosted/test_arm_command.py index 75c2dbc9a0..14b71c4070 100644 --- a/dimos/teleop/hosted/test_arm_command.py +++ b/dimos/teleop/hosted/test_arm_command.py @@ -26,6 +26,7 @@ from collections.abc import Iterator import json import time +from types import SimpleNamespace from typing import Any from unittest.mock import MagicMock, patch