From c04454d66be54bee3846a4165be2250e0eb1f4e0 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Sat, 14 Feb 2026 15:49:36 -0800 Subject: [PATCH 01/30] initial commit --- dimos/teleop/iphone/__init__.py | 31 ++ dimos/teleop/iphone/iphone_teleop_module.py | 258 +++++++++++ dimos/teleop/iphone/web/static/index.html | 474 ++++++++++++++++++++ dimos/teleop/iphone/web/teleop_server.ts | 84 ++++ 4 files changed, 847 insertions(+) create mode 100644 dimos/teleop/iphone/__init__.py create mode 100644 dimos/teleop/iphone/iphone_teleop_module.py create mode 100644 dimos/teleop/iphone/web/static/index.html create mode 100644 dimos/teleop/iphone/web/teleop_server.ts diff --git a/dimos/teleop/iphone/__init__.py b/dimos/teleop/iphone/__init__.py new file mode 100644 index 0000000000..7148ad0fb6 --- /dev/null +++ b/dimos/teleop/iphone/__init__.py @@ -0,0 +1,31 @@ +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""iPhone teleoperation module for DimOS.""" + +from dimos.teleop.iphone.iphone_teleop_module import ( + IPhoneTeleopConfig, + IPhoneTeleopModule, + VisualizingIPhoneTeleopModule, + iphone_teleop_module, + visualizing_iphone_teleop_module, +) + +__all__ = [ + "IPhoneTeleopConfig", + "IPhoneTeleopModule", + "VisualizingIPhoneTeleopModule", + "iphone_teleop_module", + "visualizing_iphone_teleop_module", +] diff --git a/dimos/teleop/iphone/iphone_teleop_module.py b/dimos/teleop/iphone/iphone_teleop_module.py new file mode 100644 index 0000000000..03b42e383a --- /dev/null +++ b/dimos/teleop/iphone/iphone_teleop_module.py @@ -0,0 +1,258 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +iPhone Teleoperation Module. + +Receives twist commands via LCM from the Deno WebSocket bridge, +applies dead-man's switch logic, and publishes TwistStamped commands. +""" + +from dataclasses import dataclass +import threading +import time +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.geometry_msgs import TwistStamped +from dimos.teleop.base import TeleopProtocol +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +@dataclass +class IPhoneTeleopConfig(ModuleConfig): + """Configuration for iPhone Teleoperation Module.""" + + control_loop_hz: float = 50.0 + timeout_s: float = 0.2 # Dead-man's switch timeout + + +class IPhoneTeleopModule(Module[IPhoneTeleopConfig], TeleopProtocol): + """iPhone Teleoperation Module for mobile base velocity control. + + Receives twist commands from the iPhone web app via the Deno LCM bridge. + Uses a dead-man's switch: if no twist is received within timeout_s, + automatically publishes zero velocity. + + Implements TeleopProtocol. + + Outputs: + - twist_output: TwistStamped (velocity command for robot) + """ + + default_config = IPhoneTeleopConfig + + # Input from Deno bridge + iphone_twist: In[TwistStamped] + + # Output: velocity command to robot + twist_output: Out[TwistStamped] + + # ------------------------------------------------------------------------- + # Initialization + # ------------------------------------------------------------------------- + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + + self._is_engaged: bool = False + self._current_twist: TwistStamped | None = None + self._last_twist_time: float = 0.0 + self._lock = threading.RLock() + + # Control loop + self._control_loop_thread: threading.Thread | None = None + self._control_loop_running = False + + logger.info("IPhoneTeleopModule initialized") + + # ------------------------------------------------------------------------- + # Public RPC Methods + # ------------------------------------------------------------------------- + + @rpc + def start(self) -> None: + """Start the iPhone teleoperation module.""" + super().start() + + if self.iphone_twist and self.iphone_twist.transport: # type: ignore[attr-defined] + self._disposables.add(Disposable(self.iphone_twist.subscribe(self._on_twist))) # type: ignore[attr-defined] + logger.info("Subscribed to: iphone_twist") + else: + logger.warning("Stream 'iphone_twist' has no transport — skipping") + + self._start_control_loop() + logger.info("iPhone Teleoperation Module started") + + @rpc + def stop(self) -> None: + """Stop the iPhone teleoperation module.""" + logger.info("Stopping iPhone Teleoperation Module...") + self._stop_control_loop() + super().stop() + + @rpc + def engage(self, hand: Any = None) -> bool: + """Engage teleoperation.""" + with self._lock: + self._is_engaged = True + logger.info("iPhone teleop engaged") + return True + + @rpc + def disengage(self, hand: Any = None) -> None: + """Disengage teleoperation.""" + with self._lock: + self._is_engaged = False + self._current_twist = None + logger.info("iPhone teleop disengaged") + + # ------------------------------------------------------------------------- + # Callbacks + # ------------------------------------------------------------------------- + + def _on_twist(self, msg: TwistStamped) -> None: + """Callback for incoming twist from the iPhone web app.""" + with self._lock: + self._current_twist = msg + self._last_twist_time = time.perf_counter() + if not self._is_engaged: + self._is_engaged = True + logger.info("iPhone teleop engaged (twist received)") + + # ------------------------------------------------------------------------- + # Control Loop + # ------------------------------------------------------------------------- + + def _start_control_loop(self) -> None: + """Start the control loop thread.""" + if self._control_loop_running: + return + + self._control_loop_running = True + self._control_loop_thread = threading.Thread( + target=self._control_loop, + daemon=True, + name="IPhoneTeleopControlLoop", + ) + self._control_loop_thread.start() + logger.info(f"Control loop started at {self.config.control_loop_hz} Hz") + + def _stop_control_loop(self) -> None: + """Stop the control loop thread.""" + self._control_loop_running = False + if self._control_loop_thread is not None: + self._control_loop_thread.join(timeout=1.0) + self._control_loop_thread = None + logger.info("Control loop stopped") + + def _control_loop(self) -> None: + """Main control loop: publish twist at fixed rate with dead-man's switch. + + Holds self._lock for the entire iteration so overridable methods + don't need to acquire it themselves. + """ + period = 1.0 / self.config.control_loop_hz + + while self._control_loop_running: + loop_start = time.perf_counter() + try: + with self._lock: + # Dead-man's switch: auto-disengage if no twist received recently + if self._is_engaged and self._last_twist_time > 0: + elapsed = time.perf_counter() - self._last_twist_time + if elapsed > self.config.timeout_s: + self._is_engaged = False + self._current_twist = None + logger.info("iPhone teleop disengaged (timeout)") + + if self._should_publish(): + output_twist = self._get_output_twist() + if output_twist is not None: + self._publish_msg(output_twist) + except Exception: + logger.exception("Error in iPhone teleop control loop") + + elapsed = time.perf_counter() - loop_start + sleep_time = period - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + # ------------------------------------------------------------------------- + # Overridable Control Loop Methods + # ------------------------------------------------------------------------- + + def _should_publish(self) -> bool: + """Check if we should publish a twist command. + + Override to add custom conditions. + Default: Returns True if engaged. + """ + return self._is_engaged + + def _get_output_twist(self) -> TwistStamped | None: + """Get the twist to publish. + + Override to customize twist computation (e.g., apply scaling, filtering). + Default: Returns the latest twist from the phone. + """ + return self._current_twist + + def _publish_msg(self, output_msg: TwistStamped) -> None: + """Publish twist command. + + Override to customize output (e.g., apply limits, remap axes). + """ + self.twist_output.publish(output_msg) + + +class VisualizingIPhoneTeleopModule(IPhoneTeleopModule): + """iPhone teleop with Rerun visualization. + + Logs twist linear/angular components as scalar time series to Rerun. + Useful for debugging and development. + """ + + def _publish_msg(self, output_msg: TwistStamped) -> None: + """Publish twist and log to Rerun.""" + super()._publish_msg(output_msg) + + try: + import rerun as rr + + base = "world/teleop/iphone" + rr.log(f"{base}/linear_x", rr.Scalars(output_msg.linear.x)) # type: ignore[attr-defined] + rr.log(f"{base}/linear_y", rr.Scalars(output_msg.linear.y)) # type: ignore[attr-defined] + rr.log(f"{base}/angular_z", rr.Scalars(output_msg.angular.z)) # type: ignore[attr-defined] + rr.log(f"{base}/engaged", rr.Scalars(float(self._is_engaged))) # type: ignore[attr-defined] + except Exception: + pass + + +iphone_teleop_module = IPhoneTeleopModule.blueprint +visualizing_iphone_teleop_module = VisualizingIPhoneTeleopModule.blueprint + +__all__ = [ + "IPhoneTeleopConfig", + "IPhoneTeleopModule", + "VisualizingIPhoneTeleopModule", + "iphone_teleop_module", + "visualizing_iphone_teleop_module", +] diff --git a/dimos/teleop/iphone/web/static/index.html b/dimos/teleop/iphone/web/static/index.html new file mode 100644 index 0000000000..54f02fc62a --- /dev/null +++ b/dimos/teleop/iphone/web/static/index.html @@ -0,0 +1,474 @@ + + + + + + Phone Teleop + + + + +
+
+ Sensors: off + WS: disconnected +
+ +
+ + +
+

Connection

+
+ +
+
+ + +
+
+ + +
+

Sensors

+
+
+
X
+
Y
+
Z
+ +
Ori
+
0.0°
+
0.0°
+
0.0°
+ +
Gyro
+
0.0
+
0.0
+
0.0
+
+
+ + +
+

Twist (cmd_vel)

