From 308cc3d8d5a0d4ab3b19a5df2d6c05e0904cf4c2 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 14 Nov 2025 06:44:04 +0200 Subject: [PATCH 1/2] add keyboard-teleop --- dimos/robot/all_blueprints.py | 2 +- .../unitree_webrtc/g1_joystick_module.py | 185 ---------------- dimos/robot/unitree_webrtc/keyboard_teleop.py | 205 ++++++++++++++++++ dimos/robot/unitree_webrtc/unitree_g1.py | 2 +- .../unitree_webrtc/unitree_g1_blueprints.py | 6 +- 5 files changed, 210 insertions(+), 190 deletions(-) delete mode 100644 dimos/robot/unitree_webrtc/g1_joystick_module.py create mode 100644 dimos/robot/unitree_webrtc/keyboard_teleop.py diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 1054b8133c..6121a8bc4d 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -49,12 +49,12 @@ "detection_2d": "dimos.perception.detection2d.module2D", "foxglove_bridge": "dimos.robot.foxglove_bridge", "g1_connection": "dimos.robot.unitree_webrtc.unitree_g1", - "g1_joystick": "dimos.robot.unitree_webrtc.g1_joystick_module", "g1_skills": "dimos.robot.unitree_webrtc.unitree_g1_skill_container", "google_maps_skill": "dimos.agents2.skills.google_maps_skill_container", "gps_nav_skill": "dimos.agents2.skills.gps_nav_skill", "holonomic_local_planner": "dimos.navigation.local_planner.holonomic_local_planner", "human_input": "dimos.agents2.cli.human", + "keyboard_teleop": "dimos.robot.unitree_webrtc.keyboard_teleop", "llm_agent": "dimos.agents2.agent", "mapper": "dimos.robot.unitree_webrtc.type.map", "navigation_skill": "dimos.agents2.skills.navigation", diff --git a/dimos/robot/unitree_webrtc/g1_joystick_module.py b/dimos/robot/unitree_webrtc/g1_joystick_module.py deleted file mode 100644 index 3a796d4011..0000000000 --- a/dimos/robot/unitree_webrtc/g1_joystick_module.py +++ /dev/null @@ -1,185 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2025 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. - -"""Pygame Joystick Module for testing G1 humanoid control.""" - -import os -import threading - -# Force X11 driver to avoid OpenGL threading issues -os.environ["SDL_VIDEODRIVER"] = "x11" - -from dimos.core import Module, Out, rpc -from dimos.msgs.geometry_msgs import Twist, Vector3 - - -class G1JoystickModule(Module): - """Pygame-based joystick control module for G1 humanoid testing. - - Outputs standard Twist messages on /cmd_vel for velocity control. - Simplified version without mode switching since G1 handles that differently. - """ - - twist_out: Out[Twist] = None # Standard velocity commands - - def __init__(self, *args, **kwargs) -> None: - Module.__init__(self, *args, **kwargs) - self.pygame_ready = False - self.running = False - - @rpc - def start(self) -> bool: - """Initialize pygame and start control loop.""" - super().start() - - try: - import pygame - except ImportError: - print("ERROR: pygame not installed. Install with: pip install pygame") - return False - - self.keys_held = set() - self.pygame_ready = True - self.running = True - - # Start pygame loop in background thread - self._thread = threading.Thread(target=self._pygame_loop, daemon=True) - self._thread.start() - - return True - - @rpc - def stop(self) -> None: - super().stop() - - self.running = False - self.pygame_ready = False - - stop_twist = Twist() - stop_twist.linear = Vector3(0, 0, 0) - stop_twist.angular = Vector3(0, 0, 0) - - self._thread.join(2) - - self.twist_out.publish(stop_twist) - - def _pygame_loop(self) -> None: - """Main pygame event loop - ALL pygame operations happen here.""" - import pygame - - pygame.init() - self.screen = pygame.display.set_mode((500, 400), pygame.SWSURFACE) - pygame.display.set_caption("G1 Humanoid Joystick Control") - self.clock = pygame.time.Clock() - self.font = pygame.font.Font(None, 24) - - print("G1 JoystickModule started - Focus pygame window to control") - print("Controls:") - print(" WS = Forward/Back") - print(" AD = Turn Left/Right") - print(" Space = Emergency Stop") - print(" ESC = Quit") - - while self.running and self.pygame_ready: - for event in pygame.event.get(): - if event.type == pygame.QUIT: - self.running = False - elif event.type == pygame.KEYDOWN: - self.keys_held.add(event.key) - - if event.key == pygame.K_SPACE: - # Emergency stop - clear all keys and send zero twist - self.keys_held.clear() - stop_twist = Twist() - stop_twist.linear = Vector3(0, 0, 0) - stop_twist.angular = Vector3(0, 0, 0) - self.twist_out.publish(stop_twist) - print("EMERGENCY STOP!") - elif event.key == pygame.K_ESCAPE: - # ESC quits - self.running = False - - elif event.type == pygame.KEYUP: - self.keys_held.discard(event.key) - - # Generate Twist message from held keys - twist = Twist() - twist.linear = Vector3(0, 0, 0) - twist.angular = Vector3(0, 0, 0) - - # Forward/backward (W/S) - if pygame.K_w in self.keys_held: - twist.linear.x = 0.5 - if pygame.K_s in self.keys_held: - twist.linear.x = -0.5 - - # Turning (A/D) - if pygame.K_a in self.keys_held: - twist.angular.z = 0.5 - if pygame.K_d in self.keys_held: - twist.angular.z = -0.5 - - # Always publish twist at 50Hz - self.twist_out.publish(twist) - - self._update_display(twist) - - # Maintain 50Hz rate - self.clock.tick(50) - - pygame.quit() - print("G1 JoystickModule stopped") - - def _update_display(self, twist) -> None: - """Update pygame window with current status.""" - import pygame - - self.screen.fill((30, 30, 30)) - - y_pos = 20 - - texts = [ - "G1 Humanoid Control", - "", - f"Linear X (Forward/Back): {twist.linear.x:+.2f} m/s", - f"Angular Z (Turn L/R): {twist.angular.z:+.2f} rad/s", - "", - "Keys: " + ", ".join([pygame.key.name(k).upper() for k in self.keys_held if k < 256]), - ] - - for text in texts: - if text: - color = (0, 255, 255) if text == "G1 Humanoid Control" else (255, 255, 255) - surf = self.font.render(text, True, color) - self.screen.blit(surf, (20, y_pos)) - y_pos += 30 - - if twist.linear.x != 0 or twist.linear.y != 0 or twist.angular.z != 0: - pygame.draw.circle(self.screen, (255, 0, 0), (450, 30), 15) # Red = moving - else: - pygame.draw.circle(self.screen, (0, 255, 0), (450, 30), 15) # Green = stopped - - y_pos = 300 - help_texts = ["WS: Move | AD: Turn", "Space: E-Stop | ESC: Quit"] - for text in help_texts: - surf = self.font.render(text, True, (150, 150, 150)) - self.screen.blit(surf, (20, y_pos)) - y_pos += 25 - - pygame.display.flip() - - -# Create blueprint function for easy instantiation -g1_joystick = G1JoystickModule.blueprint diff --git a/dimos/robot/unitree_webrtc/keyboard_teleop.py b/dimos/robot/unitree_webrtc/keyboard_teleop.py new file mode 100644 index 0000000000..b4e9153eef --- /dev/null +++ b/dimos/robot/unitree_webrtc/keyboard_teleop.py @@ -0,0 +1,205 @@ +#!/usr/bin/env python3 +# Copyright 2025 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. + +import os +import threading + +import pygame + +from dimos.core import Module, Out, rpc +from dimos.msgs.geometry_msgs import Twist, Vector3 + +# Force X11 driver to avoid OpenGL threading issues +os.environ["SDL_VIDEODRIVER"] = "x11" + + +class KeyboardTeleop(Module): + """Pygame-based keyboard control module. + + Outputs standard Twist messages on /cmd_vel for velocity control. + """ + + cmd_vel: Out[Twist] = None # Standard velocity commands + + _stop_event: threading.Event + _keys_held: set[int] | None = None + _thread: threading.Thread | None = None + _screen: pygame.Surface | None = None + _clock: pygame.time.Clock | None = None + _font: pygame.font.Font | None = None + + def __init__(self) -> None: + super().__init__() + self._stop_event = threading.Event() + + @rpc + def start(self) -> bool: + super().start() + + self._keys_held = set() + self._stop_event.clear() + + self._thread = threading.Thread(target=self._pygame_loop, daemon=True) + self._thread.start() + + return True + + @rpc + def stop(self) -> None: + stop_twist = Twist() + stop_twist.linear = Vector3(0, 0, 0) + stop_twist.angular = Vector3(0, 0, 0) + self.cmd_vel.publish(stop_twist) + + self._stop_event.set() + + if self._thread is None: + raise RuntimeError("Cannot stop: thread was never started") + self._thread.join(2) + + super().stop() + + def _pygame_loop(self) -> None: + if self._keys_held is None: + raise RuntimeError("_keys_held not initialized") + + pygame.init() + self._screen = pygame.display.set_mode((500, 400), pygame.SWSURFACE) + pygame.display.set_caption("Keyboard Teleop") + self._clock = pygame.time.Clock() + self._font = pygame.font.Font(None, 24) + + while not self._stop_event.is_set(): + for event in pygame.event.get(): + if event.type == pygame.QUIT: + self._stop_event.set() + elif event.type == pygame.KEYDOWN: + self._keys_held.add(event.key) + + if event.key == pygame.K_SPACE: + # Emergency stop - clear all keys and send zero twist + self._keys_held.clear() + stop_twist = Twist() + stop_twist.linear = Vector3(0, 0, 0) + stop_twist.angular = Vector3(0, 0, 0) + self.cmd_vel.publish(stop_twist) + print("EMERGENCY STOP!") + elif event.key == pygame.K_ESCAPE: + # ESC quits + self._stop_event.set() + + elif event.type == pygame.KEYUP: + self._keys_held.discard(event.key) + + # Generate Twist message from held keys + twist = Twist() + twist.linear = Vector3(0, 0, 0) + twist.angular = Vector3(0, 0, 0) + + # Forward/backward (W/S) + if pygame.K_w in self._keys_held: + twist.linear.x = 0.5 + if pygame.K_s in self._keys_held: + twist.linear.x = -0.5 + + # Strafe left/right (Q/E) + if pygame.K_q in self._keys_held: + twist.linear.y = 0.5 + if pygame.K_e in self._keys_held: + twist.linear.y = -0.5 + + # Turning (A/D) + if pygame.K_a in self._keys_held: + twist.angular.z = 0.8 + if pygame.K_d in self._keys_held: + twist.angular.z = -0.8 + + # Apply speed modifiers (Shift = 2x, Ctrl = 0.5x) + speed_multiplier = 1.0 + if pygame.K_LSHIFT in self._keys_held or pygame.K_RSHIFT in self._keys_held: + speed_multiplier = 2.0 + elif pygame.K_LCTRL in self._keys_held or pygame.K_RCTRL in self._keys_held: + speed_multiplier = 0.5 + + twist.linear.x *= speed_multiplier + twist.linear.y *= speed_multiplier + twist.angular.z *= speed_multiplier + + # Always publish twist at 50Hz + self.cmd_vel.publish(twist) + + self._update_display(twist) + + # Maintain 50Hz rate + if self._clock is None: + raise RuntimeError("_clock not initialized") + self._clock.tick(50) + + pygame.quit() + + def _update_display(self, twist: Twist) -> None: + if self._screen is None or self._font is None or self._keys_held is None: + raise RuntimeError("Not initialized correctly") + + self._screen.fill((30, 30, 30)) + + y_pos = 20 + + # Determine active speed multiplier + speed_mult_text = "" + if pygame.K_LSHIFT in self._keys_held or pygame.K_RSHIFT in self._keys_held: + speed_mult_text = " [BOOST 2x]" + elif pygame.K_LCTRL in self._keys_held or pygame.K_RCTRL in self._keys_held: + speed_mult_text = " [SLOW 0.5x]" + + texts = [ + "Keyboard Teleop" + speed_mult_text, + "", + f"Linear X (Forward/Back): {twist.linear.x:+.2f} m/s", + f"Linear Y (Strafe L/R): {twist.linear.y:+.2f} m/s", + f"Angular Z (Turn L/R): {twist.angular.z:+.2f} rad/s", + "", + "Keys: " + ", ".join([pygame.key.name(k).upper() for k in self._keys_held if k < 256]), + ] + + for text in texts: + if text: + color = (0, 255, 255) if text.startswith("Keyboard Teleop") else (255, 255, 255) + surf = self._font.render(text, True, color) + self._screen.blit(surf, (20, y_pos)) + y_pos += 30 + + if twist.linear.x != 0 or twist.linear.y != 0 or twist.angular.z != 0: + pygame.draw.circle(self._screen, (255, 0, 0), (450, 30), 15) # Red = moving + else: + pygame.draw.circle(self._screen, (0, 255, 0), (450, 30), 15) # Green = stopped + + y_pos = 280 + help_texts = [ + "WS: Move | AD: Turn | QE: Strafe", + "Shift: Boost | Ctrl: Slow", + "Space: E-Stop | ESC: Quit", + ] + for text in help_texts: + surf = self._font.render(text, True, (150, 150, 150)) + self._screen.blit(surf, (20, y_pos)) + y_pos += 25 + + pygame.display.flip() + + +keyboard_teleop = KeyboardTeleop.blueprint + +__all__ = ["KeyboardTeleop", "keyboard_teleop"] diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 55d7537ef0..9cd050dd22 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -465,7 +465,7 @@ def _deploy_joystick(self) -> None: logger.info("Deploying G1 joystick module...") self.joystick = self._dimos.deploy(G1JoystickModule) - self.joystick.twist_out.transport = core.LCMTransport("/cmd_vel", Twist) + self.joystick.cmd_vel.transport = core.LCMTransport("/cmd_vel", Twist) logger.info("Joystick module deployed - pygame window will open") def _deploy_ros_bridge(self) -> None: diff --git a/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py b/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py index 975e951e40..8e71173c30 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py +++ b/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py @@ -54,7 +54,7 @@ from dimos.perception.object_tracker import object_tracking from dimos.perception.spatial_perception import spatial_memory from dimos.robot.foxglove_bridge import foxglove_bridge -from dimos.robot.unitree_webrtc.g1_joystick_module import g1_joystick +from dimos.robot.unitree_webrtc.keyboard_teleop import keyboard_teleop from dimos.robot.unitree_webrtc.type.map import mapper from dimos.robot.unitree_webrtc.unitree_g1 import g1_connection from dimos.robot.unitree_webrtc.unitree_g1_skill_container import g1_skills @@ -185,12 +185,12 @@ # Configuration with joystick control for teleoperation with_joystick = autoconnect( basic_ros, - g1_joystick(), # Pygame-based joystick control + keyboard_teleop(), # Pygame-based joystick control ) # Full featured configuration with everything full_featured = autoconnect( standard_with_shm, _agentic_skills, - g1_joystick(), + keyboard_teleop(), ) From bf03e04a19d7a526137aa94496b78b7521f6e839 Mon Sep 17 00:00:00 2001 From: Paul Nechifor Date: Fri, 14 Nov 2025 07:44:32 +0200 Subject: [PATCH 2/2] fix import --- dimos/robot/unitree_webrtc/unitree_g1.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index 9cd050dd22..50f7504d31 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -63,6 +63,7 @@ from dimos.robot.robot import Robot from dimos.robot.ros_bridge import BridgeDirection, ROSBridge from dimos.robot.unitree_webrtc.connection import UnitreeWebRTCConnection +from dimos.robot.unitree_webrtc.keyboard_teleop import KeyboardTeleop from dimos.robot.unitree_webrtc.rosnav import NavigationModule from dimos.robot.unitree_webrtc.type.lidar import LidarMessage from dimos.robot.unitree_webrtc.type.odometry import Odometry as SimOdometry @@ -461,10 +462,8 @@ def _deploy_perception(self) -> None: def _deploy_joystick(self) -> None: """Deploy joystick control module.""" - from dimos.robot.unitree_webrtc.g1_joystick_module import G1JoystickModule - logger.info("Deploying G1 joystick module...") - self.joystick = self._dimos.deploy(G1JoystickModule) + self.joystick = self._dimos.deploy(KeyboardTeleop) self.joystick.cmd_vel.transport = core.LCMTransport("/cmd_vel", Twist) logger.info("Joystick module deployed - pygame window will open")