+
+
+
X
+
Y
+
Z
+ +
Lin
+
0.000
+
0.000
+
0.000
+ +
Ang
+
0.000
+
0.000
+
0.000
+
+
+ +
+ + +
+ +
+ + + + diff --git a/dimos/teleop/iphone/web/teleop_server.ts b/dimos/teleop/iphone/web/teleop_server.ts new file mode 100644 index 0000000000..5c25b81967 --- /dev/null +++ b/dimos/teleop/iphone/web/teleop_server.ts @@ -0,0 +1,84 @@ +#!/usr/bin/env -S deno run --allow-net --allow-read --allow-run --allow-write --unstable-net + +// WebSocket to LCM Bridge for iPhone Teleop +// Forwards twist data from iPhone browser to LCM + +import { LCM } from "jsr:@dimos/lcm"; +import { dirname, fromFileUrl, join } from "jsr:@std/path"; + +const PORT = 8443; + +// Resolve paths relative to script location +const scriptDir = dirname(fromFileUrl(import.meta.url)); +const certsDir = join(scriptDir, "../../../../assets/teleop_certs"); +const certPath = join(certsDir, "cert.pem"); +const keyPath = join(certsDir, "key.pem"); + +// Auto-generate self-signed certificates if they don't exist +async function ensureCerts(): Promise<{ cert: string; key: string }> { + try { + const cert = await Deno.readTextFile(certPath); + const key = await Deno.readTextFile(keyPath); + return { cert, key }; + } catch { + console.log("Generating self-signed certificates..."); + await Deno.mkdir(certsDir, { recursive: true }); + const cmd = new Deno.Command("openssl", { + args: [ + "req", "-x509", "-newkey", "rsa:2048", + "-keyout", keyPath, "-out", certPath, + "-days", "365", "-nodes", "-subj", "/CN=localhost" + ], + }); + const { code } = await cmd.output(); + if (code !== 0) { + throw new Error("Failed to generate certificates. Is openssl installed?"); + } + console.log("Certificates generated in assets/teleop_certs/"); + return { + cert: await Deno.readTextFile(certPath), + key: await Deno.readTextFile(keyPath), + }; + } +} + +const { cert, key } = await ensureCerts(); + +const lcm = new LCM(); +await lcm.start(); + +Deno.serve({ port: PORT, cert, key }, async (req) => { + const url = new URL(req.url); + + if (req.headers.get("upgrade") === "websocket") { + const { socket, response } = Deno.upgradeWebSocket(req); + socket.onopen = () => console.log("iPhone client connected"); + socket.onclose = () => console.log("iPhone client disconnected"); + + // Forward binary LCM packets from browser directly to UDP + socket.binaryType = "arraybuffer"; + socket.onmessage = async (event) => { + if (event.data instanceof ArrayBuffer) { + const packet = new Uint8Array(event.data); + try { + await lcm.publishPacket(packet); + } catch (e) { + console.error("Forward error:", e); + } + } + }; + + return response; + } + + if (url.pathname === "/" || url.pathname === "/index.html") { + const html = await Deno.readTextFile(new URL("./static/index.html", import.meta.url)); + return new Response(html, { headers: { "content-type": "text/html" } }); + } + + return new Response("Not found", { status: 404 }); +}); + +console.log(`iPhone Teleop Server: https://localhost:${PORT}`); + +await lcm.run(); From d2a9b646aa4fe6ae95bb33e634915a5a5bc595b2 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 16 Feb 2026 14:50:38 -0800 Subject: [PATCH 02/30] Feat: Added add/sub operations --- dimos/msgs/geometry_msgs/Twist.py | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/dimos/msgs/geometry_msgs/Twist.py b/dimos/msgs/geometry_msgs/Twist.py index b1320bf986..be5d9a34a0 100644 --- a/dimos/msgs/geometry_msgs/Twist.py +++ b/dimos/msgs/geometry_msgs/Twist.py @@ -88,6 +88,24 @@ def is_zero(self) -> bool: """Check if this is a zero twist (no linear or angular velocity).""" return self.linear.is_zero() and self.angular.is_zero() + def __sub__(self, other: Twist) -> Twist: + """Component-wise subtraction: self - other.""" + if not isinstance(other, Twist): + return NotImplemented + return Twist( + linear=self.linear - other.linear, + angular=self.angular - other.angular, + ) + + def __add__(self, other: Twist) -> Twist: + """Component-wise addition: self + other.""" + if not isinstance(other, Twist): + return NotImplemented + return Twist( + linear=self.linear + other.linear, + angular=self.angular + other.angular, + ) + def __bool__(self) -> bool: """Boolean conversion for Twist. From 71c43dc35ae987ae82fd71c0daf614371eedfd8c Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 16 Feb 2026 15:11:18 -0800 Subject: [PATCH 03/30] Feat: Added phone_teleop blueprints --- dimos/teleop/phone/blueprints.py | 53 +++++++++++++++++ dimos/teleop/phone/phone_extensions.py | 79 ++++++++++++++++++++++++++ 2 files changed, 132 insertions(+) create mode 100644 dimos/teleop/phone/blueprints.py create mode 100644 dimos/teleop/phone/phone_extensions.py diff --git a/dimos/teleop/phone/blueprints.py b/dimos/teleop/phone/blueprints.py new file mode 100644 index 0000000000..1d0b41ca05 --- /dev/null +++ b/dimos/teleop/phone/blueprints.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Phone teleop blueprints for testing and deployment.""" + +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport +from dimos.msgs.geometry_msgs import TwistStamped +from dimos.msgs.std_msgs.Bool import Bool +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.teleop.iphone.iphone_extensions import ( + phone_go2_teleop_module, + simple_phone_teleop_module, +) + +# ----------------------------------------------------------------------------- +# Phone Teleop Blueprints +# ----------------------------------------------------------------------------- + +_phone_input_transports = { + ("phone_sensors", TwistStamped): LCMTransport("/phone/sensors", TwistStamped), + ("phone_button", Bool): LCMTransport("/phone/button", Bool), + ("twist_output", TwistStamped): LCMTransport("/teleop/twist", TwistStamped), +} + +# Simple phone teleop (mobile base axis filtering) +simple_phone_teleop = autoconnect( + simple_phone_teleop_module(), +).transports(_phone_input_transports) + +# Phone teleop wired to Unitree Go2 +phone_go2_teleop = autoconnect( + phone_go2_teleop_module(), + unitree_go2_basic, +).transports({ + ("phone_sensors", TwistStamped): LCMTransport("/phone/sensors", TwistStamped), + ("phone_button", Bool): LCMTransport("/phone/button", Bool), +}) + + +__all__ = ["simple_phone_teleop", "phone_go2_teleop"] diff --git a/dimos/teleop/phone/phone_extensions.py b/dimos/teleop/phone/phone_extensions.py new file mode 100644 index 0000000000..5ff1483028 --- /dev/null +++ b/dimos/teleop/phone/phone_extensions.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Phone teleop module extensions and subclasses. + +Available subclasses: + - SimplePhoneTeleop: Maps raw twist to ground robot axes (linear.x, linear.y, angular.z) + - PhoneGo2Teleop: Go2-specific extension, outputs cmd_vel: Out[Twist] for direct Go2 wiring +""" + +from dimos.core import Out +from dimos.msgs.geometry_msgs import Twist, TwistStamped, Vector3 +from dimos.teleop.phone.phone_teleop_module import PhoneTeleopModule + + +class SimplePhoneTeleop(PhoneTeleopModule): + """Phone teleop mapped to ground robot axes. + + Takes the raw 6-axis twist from the base module and extracts only the + axes relevant for a mobile base: + - linear.x (forward/back from pitch tilt) + - linear.y (strafe from roll tilt) + - angular.z (turn from yaw orientation delta) + + All other axes are zeroed. + """ + + def _get_output_twist(self) -> TwistStamped | None: + """Extract mobile-base axes from raw twist.""" + raw = super()._get_output_twist() + if raw is None: + return None + + print(f"Raw twist from phone: linear=({raw.linear.x}, {raw.linear.y}, {raw.linear.z})") + + return TwistStamped( + ts=raw.ts, + frame_id="mobile_base", + linear=Vector3(x=raw.linear.x, y=raw.linear.y, z=0.0), + angular=Vector3(x=0.0, y=0.0, z=raw.linear.z), + ) + + +class PhoneGo2Teleop(SimplePhoneTeleop): + """Phone teleop for Unitree Go2. + + Extends SimplePhoneTeleop with cmd_vel: Out[Twist] + that matches GO2Connection.cmd_vel: In[Twist] for direct autoconnect wiring. + """ + + cmd_vel: Out[Twist] + + def _publish_msg(self, output_msg: TwistStamped) -> None: + """Publish as Twist on cmd_vel to match GO2Connection.""" + self.cmd_vel.publish(Twist(linear=output_msg.linear, angular=output_msg.angular)) + print(f"Published cmd_vel: linear={output_msg.linear}, angular={output_msg.angular}") + + +simple_phone_teleop_module = SimplePhoneTeleop.blueprint +phone_go2_teleop_module = PhoneGo2Teleop.blueprint + +__all__ = [ + "SimplePhoneTeleop", + "PhoneGo2Teleop", + "simple_phone_teleop_module", + "phone_go2_teleop_module", +] From 42b5833d7b6ecf6669c1cf212e8c601bb88e657c Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 16 Feb 2026 15:12:17 -0800 Subject: [PATCH 04/30] Feat/Misc - phoneteleop with go2, renamed files --- dimos/robot/all_blueprints.py | 15 +- dimos/teleop/iphone/iphone_teleop_module.py | 258 -------------- dimos/teleop/{iphone => phone}/__init__.py | 26 +- dimos/teleop/phone/blueprints.py | 2 +- dimos/teleop/phone/phone_teleop_module.py | 317 ++++++++++++++++++ .../{iphone => phone}/web/static/index.html | 314 ++++++++--------- .../{iphone => phone}/web/teleop_server.ts | 0 dimos/teleop/{ => quest}/blueprints.py | 0 8 files changed, 477 insertions(+), 455 deletions(-) delete mode 100644 dimos/teleop/iphone/iphone_teleop_module.py rename dimos/teleop/{iphone => phone}/__init__.py (59%) create mode 100644 dimos/teleop/phone/phone_teleop_module.py rename dimos/teleop/{iphone => phone}/web/static/index.html (61%) rename dimos/teleop/{iphone => phone}/web/teleop_server.ts (100%) mode change 100644 => 100755 rename dimos/teleop/{ => quest}/blueprints.py (100%) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 4601dff7c9..92635773b5 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -16,11 +16,11 @@ # Run `pytest dimos/robot/test_all_blueprints_generation.py` to regenerate. all_blueprints = { - "arm-teleop": "dimos.teleop.blueprints:arm_teleop", - "arm-teleop-dual": "dimos.teleop.blueprints:arm_teleop_dual", - "arm-teleop-piper": "dimos.teleop.blueprints:arm_teleop_piper", - "arm-teleop-visualizing": "dimos.teleop.blueprints:arm_teleop_visualizing", - "arm-teleop-xarm6": "dimos.teleop.blueprints:arm_teleop_xarm6", + "arm-teleop": "dimos.teleop.quest.blueprints:arm_teleop", + "arm-teleop-dual": "dimos.teleop.quest.blueprints:arm_teleop_dual", + "arm-teleop-piper": "dimos.teleop.quest.blueprints:arm_teleop_piper", + "arm-teleop-visualizing": "dimos.teleop.quest.blueprints:arm_teleop_visualizing", + "arm-teleop-xarm6": "dimos.teleop.quest.blueprints:arm_teleop_xarm6", "coordinator-basic": "dimos.control.blueprints:coordinator_basic", "coordinator-cartesian-ik-mock": "dimos.control.blueprints:coordinator_cartesian_ik_mock", "coordinator-cartesian-ik-piper": "dimos.control.blueprints:coordinator_cartesian_ik_piper", @@ -47,6 +47,8 @@ "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "dual-xarm6-planner": "dimos.manipulation.manipulation_blueprints:dual_xarm6_planner", + "phone-go2-teleop": "dimos.teleop.phone.blueprints:phone_go2_teleop", + "simple-phone-teleop": "dimos.teleop.phone.blueprints:simple_phone_teleop", "uintree-g1-primitive-no-nav": "dimos.robot.unitree.g1.blueprints.primitive.uintree_g1_primitive_no_nav:uintree_g1_primitive_no_nav", "unitree-g1": "dimos.robot.unitree.g1.blueprints.perceptive.unitree_g1:unitree_g1", "unitree-g1-agentic": "dimos.robot.unitree.g1.blueprints.agentic.unitree_g1_agentic:unitree_g1_agentic", @@ -96,6 +98,7 @@ "google_maps_skill": "dimos.agents.skills.google_maps_skill_container", "gps_nav_skill": "dimos.agents.skills.gps_nav_skill", "grasping_module": "dimos.manipulation.grasping.grasping", + "phone_teleop_module": "dimos.teleop.phone.phone_teleop_module", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree.keyboard_teleop", "manipulation_module": "dimos.manipulation.manipulation_module", @@ -118,6 +121,8 @@ "twist_teleop_module": "dimos.teleop.quest.quest_extensions", "unitree_skills": "dimos.robot.unitree.unitree_skill_container", "utilization": "dimos.utils.monitoring", + "phone_go2_teleop_module": "dimos.teleop.phone.phone_extensions", + "simple_phone_teleop_module": "dimos.teleop.phone.phone_extensions", "visualizing_teleop_module": "dimos.teleop.quest.quest_extensions", "vlm_agent": "dimos.agents.vlm_agent", "vlm_stream_tester": "dimos.agents.vlm_stream_tester", diff --git a/dimos/teleop/iphone/iphone_teleop_module.py b/dimos/teleop/iphone/iphone_teleop_module.py deleted file mode 100644 index 03b42e383a..0000000000 --- a/dimos/teleop/iphone/iphone_teleop_module.py +++ /dev/null @@ -1,258 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -""" -iPhone Teleoperation Module. - -Receives twist commands via LCM from the Deno WebSocket bridge, -applies dead-man's switch logic, and publishes TwistStamped commands. -""" - -from dataclasses import dataclass -import threading -import time -from typing import Any - -from reactivex.disposable import Disposable - -from dimos.core import In, Module, Out, rpc -from dimos.core.module import ModuleConfig -from dimos.msgs.geometry_msgs import TwistStamped -from dimos.teleop.base import TeleopProtocol -from dimos.utils.logging_config import setup_logger - -logger = setup_logger() - - -@dataclass -class IPhoneTeleopConfig(ModuleConfig): - """Configuration for iPhone Teleoperation Module.""" - - control_loop_hz: float = 50.0 - timeout_s: float = 0.2 # Dead-man's switch timeout - - -class IPhoneTeleopModule(Module[IPhoneTeleopConfig], TeleopProtocol): - """iPhone Teleoperation Module for mobile base velocity control. - - Receives twist commands from the iPhone web app via the Deno LCM bridge. - Uses a dead-man's switch: if no twist is received within timeout_s, - automatically publishes zero velocity. - - Implements TeleopProtocol. - - Outputs: - - twist_output: TwistStamped (velocity command for robot) - """ - - default_config = IPhoneTeleopConfig - - # Input from Deno bridge - iphone_twist: In[TwistStamped] - - # Output: velocity command to robot - twist_output: Out[TwistStamped] - - # ------------------------------------------------------------------------- - # Initialization - # ------------------------------------------------------------------------- - - def __init__(self, *args: Any, **kwargs: Any) -> None: - super().__init__(*args, **kwargs) - - self._is_engaged: bool = False - self._current_twist: TwistStamped | None = None - self._last_twist_time: float = 0.0 - self._lock = threading.RLock() - - # Control loop - self._control_loop_thread: threading.Thread | None = None - self._control_loop_running = False - - logger.info("IPhoneTeleopModule initialized") - - # ------------------------------------------------------------------------- - # Public RPC Methods - # ------------------------------------------------------------------------- - - @rpc - def start(self) -> None: - """Start the iPhone teleoperation module.""" - super().start() - - if self.iphone_twist and self.iphone_twist.transport: # type: ignore[attr-defined] - self._disposables.add(Disposable(self.iphone_twist.subscribe(self._on_twist))) # type: ignore[attr-defined] - logger.info("Subscribed to: iphone_twist") - else: - logger.warning("Stream 'iphone_twist' has no transport — skipping") - - self._start_control_loop() - logger.info("iPhone Teleoperation Module started") - - @rpc - def stop(self) -> None: - """Stop the iPhone teleoperation module.""" - logger.info("Stopping iPhone Teleoperation Module...") - self._stop_control_loop() - super().stop() - - @rpc - def engage(self, hand: Any = None) -> bool: - """Engage teleoperation.""" - with self._lock: - self._is_engaged = True - logger.info("iPhone teleop engaged") - return True - - @rpc - def disengage(self, hand: Any = None) -> None: - """Disengage teleoperation.""" - with self._lock: - self._is_engaged = False - self._current_twist = None - logger.info("iPhone teleop disengaged") - - # ------------------------------------------------------------------------- - # Callbacks - # ------------------------------------------------------------------------- - - def _on_twist(self, msg: TwistStamped) -> None: - """Callback for incoming twist from the iPhone web app.""" - with self._lock: - self._current_twist = msg - self._last_twist_time = time.perf_counter() - if not self._is_engaged: - self._is_engaged = True - logger.info("iPhone teleop engaged (twist received)") - - # ------------------------------------------------------------------------- - # Control Loop - # ------------------------------------------------------------------------- - - def _start_control_loop(self) -> None: - """Start the control loop thread.""" - if self._control_loop_running: - return - - self._control_loop_running = True - self._control_loop_thread = threading.Thread( - target=self._control_loop, - daemon=True, - name="IPhoneTeleopControlLoop", - ) - self._control_loop_thread.start() - logger.info(f"Control loop started at {self.config.control_loop_hz} Hz") - - def _stop_control_loop(self) -> None: - """Stop the control loop thread.""" - self._control_loop_running = False - if self._control_loop_thread is not None: - self._control_loop_thread.join(timeout=1.0) - self._control_loop_thread = None - logger.info("Control loop stopped") - - def _control_loop(self) -> None: - """Main control loop: publish twist at fixed rate with dead-man's switch. - - Holds self._lock for the entire iteration so overridable methods - don't need to acquire it themselves. - """ - period = 1.0 / self.config.control_loop_hz - - while self._control_loop_running: - loop_start = time.perf_counter() - try: - with self._lock: - # Dead-man's switch: auto-disengage if no twist received recently - if self._is_engaged and self._last_twist_time > 0: - elapsed = time.perf_counter() - self._last_twist_time - if elapsed > self.config.timeout_s: - self._is_engaged = False - self._current_twist = None - logger.info("iPhone teleop disengaged (timeout)") - - if self._should_publish(): - output_twist = self._get_output_twist() - if output_twist is not None: - self._publish_msg(output_twist) - except Exception: - logger.exception("Error in iPhone teleop control loop") - - elapsed = time.perf_counter() - loop_start - sleep_time = period - elapsed - if sleep_time > 0: - time.sleep(sleep_time) - - # ------------------------------------------------------------------------- - # Overridable Control Loop Methods - # ------------------------------------------------------------------------- - - def _should_publish(self) -> bool: - """Check if we should publish a twist command. - - Override to add custom conditions. - Default: Returns True if engaged. - """ - return self._is_engaged - - def _get_output_twist(self) -> TwistStamped | None: - """Get the twist to publish. - - Override to customize twist computation (e.g., apply scaling, filtering). - Default: Returns the latest twist from the phone. - """ - return self._current_twist - - def _publish_msg(self, output_msg: TwistStamped) -> None: - """Publish twist command. - - Override to customize output (e.g., apply limits, remap axes). - """ - self.twist_output.publish(output_msg) - - -class VisualizingIPhoneTeleopModule(IPhoneTeleopModule): - """iPhone teleop with Rerun visualization. - - Logs twist linear/angular components as scalar time series to Rerun. - Useful for debugging and development. - """ - - def _publish_msg(self, output_msg: TwistStamped) -> None: - """Publish twist and log to Rerun.""" - super()._publish_msg(output_msg) - - try: - import rerun as rr - - base = "world/teleop/iphone" - rr.log(f"{base}/linear_x", rr.Scalars(output_msg.linear.x)) # type: ignore[attr-defined] - rr.log(f"{base}/linear_y", rr.Scalars(output_msg.linear.y)) # type: ignore[attr-defined] - rr.log(f"{base}/angular_z", rr.Scalars(output_msg.angular.z)) # type: ignore[attr-defined] - rr.log(f"{base}/engaged", rr.Scalars(float(self._is_engaged))) # type: ignore[attr-defined] - except Exception: - pass - - -iphone_teleop_module = IPhoneTeleopModule.blueprint -visualizing_iphone_teleop_module = VisualizingIPhoneTeleopModule.blueprint - -__all__ = [ - "IPhoneTeleopConfig", - "IPhoneTeleopModule", - "VisualizingIPhoneTeleopModule", - "iphone_teleop_module", - "visualizing_iphone_teleop_module", -] diff --git a/dimos/teleop/iphone/__init__.py b/dimos/teleop/phone/__init__.py similarity index 59% rename from dimos/teleop/iphone/__init__.py rename to dimos/teleop/phone/__init__.py index 7148ad0fb6..552032a47b 100644 --- a/dimos/teleop/iphone/__init__.py +++ b/dimos/teleop/phone/__init__.py @@ -12,20 +12,22 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""iPhone teleoperation module for DimOS.""" +"""Phone teleoperation module for DimOS.""" -from dimos.teleop.iphone.iphone_teleop_module import ( - IPhoneTeleopConfig, - IPhoneTeleopModule, - VisualizingIPhoneTeleopModule, - iphone_teleop_module, - visualizing_iphone_teleop_module, +from dimos.teleop.phone.phone_extensions import ( + SimplePhoneTeleop, + simple_phone_teleop_module, +) +from dimos.teleop.phone.phone_teleop_module import ( + PhoneTeleopConfig, + PhoneTeleopModule, + phone_teleop_module, ) __all__ = [ - "IPhoneTeleopConfig", - "IPhoneTeleopModule", - "VisualizingIPhoneTeleopModule", - "iphone_teleop_module", - "visualizing_iphone_teleop_module", + "PhoneTeleopConfig", + "PhoneTeleopModule", + "SimplePhoneTeleop", + "phone_teleop_module", + "simple_phone_teleop_module", ] diff --git a/dimos/teleop/phone/blueprints.py b/dimos/teleop/phone/blueprints.py index 1d0b41ca05..a0b19b5ab5 100644 --- a/dimos/teleop/phone/blueprints.py +++ b/dimos/teleop/phone/blueprints.py @@ -20,7 +20,7 @@ from dimos.msgs.geometry_msgs import TwistStamped from dimos.msgs.std_msgs.Bool import Bool from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic -from dimos.teleop.iphone.iphone_extensions import ( +from dimos.teleop.phone.phone_extensions import ( phone_go2_teleop_module, simple_phone_teleop_module, ) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py new file mode 100644 index 0000000000..3229112d64 --- /dev/null +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -0,0 +1,317 @@ +#!/usr/bin/env python3 +# Copyright 2025-2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +Phone Teleoperation Module. + +Receives raw sensor data (TwistStamped) and button state (Bool) from the +phone web app via the Deno LCM bridge. Computes orientation deltas from +a home orientation captured on engage, converts to TwistStamped velocity +commands via configurable gains, and publishes. + +Raw sensor TwistStamped layout from browser: + linear = (roll, pitch, yaw) in degrees (DeviceOrientation) + angular = (gyro_x, gyro_y, gyro_z) in deg/s (DeviceMotion) +""" + +from dataclasses import dataclass +import threading +import time +from typing import Any + +from reactivex.disposable import Disposable + +from dimos.core import In, Module, Out, rpc +from dimos.core.module import ModuleConfig +from dimos.msgs.geometry_msgs import TwistStamped, Vector3 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.teleop.base import TeleopProtocol +from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + + +@dataclass +class PhoneTeleopConfig(ModuleConfig): + """Configuration for Phone Teleoperation Module.""" + + control_loop_hz: float = 50.0 + + # Gain: maps degrees of tilt to m/s. 45 deg tilt -> 1.0 m/s + linear_gain: float = 1.0 / 30.0 + # Gain: maps degrees of yaw delta to rad/s. 90 deg -> 1.0 rad/s + angular_gain: float = 1.0 / 30.0 + + +class PhoneTeleopModule(Module[PhoneTeleopConfig], TeleopProtocol): + """Phone Teleoperation Module. + + Receives raw sensor data from the phone web app: + - TwistStamped: linear=(roll, pitch, yaw) deg, angular=(gyro) deg/s + - Bool: teleop button state (True = held) + + On engage (button pressed), captures the current orientation as home. + Each control-loop tick computes the orientation delta from home, + converts it to a TwistStamped via configurable gains, and publishes. + + Implements TeleopProtocol. + + Outputs: + - twist_output: TwistStamped (velocity command for robot) + """ + + default_config = PhoneTeleopConfig + + # Inputs from Deno bridge + phone_sensors: In[TwistStamped] + phone_button: In[Bool] + + # Output: velocity command to robot + twist_output: Out[TwistStamped] + + # ------------------------------------------------------------------------- + # Initialization + # ------------------------------------------------------------------------- + + def __init__(self, *args: Any, **kwargs: Any) -> None: + super().__init__(*args, **kwargs) + + self._is_engaged: bool = False + self._teleop_button: bool = False + + # Raw sensor messages (like Quest stores PoseStamped) + self._current_sensors: TwistStamped | None = None + self._home_sensors: TwistStamped | None = None + + self._lock = threading.RLock() + + # Control loop + self._control_loop_thread: threading.Thread | None = None + self._control_loop_running = False + + logger.info("PhoneTeleopModule initialized") + + # ------------------------------------------------------------------------- + # Public RPC Methods + # ------------------------------------------------------------------------- + + @rpc + def start(self) -> None: + """Start the phone teleoperation module.""" + super().start() + + input_streams = { + "phone_sensors": (self.phone_sensors, self._on_sensors), + "phone_button": (self.phone_button, self._on_button), + } + connected = [] + for name, (stream, handler) in input_streams.items(): + if not (stream and stream.transport): # type: ignore[attr-defined] + logger.warning(f"Stream '{name}' has no transport — skipping") + continue + self._disposables.add(Disposable(stream.subscribe(handler))) # type: ignore[attr-defined] + connected.append(name) + + if connected: + logger.info(f"Subscribed to: {', '.join(connected)}") + + self._start_control_loop() + logger.info("Phone Teleoperation Module started") + + @rpc + def stop(self) -> None: + """Stop the phone teleoperation module.""" + logger.info("Stopping Phone Teleoperation Module...") + self._stop_control_loop() + super().stop() + + @rpc + def engage(self, hand: Any = None) -> bool: + """Engage teleoperation.""" + with self._lock: + return self._engage() + + @rpc + def disengage(self, hand: Any = None) -> None: + """Disengage teleoperation.""" + with self._lock: + self._disengage() + + # ------------------------------------------------------------------------- + # Internal engage / disengage (assumes lock is held) + # ------------------------------------------------------------------------- + + def _engage(self) -> bool: + """Engage: capture current sensors as home. Assumes lock held.""" + if self._current_sensors is None: + logger.error("Engage failed: no sensor data yet") + return False + self._home_sensors = self._current_sensors + self._is_engaged = True + logger.info("Phone teleop engaged") + return True + + def _disengage(self) -> None: + """Disengage: stop publishing. Assumes lock held.""" + self._is_engaged = False + self._home_sensors = None + logger.info("Phone teleop disengaged") + + # ------------------------------------------------------------------------- + # Callbacks + # ------------------------------------------------------------------------- + + def _on_sensors(self, msg: TwistStamped) -> None: + """Callback for raw sensor TwistStamped from the phone web app. + + linear = (roll, pitch, yaw) in degrees + angular = (gyro_x, gyro_y, gyro_z) in deg/s + """ + with self._lock: + self._current_sensors = msg + + def _on_button(self, msg: Bool) -> None: + """Callback for teleop button state.""" + with self._lock: + self._teleop_button = bool(msg.data) + + # ------------------------------------------------------------------------- + # Control Loop + # ------------------------------------------------------------------------- + + def _start_control_loop(self) -> None: + """Start the control loop thread.""" + if self._control_loop_running: + return + + self._control_loop_running = True + self._control_loop_thread = threading.Thread( + target=self._control_loop, + daemon=True, + name="PhoneTeleopControlLoop", + ) + self._control_loop_thread.start() + logger.info(f"Control loop started at {self.config.control_loop_hz} Hz") + + def _stop_control_loop(self) -> None: + """Stop the control loop thread.""" + self._control_loop_running = False + if self._control_loop_thread is not None: + self._control_loop_thread.join(timeout=1.0) + self._control_loop_thread = None + logger.info("Control loop stopped") + + def _control_loop(self) -> None: + """Main control loop: handle engagement, compute deltas, publish twist. + + Holds self._lock for the entire iteration so overridable methods + don't need to acquire it themselves. + """ + period = 1.0 / self.config.control_loop_hz + + while self._control_loop_running: + loop_start = time.perf_counter() + try: + with self._lock: + self._handle_engage() + + if self._should_publish(): + output_twist = self._get_output_twist() + if output_twist is not None: + self._publish_msg(output_twist) + except Exception: + logger.exception("Error in phone teleop control loop") + + elapsed = time.perf_counter() - loop_start + sleep_time = period - elapsed + if sleep_time > 0: + time.sleep(sleep_time) + + # ------------------------------------------------------------------------- + # Overridable Control Loop Methods + # ------------------------------------------------------------------------- + + def _handle_engage(self) -> None: + """Check button state and engage/disengage accordingly. + + Override to customize engagement logic. + Default: button hold = engaged, release = disengaged. + """ + if self._teleop_button: + if not self._is_engaged: + self._engage() + else: + if self._is_engaged: + self._disengage() + + def _should_publish(self) -> bool: + """Check if we should publish a twist command. + + Override to add custom conditions. + Default: Returns True if engaged. + """ + return self._is_engaged + + def _get_output_twist(self) -> TwistStamped | None: + """Compute twist from orientation delta. + Override to customize twist computation (e.g., apply scaling, filtering). + Default: Computes delta angles from home orientation, applies gains. + """ + current = self._current_sensors + home = self._home_sensors + if current is None or home is None: + return None + + # Twist subtraction: delta.linear = orientation delta, delta.angular = gyro delta + delta = current - home + + # Handle yaw wraparound (linear.z = yaw, 0-360 degrees) + d_yaw = delta.linear.z + if d_yaw > 180: + d_yaw -= 360 + elif d_yaw < -180: + d_yaw += 360 + + cfg = self.config + return TwistStamped( + ts=current.ts, + frame_id="phone", + linear=Vector3( + x=-delta.linear.y * cfg.linear_gain, # pitch forward -> drive forward + y=-delta.linear.x * cfg.linear_gain, # roll right -> strafe right + z=d_yaw * cfg.linear_gain, # yaw right -> positive z + ), + angular=Vector3( + x=current.angular.x * cfg.angular_gain, + y=current.angular.y * cfg.angular_gain, + z=current.angular.z * cfg.angular_gain, + ), + ) + + def _publish_msg(self, output_msg: TwistStamped) -> None: + """Publish twist command. + + Override to customize output (e.g., apply limits, remap axes). + """ + self.twist_output.publish(output_msg) + + +phone_teleop_module = PhoneTeleopModule.blueprint + +__all__ = [ + "PhoneTeleopConfig", + "PhoneTeleopModule", + "phone_teleop_module", +] diff --git a/dimos/teleop/iphone/web/static/index.html b/dimos/teleop/phone/web/static/index.html similarity index 61% rename from dimos/teleop/iphone/web/static/index.html rename to dimos/teleop/phone/web/static/index.html index 54f02fc62a..ce233d562f 100644 --- a/dimos/teleop/iphone/web/static/index.html +++ b/dimos/teleop/phone/web/static/index.html @@ -3,46 +3,69 @@ - Phone Teleop + DimOS Phone Teleop - -
-
- Sensors: off - WS: disconnected -
+

DimOS Phone Teleop

+ + +
+ Sensors: off + WS: disconnected +
+ + +
+
- -
-

Connection

-
- -
-
- - -
+ +
+ +
@@ -151,33 +189,10 @@

Sensors

- -
-

Twist (cmd_vel)

-
-
-
X
-
Y
-
Z
- -
Lin
-
0.000
-
0.000
-
0.000
- -
Ang
-
0.000
-
0.000
-
0.000
-
-
-
-
- -
+ diff --git a/dimos/teleop/phone/web/teleop_server.ts b/dimos/teleop/phone/web/teleop_server.ts index 5c25b81967..26202cf166 100755 --- a/dimos/teleop/phone/web/teleop_server.ts +++ b/dimos/teleop/phone/web/teleop_server.ts @@ -1,12 +1,12 @@ #!/usr/bin/env -S deno run --allow-net --allow-read --allow-run --allow-write --unstable-net -// WebSocket to LCM Bridge for iPhone Teleop -// Forwards twist data from iPhone browser to LCM +// WebSocket to LCM Bridge for Phone Teleop +// Forwards twist data from Phone browser to LCM import { LCM } from "jsr:@dimos/lcm"; import { dirname, fromFileUrl, join } from "jsr:@std/path"; -const PORT = 8443; +const PORT = 8444; // Resolve paths relative to script location const scriptDir = dirname(fromFileUrl(import.meta.url)); @@ -47,13 +47,14 @@ const { cert, key } = await ensureCerts(); const lcm = new LCM(); await lcm.start(); +// Binds to all interfaces so the phone can reach the server over LAN. Deno.serve({ port: PORT, cert, key }, async (req) => { const url = new URL(req.url); if (req.headers.get("upgrade") === "websocket") { const { socket, response } = Deno.upgradeWebSocket(req); - socket.onopen = () => console.log("iPhone client connected"); - socket.onclose = () => console.log("iPhone client disconnected"); + socket.onopen = () => console.log("Phone client connected"); + socket.onclose = () => console.log("Phone client disconnected"); // Forward binary LCM packets from browser directly to UDP socket.binaryType = "arraybuffer"; @@ -79,6 +80,6 @@ Deno.serve({ port: PORT, cert, key }, async (req) => { return new Response("Not found", { status: 404 }); }); -console.log(`iPhone Teleop Server: https://localhost:${PORT}`); +console.log(`Phone Teleop Server: https://localhost:${PORT}`); await lcm.run(); From a804625d7d5183f195f8028877fe862fde3b2175 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Mon, 16 Feb 2026 23:43:24 -0800 Subject: [PATCH 10/30] Fix: button margin auto --- dimos/teleop/phone/web/static/index.html | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/dimos/teleop/phone/web/static/index.html b/dimos/teleop/phone/web/static/index.html index 87e897a358..c08fdcac6b 100644 --- a/dimos/teleop/phone/web/static/index.html +++ b/dimos/teleop/phone/web/static/index.html @@ -107,9 +107,8 @@ margin-bottom: 12px; } - .spacer { flex: 1; } - #teleop-btn { + margin-top: auto; width: 100%; max-width: 400px; padding: 28px; @@ -162,8 +161,6 @@

Sensors

-
- From 4ed86f8d7195294d02152a35e68cabf1211fb466 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 00:24:42 -0800 Subject: [PATCH 11/30] Feat: server stop with timeout and kill --- dimos/teleop/phone/phone_teleop_module.py | 33 ++++++++++++----------- dimos/teleop/quest/quest_teleop_module.py | 19 ++++++++----- 2 files changed, 31 insertions(+), 21 deletions(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index eb8d01e99e..5e8c3ef394 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -28,6 +28,7 @@ from dataclasses import dataclass from pathlib import Path +import signal import subprocess import threading import time @@ -37,7 +38,7 @@ from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig -from dimos.msgs.geometry_msgs import TwistStamped, Vector3 +from dimos.msgs.geometry_msgs import Twist, TwistStamped, Vector3 from dimos.msgs.std_msgs.Bool import Bool from dimos.teleop.base import TeleopProtocol from dimos.utils.logging_config import setup_logger @@ -201,7 +202,8 @@ def _on_button(self, msg: Bool) -> None: def _start_server(self) -> None: """Launch the Deno WebSocket-to-LCM bridge server as a subprocess.""" - if self._server_process is not None: + if self._server_process is not None and self._server_process.poll() is None: + logger.warning("Deno bridge already running", pid=self._server_process.pid) return script = str(self._server_script) @@ -218,20 +220,25 @@ def _start_server(self) -> None: try: self._server_process = subprocess.Popen(cmd) logger.info(f"Deno bridge server started (pid {self._server_process.pid})") - except FileNotFoundError: - logger.error("'deno' not found in PATH — install Deno or start the server manually") + except OSError as e: + logger.error(f"Failed to start Deno bridge: {e}") def _stop_server(self) -> None: """Terminate the Deno bridge server subprocess.""" - if self._server_process is None: + if self._server_process is None or self._server_process.poll() is not None: + self._server_process = None return - self._server_process.terminate() + logger.info("Stopping Deno bridge server", pid=self._server_process.pid) + self._server_process.send_signal(signal.SIGTERM) try: self._server_process.wait(timeout=3) except subprocess.TimeoutExpired: + logger.warning( + "Deno bridge did not exit, sending SIGKILL", pid=self._server_process.pid + ) self._server_process.kill() - self._server_process.wait(timeout=1) + self._server_process.wait(timeout=5) logger.info("Deno bridge server stopped") self._server_process = None @@ -262,11 +269,7 @@ def _stop_control_loop(self) -> None: logger.info("Control loop stopped") def _control_loop(self) -> None: - """Main control loop: handle engagement, compute deltas, publish twist. - - Holds self._lock for the entire iteration so overridable methods - don't need to acquire it themselves. - """ + """Main control loop: handle engagement, compute deltas, publish twist""" period = 1.0 / self.config.control_loop_hz while self._control_loop_running: @@ -322,8 +325,8 @@ def _get_output_twist(self) -> TwistStamped | None: if current is None or home is None: return None - # Twist subtraction: delta.linear = orientation delta, delta.angular = gyro delta - delta = current - home + # Subtract as Twist (drops ts/frame_id, returns Twist) + delta: Twist = Twist(current) - Twist(home) # Handle yaw wraparound (linear.z = yaw, 0-360 degrees) d_yaw = delta.linear.z @@ -351,7 +354,7 @@ def _get_output_twist(self) -> TwistStamped | None: def _publish_msg(self, output_msg: TwistStamped) -> None: """Publish twist command. - + Override to customize output (e.g., apply limits, remap axes). """ self.twist_output.publish(output_msg) diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index 6efc6398c0..70dc912db0 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -23,6 +23,7 @@ from dataclasses import dataclass from enum import IntEnum from pathlib import Path +import signal import subprocess import threading import time @@ -239,7 +240,8 @@ def _on_joy(self, hand: Hand, joy: Joy) -> None: def _start_server(self) -> None: """Launch the Deno WebSocket-to-LCM bridge server as a subprocess.""" - if self._server_process is not None: + if self._server_process is not None and self._server_process.poll() is None: + logger.warning("Deno bridge already running", pid=self._server_process.pid) return script = str(self._server_script) @@ -256,20 +258,25 @@ def _start_server(self) -> None: try: self._server_process = subprocess.Popen(cmd) logger.info(f"Deno bridge server started (pid {self._server_process.pid})") - except FileNotFoundError: - logger.error("'deno' not found in PATH — install Deno or start the server manually") + except OSError as e: + logger.error(f"Failed to start Deno bridge: {e}") def _stop_server(self) -> None: """Terminate the Deno bridge server subprocess.""" - if self._server_process is None: + if self._server_process is None or self._server_process.poll() is not None: + self._server_process = None return - self._server_process.terminate() + logger.info("Stopping Deno bridge server", pid=self._server_process.pid) + self._server_process.send_signal(signal.SIGTERM) try: self._server_process.wait(timeout=3) except subprocess.TimeoutExpired: + logger.warning( + "Deno bridge did not exit, sending SIGKILL", pid=self._server_process.pid + ) self._server_process.kill() - self._server_process.wait(timeout=1) + self._server_process.wait(timeout=5) logger.info("Deno bridge server stopped") self._server_process = None From 91bfaf57a1c01d4db88d4e7367a45585e29e9137 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 00:25:05 -0800 Subject: [PATCH 12/30] Fix: pre-commit errors --- dimos/teleop/phone/phone_teleop_module.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 5e8c3ef394..ec522bf7db 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -354,7 +354,7 @@ def _get_output_twist(self) -> TwistStamped | None: def _publish_msg(self, output_msg: TwistStamped) -> None: """Publish twist command. - + Override to customize output (e.g., apply limits, remap axes). """ self.twist_output.publish(output_msg) From 5164357741bef1aad06b534f47dba608f5894e45 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 00:46:49 -0800 Subject: [PATCH 13/30] blueprints update --- dimos/robot/all_blueprints.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 92635773b5..0669b46411 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -98,7 +98,6 @@ "google_maps_skill": "dimos.agents.skills.google_maps_skill_container", "gps_nav_skill": "dimos.agents.skills.gps_nav_skill", "grasping_module": "dimos.manipulation.grasping.grasping", - "phone_teleop_module": "dimos.teleop.phone.phone_teleop_module", "joint_trajectory_controller": "dimos.manipulation.control.trajectory_controller.joint_trajectory_controller", "keyboard_teleop": "dimos.robot.unitree.keyboard_teleop", "manipulation_module": "dimos.manipulation.manipulation_module", @@ -109,11 +108,14 @@ "osm_skill": "dimos.agents.skills.osm", "person_follow_skill": "dimos.agents.skills.person_follow", "person_tracker_module": "dimos.perception.detection.person_tracker", + "phone_go2_teleop_module": "dimos.teleop.phone.phone_extensions", + "phone_teleop_module": "dimos.teleop.phone.phone_teleop_module", "quest_teleop_module": "dimos.teleop.quest.quest_teleop_module", "realsense_camera": "dimos.hardware.sensors.camera.realsense.camera", "replanning_a_star_planner": "dimos.navigation.replanning_a_star.module", "rerun_bridge": "dimos.visualization.rerun.bridge", "ros_nav": "dimos.navigation.rosnav", + "simple_phone_teleop_module": "dimos.teleop.phone.phone_extensions", "simulation": "dimos.simulation.manipulators.sim_module", "spatial_memory": "dimos.perception.spatial_perception", "speak_skill": "dimos.agents.skills.speak_skill", @@ -121,8 +123,6 @@ "twist_teleop_module": "dimos.teleop.quest.quest_extensions", "unitree_skills": "dimos.robot.unitree.unitree_skill_container", "utilization": "dimos.utils.monitoring", - "phone_go2_teleop_module": "dimos.teleop.phone.phone_extensions", - "simple_phone_teleop_module": "dimos.teleop.phone.phone_extensions", "visualizing_teleop_module": "dimos.teleop.quest.quest_extensions", "vlm_agent": "dimos.agents.vlm_agent", "vlm_stream_tester": "dimos.agents.vlm_stream_tester", From beb8fd1739a96df8e9a1b31ee3bf1866d83b7277 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 00:53:13 -0800 Subject: [PATCH 14/30] Fix: mypy errors --- dimos/teleop/phone/phone_teleop_module.py | 4 ++-- dimos/teleop/quest/quest_teleop_module.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index ec522bf7db..8003ebce3d 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -105,7 +105,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._control_loop_running = False # Deno bridge server - self._server_process: subprocess.Popen | None = None + self._server_process: subprocess.Popen[bytes] | None = None self._server_script = Path(__file__).parent / "web" / "teleop_server.ts" logger.info("PhoneTeleopModule initialized") @@ -119,7 +119,7 @@ def start(self) -> None: """Start the phone teleoperation module.""" super().start() - input_streams = { + input_streams: dict[str, tuple[Any, Any]] = { "phone_sensors": (self.phone_sensors, self._on_sensors), "phone_button": (self.phone_button, self._on_button), } diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index 3493169714..4b55b3377d 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -117,7 +117,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._control_loop_running = False # Deno bridge server - self._server_process: subprocess.Popen | None = None + self._server_process: subprocess.Popen[bytes] | None = None self._server_script = Path(__file__).parent / "web" / "teleop_server.ts" logger.info("QuestTeleopModule initialized") From df08e54b32b5511ec7957262194deb7804156adc Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 11:38:11 -0800 Subject: [PATCH 15/30] Fix: if doesnt die - check - greptile issue --- dimos/teleop/phone/phone_teleop_module.py | 5 ++++- dimos/teleop/quest/quest_teleop_module.py | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 8003ebce3d..91180b3c7c 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -238,7 +238,10 @@ def _stop_server(self) -> None: "Deno bridge did not exit, sending SIGKILL", pid=self._server_process.pid ) self._server_process.kill() - self._server_process.wait(timeout=5) + try: + self._server_process.wait(timeout=5) + except subprocess.TimeoutExpired: + logger.error("Deno bridge did not exit after SIGKILL") logger.info("Deno bridge server stopped") self._server_process = None diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index 4b55b3377d..961f4096ee 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -276,7 +276,10 @@ def _stop_server(self) -> None: "Deno bridge did not exit, sending SIGKILL", pid=self._server_process.pid ) self._server_process.kill() - self._server_process.wait(timeout=5) + try: + self._server_process.wait(timeout=5) + except subprocess.TimeoutExpired: + logger.error("Deno bridge did not exit after SIGKILL") logger.info("Deno bridge server stopped") self._server_process = None From 0c66ae5edb50dd28007117ca8afbb40aac1856ce Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 15:48:36 -0800 Subject: [PATCH 16/30] changed 'hold_to_teleop' button position + topic names --- dimos/teleop/phone/web/static/index.html | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/teleop/phone/web/static/index.html b/dimos/teleop/phone/web/static/index.html index c08fdcac6b..ae7cd18028 100644 --- a/dimos/teleop/phone/web/static/index.html +++ b/dimos/teleop/phone/web/static/index.html @@ -108,7 +108,7 @@ } #teleop-btn { - margin-top: auto; + margin-top: 12px; width: 100%; max-width: 400px; padding: 28px; @@ -185,8 +185,8 @@

Sensors

let wsSendInterval = null; // LCM channels - const SENSOR_CHANNEL = "/phone/sensors#geometry_msgs.TwistStamped"; - const BUTTON_CHANNEL = "/phone/button#std_msgs.Bool"; + const SENSOR_CHANNEL = "/phone_sensors#geometry_msgs.TwistStamped"; + const BUTTON_CHANNEL = "/phone_button#std_msgs.Bool"; // ============================================ // DOM From 4c9ffe564b5e6102cd1a1702cc434b21101d4e90 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 15:49:15 -0800 Subject: [PATCH 17/30] Misc: transports autoconnect (matching names) --- dimos/teleop/phone/blueprints.py | 21 +-------------------- 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/dimos/teleop/phone/blueprints.py b/dimos/teleop/phone/blueprints.py index 023dfcf5b1..13f0b6286f 100644 --- a/dimos/teleop/phone/blueprints.py +++ b/dimos/teleop/phone/blueprints.py @@ -13,42 +13,23 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Phone teleop blueprints for testing and deployment.""" - from dimos.core.blueprints import autoconnect -from dimos.core.transport import LCMTransport -from dimos.msgs.geometry_msgs import TwistStamped -from dimos.msgs.std_msgs.Bool import Bool from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic from dimos.teleop.phone.phone_extensions import ( phone_go2_teleop_module, simple_phone_teleop_module, ) -# ----------------------------------------------------------------------------- -# Phone Teleop Blueprints -# ----------------------------------------------------------------------------- # Simple phone teleop (mobile base axis filtering) simple_phone_teleop = autoconnect( simple_phone_teleop_module(), -).transports( - { - ("phone_sensors", TwistStamped): LCMTransport("/phone/sensors", TwistStamped), - ("phone_button", Bool): LCMTransport("/phone/button", Bool), - ("twist_output", TwistStamped): LCMTransport("/teleop/twist", TwistStamped), - } ) - + # Phone teleop wired to Unitree Go2 phone_go2_teleop = autoconnect( phone_go2_teleop_module(), unitree_go2_basic, -).transports( - { - ("phone_sensors", TwistStamped): LCMTransport("/phone/sensors", TwistStamped), - ("phone_button", Bool): LCMTransport("/phone/button", Bool), - } ) From db9ec28fa7df7d1111afb24da55d79efaa6dfe9a Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 15:50:32 -0800 Subject: [PATCH 18/30] Misc: remove redundant docstrings and logging --- dimos/teleop/phone/phone_extensions.py | 2 - dimos/teleop/phone/phone_teleop_module.py | 87 ++++++----------------- 2 files changed, 23 insertions(+), 66 deletions(-) diff --git a/dimos/teleop/phone/phone_extensions.py b/dimos/teleop/phone/phone_extensions.py index 684f37c27e..d34aff10bc 100644 --- a/dimos/teleop/phone/phone_extensions.py +++ b/dimos/teleop/phone/phone_extensions.py @@ -38,7 +38,6 @@ class SimplePhoneTeleop(PhoneTeleopModule): """ def _get_output_twist(self) -> TwistStamped | None: - """Extract mobile-base axes from raw twist.""" raw = super()._get_output_twist() if raw is None: return None @@ -64,7 +63,6 @@ class PhoneGo2Teleop(SimplePhoneTeleop): def _publish_msg(self, output_msg: TwistStamped) -> None: """Publish as Twist on cmd_vel to match GO2Connection. - Intentionally bypasses the base twist_output stream — only cmd_vel is used. """ self.cmd_vel.publish(Twist(linear=output_msg.linear, angular=output_msg.angular)) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 91180b3c7c..8487590578 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -18,12 +18,9 @@ Receives raw sensor data (TwistStamped) and button state (Bool) from the phone web app via the Deno LCM bridge. Computes orientation deltas from -a home orientation captured on engage, converts to TwistStamped velocity +a initial orientation captured on engage, converts to TwistStamped velocity commands via configurable gains, and publishes. -Raw sensor TwistStamped layout from browser: - linear = (roll, pitch, yaw) in degrees (DeviceOrientation) - angular = (gyro_x, gyro_y, gyro_z) in deg/s (DeviceMotion) """ from dataclasses import dataclass @@ -48,8 +45,6 @@ @dataclass class PhoneTeleopConfig(ModuleConfig): - """Configuration for Phone Teleoperation Module.""" - control_loop_hz: float = 50.0 # Gain: maps degrees of tilt to m/s. 30 deg tilt -> 1.0 m/s @@ -59,18 +54,15 @@ class PhoneTeleopConfig(ModuleConfig): class PhoneTeleopModule(Module[PhoneTeleopConfig], TeleopProtocol): - """Phone Teleoperation Module. - + """ Receives raw sensor data from the phone web app: - TwistStamped: linear=(roll, pitch, yaw) deg, angular=(gyro) deg/s - Bool: teleop button state (True = held) - On engage (button pressed), captures the current orientation as home. - Each control-loop tick computes the orientation delta from home, + On engage (button pressed), captures the current orientation as initial. + Each control-loop tick computes the orientation delta from initial, converts it to a TwistStamped via configurable gains, and publishes. - Implements TeleopProtocol. - Outputs: - twist_output: TwistStamped (velocity command for robot) """ @@ -80,7 +72,6 @@ class PhoneTeleopModule(Module[PhoneTeleopConfig], TeleopProtocol): # Inputs from Deno bridge phone_sensors: In[TwistStamped] phone_button: In[Bool] - # Output: velocity command to robot twist_output: Out[TwistStamped] @@ -93,11 +84,8 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._is_engaged: bool = False self._teleop_button: bool = False - - # Raw sensor messages (like Quest stores PoseStamped) self._current_sensors: TwistStamped | None = None - self._home_sensors: TwistStamped | None = None - + self._initial_sensors: TwistStamped | None = None self._lock = threading.RLock() # Control loop @@ -108,53 +96,35 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._server_process: subprocess.Popen[bytes] | None = None self._server_script = Path(__file__).parent / "web" / "teleop_server.ts" - logger.info("PhoneTeleopModule initialized") - # ------------------------------------------------------------------------- # Public RPC Methods # ------------------------------------------------------------------------- @rpc def start(self) -> None: - """Start the phone teleoperation module.""" super().start() - - input_streams: dict[str, tuple[Any, Any]] = { - "phone_sensors": (self.phone_sensors, self._on_sensors), - "phone_button": (self.phone_button, self._on_button), + input_streams: dict[tuple[Any, Any]] = { + (self.phone_sensors, self._on_sensors), + (self.phone_button, self._on_button), } - connected = [] - for name, (stream, handler) in input_streams.items(): - if not (stream and stream.transport): # type: ignore[attr-defined] - logger.warning(f"Stream '{name}' has no transport — skipping") - continue + for stream, handler in input_streams: self._disposables.add(Disposable(stream.subscribe(handler))) # type: ignore[attr-defined] - connected.append(name) - - if connected: - logger.info(f"Subscribed to: {', '.join(connected)}") - self._start_server() self._start_control_loop() - logger.info("Phone Teleoperation Module started") @rpc def stop(self) -> None: - """Stop the phone teleoperation module.""" - logger.info("Stopping Phone Teleoperation Module...") self._stop_control_loop() self._stop_server() super().stop() @rpc - def engage(self, hand: Any = None) -> bool: - """Engage teleoperation.""" + def engage(self, hand: Any = None) -> bool: # TeleopProtocol signature with self._lock: return self._engage() @rpc - def disengage(self, hand: Any = None) -> None: - """Disengage teleoperation.""" + def disengage(self, hand: Any = None) -> None: # TeleopProtocol signature with self._lock: self._disengage() @@ -163,19 +133,19 @@ def disengage(self, hand: Any = None) -> None: # ------------------------------------------------------------------------- def _engage(self) -> bool: - """Engage: capture current sensors as home. Assumes lock held.""" + """Engage: capture current sensors as initial""" if self._current_sensors is None: logger.error("Engage failed: no sensor data yet") return False - self._home_sensors = self._current_sensors + self._initial_sensors = self._current_sensors self._is_engaged = True logger.info("Phone teleop engaged") return True def _disengage(self) -> None: - """Disengage: stop publishing. Assumes lock held.""" + """Disengage: stop publishing""" self._is_engaged = False - self._home_sensors = None + self._initial_sensors = None logger.info("Phone teleop disengaged") # ------------------------------------------------------------------------- @@ -183,11 +153,7 @@ def _disengage(self) -> None: # ------------------------------------------------------------------------- def _on_sensors(self, msg: TwistStamped) -> None: - """Callback for raw sensor TwistStamped from the phone web app. - - linear = (roll, pitch, yaw) in degrees - angular = (gyro_x, gyro_y, gyro_z) in deg/s - """ + """Callback for raw sensor TwistStamped from the phone""" with self._lock: self._current_sensors = msg @@ -250,7 +216,6 @@ def _stop_server(self) -> None: # ------------------------------------------------------------------------- def _start_control_loop(self) -> None: - """Start the control loop thread.""" if self._control_loop_running: return @@ -264,7 +229,6 @@ def _start_control_loop(self) -> None: logger.info(f"Control loop started at {self.config.control_loop_hz} Hz") def _stop_control_loop(self) -> None: - """Stop the control loop thread.""" self._control_loop_running = False if self._control_loop_thread is not None: self._control_loop_thread.join(timeout=1.0) @@ -272,7 +236,6 @@ def _stop_control_loop(self) -> None: logger.info("Control loop stopped") def _control_loop(self) -> None: - """Main control loop: handle engagement, compute deltas, publish twist""" period = 1.0 / self.config.control_loop_hz while self._control_loop_running: @@ -298,8 +261,7 @@ def _control_loop(self) -> None: # ------------------------------------------------------------------------- def _handle_engage(self) -> None: - """Check button state and engage/disengage accordingly. - + """ Override to customize engagement logic. Default: button hold = engaged, release = disengaged. """ @@ -311,8 +273,7 @@ def _handle_engage(self) -> None: self._disengage() def _should_publish(self) -> bool: - """Check if we should publish a twist command. - + """ Override to add custom conditions. Default: Returns True if engaged. """ @@ -321,15 +282,14 @@ def _should_publish(self) -> bool: def _get_output_twist(self) -> TwistStamped | None: """Compute twist from orientation delta. Override to customize twist computation (e.g., apply scaling, filtering). - Default: Computes delta angles from home orientation, applies gains. + Default: Computes delta angles from initial orientation, applies gains. """ current = self._current_sensors - home = self._home_sensors - if current is None or home is None: + initial = self._initial_sensors + if current is None or initial is None: return None - # Subtract as Twist (drops ts/frame_id, returns Twist) - delta: Twist = Twist(current) - Twist(home) + delta: Twist = Twist(current) - Twist(initial) # Handle yaw wraparound (linear.z = yaw, 0-360 degrees) d_yaw = delta.linear.z @@ -356,8 +316,7 @@ def _get_output_twist(self) -> TwistStamped | None: ) def _publish_msg(self, output_msg: TwistStamped) -> None: - """Publish twist command. - + """ Override to customize output (e.g., apply limits, remap axes). """ self.twist_output.publish(output_msg) From 7642d09bd9c97ebc411044bf498db5799233f30f Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 16:02:47 -0800 Subject: [PATCH 19/30] Fix: Module works for any robot that takes twist input --- dimos/robot/all_blueprints.py | 2 +- dimos/teleop/phone/__init__.py | 8 ++++---- dimos/teleop/phone/blueprints.py | 4 ++-- dimos/teleop/phone/phone_extensions.py | 16 ++++++++-------- 4 files changed, 15 insertions(+), 15 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 0669b46411..3bdb2ebd76 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -108,7 +108,7 @@ "osm_skill": "dimos.agents.skills.osm", "person_follow_skill": "dimos.agents.skills.person_follow", "person_tracker_module": "dimos.perception.detection.person_tracker", - "phone_go2_teleop_module": "dimos.teleop.phone.phone_extensions", + "phone_twist_teleop_module": "dimos.teleop.phone.phone_extensions", "phone_teleop_module": "dimos.teleop.phone.phone_teleop_module", "quest_teleop_module": "dimos.teleop.quest.quest_teleop_module", "realsense_camera": "dimos.hardware.sensors.camera.realsense.camera", diff --git a/dimos/teleop/phone/__init__.py b/dimos/teleop/phone/__init__.py index 053bcc7a53..9b5cf1d8b1 100644 --- a/dimos/teleop/phone/__init__.py +++ b/dimos/teleop/phone/__init__.py @@ -15,9 +15,9 @@ """Phone teleoperation module for DimOS.""" from dimos.teleop.phone.phone_extensions import ( - PhoneGo2Teleop, + PhoneTwistTeleop, SimplePhoneTeleop, - phone_go2_teleop_module, + phone_twist_teleop_module, simple_phone_teleop_module, ) from dimos.teleop.phone.phone_teleop_module import ( @@ -27,11 +27,11 @@ ) __all__ = [ - "PhoneGo2Teleop", + "PhoneTwistTeleop", "PhoneTeleopConfig", "PhoneTeleopModule", "SimplePhoneTeleop", - "phone_go2_teleop_module", + "phone_twist_teleop_module", "phone_teleop_module", "simple_phone_teleop_module", ] diff --git a/dimos/teleop/phone/blueprints.py b/dimos/teleop/phone/blueprints.py index 13f0b6286f..e072fe3c73 100644 --- a/dimos/teleop/phone/blueprints.py +++ b/dimos/teleop/phone/blueprints.py @@ -16,7 +16,7 @@ from dimos.core.blueprints import autoconnect from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic from dimos.teleop.phone.phone_extensions import ( - phone_go2_teleop_module, + phone_twist_teleop_module, simple_phone_teleop_module, ) @@ -28,7 +28,7 @@ # Phone teleop wired to Unitree Go2 phone_go2_teleop = autoconnect( - phone_go2_teleop_module(), + phone_twist_teleop_module(), unitree_go2_basic, ) diff --git a/dimos/teleop/phone/phone_extensions.py b/dimos/teleop/phone/phone_extensions.py index d34aff10bc..21cdfff51d 100644 --- a/dimos/teleop/phone/phone_extensions.py +++ b/dimos/teleop/phone/phone_extensions.py @@ -17,7 +17,7 @@ Available subclasses: - SimplePhoneTeleop: Maps raw twist to ground robot axes (linear.x, linear.y, angular.z) - - PhoneGo2Teleop: Go2-specific extension, outputs cmd_vel: Out[Twist] for direct Go2 wiring + - PhoneTwistTeleop: Outputs cmd_vel: Out[Twist] for direct autoconnect with any Twist consumer """ from dimos.core import Out @@ -52,28 +52,28 @@ def _get_output_twist(self) -> TwistStamped | None: ) -class PhoneGo2Teleop(SimplePhoneTeleop): - """Phone teleop for Unitree Go2. +class PhoneTwistTeleop(SimplePhoneTeleop): + """Phone teleop that outputs Twist on cmd_vel. Extends SimplePhoneTeleop with cmd_vel: Out[Twist] - that matches GO2Connection.cmd_vel: In[Twist] for direct autoconnect wiring. + for direct autoconnect wiring with any module that has cmd_vel: In[Twist]. """ cmd_vel: Out[Twist] def _publish_msg(self, output_msg: TwistStamped) -> None: - """Publish as Twist on cmd_vel to match GO2Connection. + """Publish as Twist on cmd_vel. Intentionally bypasses the base twist_output stream — only cmd_vel is used. """ self.cmd_vel.publish(Twist(linear=output_msg.linear, angular=output_msg.angular)) simple_phone_teleop_module = SimplePhoneTeleop.blueprint -phone_go2_teleop_module = PhoneGo2Teleop.blueprint +phone_twist_teleop_module = PhoneTwistTeleop.blueprint __all__ = [ - "PhoneGo2Teleop", + "PhoneTwistTeleop", "SimplePhoneTeleop", - "phone_go2_teleop_module", + "phone_twist_teleop_module", "simple_phone_teleop_module", ] From f2613dad0a3772983b55343784d4956b8156174c Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 17:19:56 -0800 Subject: [PATCH 20/30] Fix: add threading --- dimos/teleop/phone/phone_teleop_module.py | 12 ++++++------ dimos/teleop/quest/quest_teleop_module.py | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 8487590578..515308339d 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -90,7 +90,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Control loop self._control_loop_thread: threading.Thread | None = None - self._control_loop_running = False + self._stop_event = threading.Event() # Deno bridge server self._server_process: subprocess.Popen[bytes] | None = None @@ -216,10 +216,10 @@ def _stop_server(self) -> None: # ------------------------------------------------------------------------- def _start_control_loop(self) -> None: - if self._control_loop_running: + if self._control_loop_thread is not None and self._control_loop_thread.is_alive(): return - self._control_loop_running = True + self._stop_event.clear() self._control_loop_thread = threading.Thread( target=self._control_loop, daemon=True, @@ -229,7 +229,7 @@ def _start_control_loop(self) -> None: logger.info(f"Control loop started at {self.config.control_loop_hz} Hz") def _stop_control_loop(self) -> None: - self._control_loop_running = False + self._stop_event.set() if self._control_loop_thread is not None: self._control_loop_thread.join(timeout=1.0) self._control_loop_thread = None @@ -238,7 +238,7 @@ def _stop_control_loop(self) -> None: def _control_loop(self) -> None: period = 1.0 / self.config.control_loop_hz - while self._control_loop_running: + while not self._stop_event.is_set(): loop_start = time.perf_counter() try: with self._lock: @@ -254,7 +254,7 @@ def _control_loop(self) -> None: elapsed = time.perf_counter() - loop_start sleep_time = period - elapsed if sleep_time > 0: - time.sleep(sleep_time) + self._stop_event.wait(sleep_time) # ------------------------------------------------------------------------- # Control Loop Internal Methods diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index 961f4096ee..ae04673373 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -114,7 +114,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Control loop self._control_loop_thread: threading.Thread | None = None - self._control_loop_running = False + self._stop_event = threading.Event() # Deno bridge server self._server_process: subprocess.Popen[bytes] | None = None @@ -285,10 +285,10 @@ def _stop_server(self) -> None: def _start_control_loop(self) -> None: """Start the control loop thread.""" - if self._control_loop_running: + if self._control_loop_thread is not None and self._control_loop_thread.is_alive(): return - self._control_loop_running = True + self._stop_event.clear() self._control_loop_thread = threading.Thread( target=self._control_loop, daemon=True, @@ -299,7 +299,7 @@ def _start_control_loop(self) -> None: def _stop_control_loop(self) -> None: """Stop the control loop thread.""" - self._control_loop_running = False + self._stop_event.set() if self._control_loop_thread is not None: self._control_loop_thread.join(timeout=1.0) self._control_loop_thread = None @@ -313,7 +313,7 @@ def _control_loop(self) -> None: """ period = 1.0 / self.config.control_loop_hz - while self._control_loop_running: + while not self._stop_event.is_set(): loop_start = time.perf_counter() try: with self._lock: @@ -337,7 +337,7 @@ def _control_loop(self) -> None: elapsed = time.perf_counter() - loop_start sleep_time = period - elapsed if sleep_time > 0: - time.sleep(sleep_time) + self._stop_event.wait(sleep_time) # ------------------------------------------------------------------------- # Control Loop Internals From 1b5aaee025d252d9fa0b1c1a4f90cde28d352000 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 17:20:26 -0800 Subject: [PATCH 21/30] Fix: pre-commit errors --- dimos/teleop/phone/__init__.py | 4 ++-- dimos/teleop/phone/blueprints.py | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/dimos/teleop/phone/__init__.py b/dimos/teleop/phone/__init__.py index 9b5cf1d8b1..f41ce41cdf 100644 --- a/dimos/teleop/phone/__init__.py +++ b/dimos/teleop/phone/__init__.py @@ -27,11 +27,11 @@ ) __all__ = [ - "PhoneTwistTeleop", "PhoneTeleopConfig", "PhoneTeleopModule", + "PhoneTwistTeleop", "SimplePhoneTeleop", - "phone_twist_teleop_module", "phone_teleop_module", + "phone_twist_teleop_module", "simple_phone_teleop_module", ] diff --git a/dimos/teleop/phone/blueprints.py b/dimos/teleop/phone/blueprints.py index e072fe3c73..346df3af4f 100644 --- a/dimos/teleop/phone/blueprints.py +++ b/dimos/teleop/phone/blueprints.py @@ -20,12 +20,11 @@ simple_phone_teleop_module, ) - # Simple phone teleop (mobile base axis filtering) simple_phone_teleop = autoconnect( simple_phone_teleop_module(), ) - + # Phone teleop wired to Unitree Go2 phone_go2_teleop = autoconnect( phone_twist_teleop_module(), From 068b47a3b92b6281ab1cc8bfb681c9321074de4f Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 17:41:34 -0800 Subject: [PATCH 22/30] Fix: updated to single subclass --- dimos/robot/all_blueprints.py | 3 +- dimos/teleop/phone/__init__.py | 4 -- dimos/teleop/phone/blueprints.py | 9 ++--- dimos/teleop/phone/phone_extensions.py | 53 +++++++------------------- 4 files changed, 17 insertions(+), 52 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 3bdb2ebd76..8ffdff451d 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -108,14 +108,13 @@ "osm_skill": "dimos.agents.skills.osm", "person_follow_skill": "dimos.agents.skills.person_follow", "person_tracker_module": "dimos.perception.detection.person_tracker", - "phone_twist_teleop_module": "dimos.teleop.phone.phone_extensions", + "simple_phone_teleop_module": "dimos.teleop.phone.phone_extensions", "phone_teleop_module": "dimos.teleop.phone.phone_teleop_module", "quest_teleop_module": "dimos.teleop.quest.quest_teleop_module", "realsense_camera": "dimos.hardware.sensors.camera.realsense.camera", "replanning_a_star_planner": "dimos.navigation.replanning_a_star.module", "rerun_bridge": "dimos.visualization.rerun.bridge", "ros_nav": "dimos.navigation.rosnav", - "simple_phone_teleop_module": "dimos.teleop.phone.phone_extensions", "simulation": "dimos.simulation.manipulators.sim_module", "spatial_memory": "dimos.perception.spatial_perception", "speak_skill": "dimos.agents.skills.speak_skill", diff --git a/dimos/teleop/phone/__init__.py b/dimos/teleop/phone/__init__.py index f41ce41cdf..552032a47b 100644 --- a/dimos/teleop/phone/__init__.py +++ b/dimos/teleop/phone/__init__.py @@ -15,9 +15,7 @@ """Phone teleoperation module for DimOS.""" from dimos.teleop.phone.phone_extensions import ( - PhoneTwistTeleop, SimplePhoneTeleop, - phone_twist_teleop_module, simple_phone_teleop_module, ) from dimos.teleop.phone.phone_teleop_module import ( @@ -29,9 +27,7 @@ __all__ = [ "PhoneTeleopConfig", "PhoneTeleopModule", - "PhoneTwistTeleop", "SimplePhoneTeleop", "phone_teleop_module", - "phone_twist_teleop_module", "simple_phone_teleop_module", ] diff --git a/dimos/teleop/phone/blueprints.py b/dimos/teleop/phone/blueprints.py index 346df3af4f..6328af8612 100644 --- a/dimos/teleop/phone/blueprints.py +++ b/dimos/teleop/phone/blueprints.py @@ -15,19 +15,16 @@ from dimos.core.blueprints import autoconnect from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic -from dimos.teleop.phone.phone_extensions import ( - phone_twist_teleop_module, - simple_phone_teleop_module, -) +from dimos.teleop.phone.phone_extensions import simple_phone_teleop_module -# Simple phone teleop (mobile base axis filtering) +# Simple phone teleop (mobile base axis filtering + cmd_vel output) simple_phone_teleop = autoconnect( simple_phone_teleop_module(), ) # Phone teleop wired to Unitree Go2 phone_go2_teleop = autoconnect( - phone_twist_teleop_module(), + simple_phone_teleop_module(), unitree_go2_basic, ) diff --git a/dimos/teleop/phone/phone_extensions.py b/dimos/teleop/phone/phone_extensions.py index 21cdfff51d..cf8638e71f 100644 --- a/dimos/teleop/phone/phone_extensions.py +++ b/dimos/teleop/phone/phone_extensions.py @@ -13,11 +13,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Phone teleop module extensions and subclasses. +"""Phone teleop module extensions. Available subclasses: - - SimplePhoneTeleop: Maps raw twist to ground robot axes (linear.x, linear.y, angular.z) - - PhoneTwistTeleop: Outputs cmd_vel: Out[Twist] for direct autoconnect with any Twist consumer + - SimplePhoneTeleop: Filters to ground robot axes and outputs cmd_vel: Out[Twist] """ from dimos.core import Out @@ -26,54 +25,28 @@ class SimplePhoneTeleop(PhoneTeleopModule): - """Phone teleop mapped to ground robot axes. + """Phone teleop for ground robots. - Takes the raw 6-axis twist from the base module and extracts only the - axes relevant for a mobile base: - - linear.x (forward/back from pitch tilt) - - linear.y (strafe from roll tilt) - - angular.z (turn from yaw orientation delta) - - All other axes are zeroed. - """ - - def _get_output_twist(self) -> TwistStamped | None: - raw = super()._get_output_twist() - if raw is None: - return None - - # raw.linear.z is the yaw orientation delta (already scaled by linear_gain - # in the base class), remapped here to angular.z for ground-robot turning. - return TwistStamped( - ts=raw.ts, - frame_id="mobile_base", - linear=Vector3(x=raw.linear.x, y=raw.linear.y, z=0.0), - angular=Vector3(x=0.0, y=0.0, z=raw.linear.z), - ) - - -class PhoneTwistTeleop(SimplePhoneTeleop): - """Phone teleop that outputs Twist on cmd_vel. - - Extends SimplePhoneTeleop with cmd_vel: Out[Twist] - for direct autoconnect wiring with any module that has cmd_vel: In[Twist]. + Filters the raw 6-axis twist to mobile base axes (linear.x, linear.y, angular.z) + and publishes as Twist on cmd_vel for direct autoconnect wiring with any + module that has cmd_vel: In[Twist]. """ cmd_vel: Out[Twist] def _publish_msg(self, output_msg: TwistStamped) -> None: - """Publish as Twist on cmd_vel. - Intentionally bypasses the base twist_output stream — only cmd_vel is used. - """ - self.cmd_vel.publish(Twist(linear=output_msg.linear, angular=output_msg.angular)) + """Filter to ground robot axes and publish as Twist on cmd_vel.""" + self.cmd_vel.publish( + Twist( + linear=Vector3(x=output_msg.linear.x, y=output_msg.linear.y, z=0.0), + angular=Vector3(x=0.0, y=0.0, z=output_msg.linear.z), + ) + ) simple_phone_teleop_module = SimplePhoneTeleop.blueprint -phone_twist_teleop_module = PhoneTwistTeleop.blueprint __all__ = [ - "PhoneTwistTeleop", "SimplePhoneTeleop", - "phone_twist_teleop_module", "simple_phone_teleop_module", ] From f0c5e7a4da6efc4b95544eeae4382f83e828d63d Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 21:38:24 -0800 Subject: [PATCH 23/30] Fix: blueprints order --- dimos/robot/all_blueprints.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 8ffdff451d..001efcf845 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -108,13 +108,13 @@ "osm_skill": "dimos.agents.skills.osm", "person_follow_skill": "dimos.agents.skills.person_follow", "person_tracker_module": "dimos.perception.detection.person_tracker", - "simple_phone_teleop_module": "dimos.teleop.phone.phone_extensions", "phone_teleop_module": "dimos.teleop.phone.phone_teleop_module", "quest_teleop_module": "dimos.teleop.quest.quest_teleop_module", "realsense_camera": "dimos.hardware.sensors.camera.realsense.camera", "replanning_a_star_planner": "dimos.navigation.replanning_a_star.module", "rerun_bridge": "dimos.visualization.rerun.bridge", "ros_nav": "dimos.navigation.rosnav", + "simple_phone_teleop_module": "dimos.teleop.phone.phone_extensions", "simulation": "dimos.simulation.manipulators.sim_module", "spatial_memory": "dimos.perception.spatial_perception", "speak_skill": "dimos.agents.skills.speak_skill", From 94c198a591fb73a1c4485ea8b6da7cbdb4cae5e3 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Tue, 17 Feb 2026 22:31:40 -0800 Subject: [PATCH 24/30] Fix: mypy errors --- dimos/teleop/phone/phone_teleop_module.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 515308339d..fe3dd5a6bb 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -103,11 +103,10 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: @rpc def start(self) -> None: super().start() - input_streams: dict[tuple[Any, Any]] = { + for stream, handler in ( (self.phone_sensors, self._on_sensors), (self.phone_button, self._on_button), - } - for stream, handler in input_streams: + ): self._disposables.add(Disposable(stream.subscribe(handler))) # type: ignore[attr-defined] self._start_server() self._start_control_loop() From e85918749a023edd0c243512f9b0f4ee7a30adaf Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 18 Feb 2026 11:23:51 -0800 Subject: [PATCH 25/30] Feat: deno check --- dimos/teleop/phone/phone_teleop_module.py | 7 +++++++ dimos/teleop/quest/quest_teleop_module.py | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index fe3dd5a6bb..e10547b424 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -25,6 +25,7 @@ from dataclasses import dataclass from pathlib import Path +import shutil import signal import subprocess import threading @@ -171,6 +172,12 @@ def _start_server(self) -> None: logger.warning("Deno bridge already running", pid=self._server_process.pid) return + if shutil.which("deno") is None: + logger.error( + "Deno is not installed. Install it with: curl -fsSL https://deno.land/install.sh | sh" + ) + return + script = str(self._server_script) cmd = [ "deno", diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index ae04673373..b238d98b9e 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -23,6 +23,7 @@ from dataclasses import dataclass from enum import IntEnum from pathlib import Path +import shutil import signal import subprocess import threading @@ -244,6 +245,12 @@ def _start_server(self) -> None: logger.warning("Deno bridge already running", pid=self._server_process.pid) return + if shutil.which("deno") is None: + logger.error( + "Deno is not installed. Install it with: curl -fsSL https://deno.land/install.sh | sh" + ) + return + script = str(self._server_script) cmd = [ "deno", From 4fc9ff9641eef2a7da72264f860f51d6f7906e78 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 18 Feb 2026 13:23:41 -0800 Subject: [PATCH 26/30] Fix: remove base protocol --- dimos/teleop/README.md | 9 ++++- dimos/teleop/__init__.py | 4 -- dimos/teleop/base/__init__.py | 19 --------- dimos/teleop/base/teleop_protocol.py | 58 ---------------------------- dimos/teleop/phone/README.md | 12 ++---- 5 files changed, 11 insertions(+), 91 deletions(-) delete mode 100644 dimos/teleop/base/__init__.py delete mode 100644 dimos/teleop/base/teleop_protocol.py diff --git a/dimos/teleop/README.md b/dimos/teleop/README.md index 3c29790cc5..e64a7b43ea 100644 --- a/dimos/teleop/README.md +++ b/dimos/teleop/README.md @@ -56,8 +56,6 @@ Adds Rerun visualization for debugging. Extends ArmTeleopModule (toggle engage). ``` teleop/ -├── base/ -│ └── teleop_protocol.py # TeleopProtocol interface ├── quest/ │ ├── quest_teleop_module.py # Base Quest teleop module │ ├── quest_extensions.py # ArmTeleop, TwistTeleop, VisualizingTeleop @@ -65,6 +63,13 @@ teleop/ │ └── web/ # Deno bridge + WebXR client │ ├── teleop_server.ts │ └── static/index.html +├── phone/ +│ ├── phone_teleop_module.py # Base Phone teleop module +│ ├── phone_extensions.py # SimplePhoneTeleop +│ ├── blueprints.py # Pre-wired configurations +│ └── web/ # Deno bridge + mobile web app +│ ├── teleop_server.ts +│ └── static/index.html ├── utils/ │ ├── teleop_transforms.py # WebXR → robot frame math │ └── teleop_visualization.py # Rerun visualization helpers diff --git a/dimos/teleop/__init__.py b/dimos/teleop/__init__.py index a8c3c0b21a..8324113111 100644 --- a/dimos/teleop/__init__.py +++ b/dimos/teleop/__init__.py @@ -13,7 +13,3 @@ # limitations under the License. """Teleoperation modules for DimOS.""" - -from dimos.teleop.base import TeleopProtocol - -__all__ = ["TeleopProtocol"] diff --git a/dimos/teleop/base/__init__.py b/dimos/teleop/base/__init__.py deleted file mode 100644 index cf3b18d597..0000000000 --- a/dimos/teleop/base/__init__.py +++ /dev/null @@ -1,19 +0,0 @@ -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Teleoperation protocol.""" - -from dimos.teleop.base.teleop_protocol import TeleopProtocol - -__all__ = ["TeleopProtocol"] diff --git a/dimos/teleop/base/teleop_protocol.py b/dimos/teleop/base/teleop_protocol.py deleted file mode 100644 index 9e1647d64d..0000000000 --- a/dimos/teleop/base/teleop_protocol.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025-2026 Dimensional Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Teleoperation specifications: Protocol. - -Defines the interface that all teleoperation modules must implement. -No implementation - just method signatures. -""" - -from typing import Any, Protocol, runtime_checkable - -# ============================================================================ -# TELEOP PROTOCOL -# ============================================================================ - - -@runtime_checkable -class TeleopProtocol(Protocol): - """Protocol defining the teleoperation interface. - - All teleop modules (Quest, keyboard, joystick, etc.) should implement these methods. - No state or implementation here - just the contract. - """ - - # --- Lifecycle --- - - def start(self) -> None: - """Start the teleoperation module.""" - ... - - def stop(self) -> None: - """Stop the teleoperation module.""" - ... - - # --- Engage / Disengage --- - - def engage(self, hand: Any = None) -> bool: - """Engage teleoperation. Hand type is device-specific (e.g., Hand enum for Quest).""" - ... - - def disengage(self, hand: Any = None) -> None: - """Disengage teleoperation. Hand type is device-specific.""" - ... - - -__all__ = ["TeleopProtocol"] diff --git a/dimos/teleop/phone/README.md b/dimos/teleop/phone/README.md index 5052ef6f8c..dd2af02281 100644 --- a/dimos/teleop/phone/README.md +++ b/dimos/teleop/phone/README.md @@ -26,10 +26,7 @@ TwistStamped / Twist outputs Base module. Receives raw sensor data and button state. On engage (button hold), captures home orientation and publishes deltas as TwistStamped. Launches the Deno bridge server automatically. ### SimplePhoneTeleop -Filters to mobile-base axes: pitch -> linear.x, roll -> linear.y, yaw -> angular.z. - -### PhoneGo2Teleop -Extends SimplePhoneTeleop. Adds `cmd_vel: Out[Twist]` for direct Go2 autoconnect wiring. +Filters to mobile-base axes (linear.x, linear.y, angular.z) and publishes as `Twist` on `cmd_vel` for direct autoconnect wiring with any module that has `cmd_vel: In[Twist]`. ## Subclassing @@ -39,7 +36,6 @@ Override these methods: |--------|---------| | `_handle_engage()` | Customize engage/disengage logic | | `_should_publish()` | Add conditions for when to publish | -| `_get_output_twist()` | Customize twist computation | | `_publish_msg()` | Change output format | **Do not acquire `self._lock` in overrides.** The control loop already holds it. @@ -48,8 +44,8 @@ Override these methods: | Topic | Type | Description | |-------|------|-------------| -| `/phone/sensors` | TwistStamped | linear=(roll,pitch,yaw) deg, angular=(gyro) deg/s | -| `/phone/button` | Bool | Teleop engage button (1=held) | +| `/phone_sensors` | TwistStamped | linear=(roll,pitch,yaw) deg, angular=(gyro) deg/s | +| `/phone_button` | Bool | Teleop engage button (1=held) | | `/teleop/twist` | TwistStamped | Output velocity command | ## Running @@ -66,7 +62,7 @@ Server starts on port `8444`. Open `https://:8444` on phone, accept the ``` phone/ ├── phone_teleop_module.py # Base phone teleop module -├── phone_extensions.py # SimplePhoneTeleop, PhoneGo2Teleop +├── phone_extensions.py # SimplePhoneTeleop ├── blueprints.py # Pre-wired configurations └── web/ ├── teleop_server.ts # Deno WSS-to-LCM bridge From ec4eb16b8924dc556b038ee091eafd44eaf9b7d9 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 18 Feb 2026 13:25:17 -0800 Subject: [PATCH 27/30] Feat: no protocol, no rpc calls, comments updated --- dimos/teleop/phone/phone_teleop_module.py | 39 +++++------------------ dimos/teleop/quest/quest_teleop_module.py | 30 ++--------------- 2 files changed, 11 insertions(+), 58 deletions(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index e10547b424..37763d2f4d 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -34,11 +34,10 @@ from reactivex.disposable import Disposable -from dimos.core import In, Module, Out, rpc +from dimos.core import In, Module, Out from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Twist, TwistStamped, Vector3 from dimos.msgs.std_msgs.Bool import Bool -from dimos.teleop.base import TeleopProtocol from dimos.utils.logging_config import setup_logger logger = setup_logger() @@ -54,7 +53,7 @@ class PhoneTeleopConfig(ModuleConfig): angular_gain: float = 1.0 / 30.0 -class PhoneTeleopModule(Module[PhoneTeleopConfig], TeleopProtocol): +class PhoneTeleopModule(Module[PhoneTeleopConfig]): """ Receives raw sensor data from the phone web app: - TwistStamped: linear=(roll, pitch, yaw) deg, angular=(gyro) deg/s @@ -101,7 +100,6 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Public RPC Methods # ------------------------------------------------------------------------- - @rpc def start(self) -> None: super().start() for stream, handler in ( @@ -112,22 +110,11 @@ def start(self) -> None: self._start_server() self._start_control_loop() - @rpc def stop(self) -> None: self._stop_control_loop() self._stop_server() super().stop() - @rpc - def engage(self, hand: Any = None) -> bool: # TeleopProtocol signature - with self._lock: - return self._engage() - - @rpc - def disengage(self, hand: Any = None) -> None: # TeleopProtocol signature - with self._lock: - self._disengage() - # ------------------------------------------------------------------------- # Internal engage / disengage (assumes lock is held) # ------------------------------------------------------------------------- @@ -246,16 +233,13 @@ def _control_loop(self) -> None: while not self._stop_event.is_set(): loop_start = time.perf_counter() - try: - with self._lock: - self._handle_engage() + with self._lock: + self._handle_engage() - if self._should_publish(): - output_twist = self._get_output_twist() - if output_twist is not None: - self._publish_msg(output_twist) - except Exception: - logger.exception("Error in phone teleop control loop") + if self._is_engaged: + output_twist = self._get_output_twist() + if output_twist is not None: + self._publish_msg(output_twist) elapsed = time.perf_counter() - loop_start sleep_time = period - elapsed @@ -278,13 +262,6 @@ def _handle_engage(self) -> None: if self._is_engaged: self._disengage() - def _should_publish(self) -> bool: - """ - Override to add custom conditions. - Default: Returns True if engaged. - """ - return self._is_engaged - def _get_output_twist(self) -> TwistStamped | None: """Compute twist from orientation delta. Override to customize twist computation (e.g., apply scaling, filtering). diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index b238d98b9e..b5ab6c2303 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -32,11 +32,10 @@ from reactivex.disposable import Disposable -from dimos.core import In, Module, Out, rpc +from dimos.core import In, Module, Out from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import Joy -from dimos.teleop.base import TeleopProtocol from dimos.teleop.quest.quest_types import Buttons, QuestControllerState from dimos.teleop.utils.teleop_transforms import webxr_to_robot from dimos.utils.logging_config import setup_logger @@ -69,7 +68,7 @@ class QuestTeleopConfig(ModuleConfig): control_loop_hz: float = 50.0 -class QuestTeleopModule(Module[QuestTeleopConfig], TeleopProtocol): +class QuestTeleopModule(Module[QuestTeleopConfig]): """Quest Teleoperation Module for Meta Quest controllers. Gets controller data from Deno bridge, computes output poses, and publishes them. Subclass to customize pose @@ -121,15 +120,11 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._server_process: subprocess.Popen[bytes] | None = None self._server_script = Path(__file__).parent / "web" / "teleop_server.ts" - logger.info("QuestTeleopModule initialized") - # ------------------------------------------------------------------------- # Public RPC Methods # ------------------------------------------------------------------------- - @rpc def start(self) -> None: - """Start the Quest teleoperation module.""" super().start() input_streams = { @@ -150,29 +145,13 @@ def start(self) -> None: logger.info(f"Subscribed to: {', '.join(connected)}") self._start_server() - self._start_control_loop() logger.info("Quest Teleoperation Module started") - @rpc def stop(self) -> None: - """Stop the Quest teleoperation module.""" - logger.info("Stopping Quest Teleoperation Module...") self._stop_control_loop() self._stop_server() super().stop() - @rpc - def engage(self, hand: Hand | None = None) -> bool: - """Engage teleoperation for a hand. If hand is None, engage both.""" - with self._lock: - return self._engage(hand) - - @rpc - def disengage(self, hand: Hand | None = None) -> None: - """Disengage teleoperation for a hand. If hand is None, disengage both.""" - with self._lock: - self._disengage(hand) - # ------------------------------------------------------------------------- # Internal engage/disengage (assumes lock is held) # ------------------------------------------------------------------------- @@ -197,9 +176,7 @@ def _disengage(self, hand: Hand | None = None) -> None: self._is_engaged[h] = False logger.info(f"{h.name} disengaged.") - @rpc def get_status(self) -> QuestTeleopStatus: - """Get current teleoperation status.""" with self._lock: left = self._controllers.get(Hand.LEFT) right = self._controllers.get(Hand.RIGHT) @@ -313,8 +290,7 @@ def _stop_control_loop(self) -> None: logger.info("Control loop stopped") def _control_loop(self) -> None: - """Main control loop: compute deltas and publish at fixed rate. - + """ Holds self._lock for the entire iteration so overridable methods don't need to acquire it themselves. """ From 2e8576e330554c3f95c34c15ac96fb4366664255 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 18 Feb 2026 13:35:11 -0800 Subject: [PATCH 28/30] Misc: comments --- dimos/teleop/phone/phone_teleop_module.py | 2 +- dimos/teleop/quest/quest_teleop_module.py | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 37763d2f4d..5c77b854f7 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -97,7 +97,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._server_script = Path(__file__).parent / "web" / "teleop_server.ts" # ------------------------------------------------------------------------- - # Public RPC Methods + # Lifecycle # ------------------------------------------------------------------------- def start(self) -> None: diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index b5ab6c2303..cbcddbfc11 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -74,8 +74,6 @@ class QuestTeleopModule(Module[QuestTeleopConfig]): Gets controller data from Deno bridge, computes output poses, and publishes them. Subclass to customize pose computation, output format, and engage behavior. - Implements TeleopProtocol. - Outputs: - left_controller_output: PoseStamped (output pose for left hand) - right_controller_output: PoseStamped (output pose for right hand) @@ -121,7 +119,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: self._server_script = Path(__file__).parent / "web" / "teleop_server.ts" # ------------------------------------------------------------------------- - # Public RPC Methods + # Lifecycle # ------------------------------------------------------------------------- def start(self) -> None: From c6f57ab9e637494aaf9d18056aef88bf2c129e73 Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 18 Feb 2026 14:08:06 -0800 Subject: [PATCH 29/30] Misc: comments + html button status fix --- dimos/teleop/phone/phone_extensions.py | 1 - dimos/teleop/phone/phone_teleop_module.py | 14 +++----------- dimos/teleop/phone/web/static/index.html | 2 +- 3 files changed, 4 insertions(+), 13 deletions(-) diff --git a/dimos/teleop/phone/phone_extensions.py b/dimos/teleop/phone/phone_extensions.py index cf8638e71f..f0a8fd4d01 100644 --- a/dimos/teleop/phone/phone_extensions.py +++ b/dimos/teleop/phone/phone_extensions.py @@ -35,7 +35,6 @@ class SimplePhoneTeleop(PhoneTeleopModule): cmd_vel: Out[Twist] def _publish_msg(self, output_msg: TwistStamped) -> None: - """Filter to ground robot axes and publish as Twist on cmd_vel.""" self.cmd_vel.publish( Twist( linear=Vector3(x=output_msg.linear.x, y=output_msg.linear.y, z=0.0), diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 5c77b854f7..2e2c1d06ec 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -46,11 +46,8 @@ @dataclass class PhoneTeleopConfig(ModuleConfig): control_loop_hz: float = 50.0 - - # Gain: maps degrees of tilt to m/s. 30 deg tilt -> 1.0 m/s - linear_gain: float = 1.0 / 30.0 - # Gain: maps degrees of yaw delta to rad/s. 30 deg -> 1.0 rad/s - angular_gain: float = 1.0 / 30.0 + linear_gain: float = 1.0 / 30.0 # Gain: maps degrees of tilt to m/s. 30 deg -> 1.0 m/s + angular_gain: float = 1.0 / 30.0 # Gain: maps gyro deg/s to rad/s. 30 deg/s -> 1.0 rad/s class PhoneTeleopModule(Module[PhoneTeleopConfig]): @@ -59,10 +56,6 @@ class PhoneTeleopModule(Module[PhoneTeleopConfig]): - TwistStamped: linear=(roll, pitch, yaw) deg, angular=(gyro) deg/s - Bool: teleop button state (True = held) - On engage (button pressed), captures the current orientation as initial. - Each control-loop tick computes the orientation delta from initial, - converts it to a TwistStamped via configurable gains, and publishes. - Outputs: - twist_output: TwistStamped (velocity command for robot) """ @@ -288,8 +281,7 @@ def _get_output_twist(self) -> TwistStamped | None: linear=Vector3( x=-delta.linear.y * cfg.linear_gain, # pitch forward -> drive forward y=-delta.linear.x * cfg.linear_gain, # roll right -> strafe right - z=d_yaw - * cfg.linear_gain, # yaw delta -> linear.z (remapped to angular by extensions) + z=d_yaw * cfg.linear_gain, # yaw delta ), angular=Vector3( x=current.angular.x * cfg.angular_gain, diff --git a/dimos/teleop/phone/web/static/index.html b/dimos/teleop/phone/web/static/index.html index ae7cd18028..6fad23b6c8 100644 --- a/dimos/teleop/phone/web/static/index.html +++ b/dimos/teleop/phone/web/static/index.html @@ -261,7 +261,7 @@

Sensors

// Teleop Button // ============================================ function startTeleop() { - if (!sensorsActive) return; + if (!sensorsActive || !ws || ws.readyState !== WebSocket.OPEN) return; teleopActive = true; $('teleop-btn').textContent = 'TRACKING...'; $('teleop-btn').classList.add('active'); From 9c1b68bc5070d21eedfa707feab1575f2a3579ed Mon Sep 17 00:00:00 2001 From: ruthwikdasyam Date: Wed, 18 Feb 2026 14:57:47 -0800 Subject: [PATCH 30/30] Fix: rpc to start/stop --- dimos/teleop/phone/phone_teleop_module.py | 4 +++- dimos/teleop/quest/quest_teleop_module.py | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/dimos/teleop/phone/phone_teleop_module.py b/dimos/teleop/phone/phone_teleop_module.py index 2e2c1d06ec..c0da85c27c 100644 --- a/dimos/teleop/phone/phone_teleop_module.py +++ b/dimos/teleop/phone/phone_teleop_module.py @@ -34,7 +34,7 @@ from reactivex.disposable import Disposable -from dimos.core import In, Module, Out +from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import Twist, TwistStamped, Vector3 from dimos.msgs.std_msgs.Bool import Bool @@ -93,6 +93,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Lifecycle # ------------------------------------------------------------------------- + @rpc def start(self) -> None: super().start() for stream, handler in ( @@ -103,6 +104,7 @@ def start(self) -> None: self._start_server() self._start_control_loop() + @rpc def stop(self) -> None: self._stop_control_loop() self._stop_server() diff --git a/dimos/teleop/quest/quest_teleop_module.py b/dimos/teleop/quest/quest_teleop_module.py index cbcddbfc11..ea77bb5fc0 100644 --- a/dimos/teleop/quest/quest_teleop_module.py +++ b/dimos/teleop/quest/quest_teleop_module.py @@ -32,7 +32,7 @@ from reactivex.disposable import Disposable -from dimos.core import In, Module, Out +from dimos.core import In, Module, Out, rpc from dimos.core.module import ModuleConfig from dimos.msgs.geometry_msgs import PoseStamped from dimos.msgs.sensor_msgs import Joy @@ -122,6 +122,7 @@ def __init__(self, *args: Any, **kwargs: Any) -> None: # Lifecycle # ------------------------------------------------------------------------- + @rpc def start(self) -> None: super().start() @@ -145,6 +146,7 @@ def start(self) -> None: self._start_server() logger.info("Quest Teleoperation Module started") + @rpc def stop(self) -> None: self._stop_control_loop() self._stop_server()