diff --git a/dimos/hardware/camera/module.py b/dimos/hardware/camera/module.py index 9a8e091c43..1c82fd3135 100644 --- a/dimos/hardware/camera/module.py +++ b/dimos/hardware/camera/module.py @@ -130,3 +130,6 @@ def stop(self) -> None: if self.hardware and hasattr(self.hardware, "stop"): self.hardware.stop() super().stop() + + +camera_module = CameraModule.blueprint diff --git a/dimos/navigation/rosnav.py b/dimos/navigation/rosnav.py index f0d04926d3..460f9fa375 100644 --- a/dimos/navigation/rosnav.py +++ b/dimos/navigation/rosnav.py @@ -24,7 +24,6 @@ import threading import time -# ROS2 message imports from geometry_msgs.msg import ( PointStamped as ROSPointStamped, PoseStamped as ROSPoseStamped, @@ -383,16 +382,18 @@ def stop(self) -> None: super().stop() +navigation_module = ROSNavigationModule.blueprint + + def deploy(dimos: DimosCluster): nav = dimos.deploy(ROSNav) nav.pointcloud.transport = pSHMTransport("/lidar") nav.global_pointcloud.transport = pSHMTransport("/map") - - nav.goal_req.transport = LCMTransport("/goal_req", PoseStamped) nav.goal_req.transport = LCMTransport("/goal_req", PoseStamped) nav.goal_active.transport = LCMTransport("/goal_active", PoseStamped) nav.path_active.transport = LCMTransport("/path_active", Path) nav.cmd_vel.transport = LCMTransport("/cmd_vel", TwistStamped) + nav.start() return nav diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index c177723e66..388b8cf1dd 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -22,6 +22,12 @@ "unitree-go2-jpegshm": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:standard_with_jpegshm", "unitree-go2-jpeglcm": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:standard_with_jpeglcm", "unitree-go2-agentic": "dimos.robot.unitree_webrtc.unitree_go2_blueprints:agentic", + "unitree-g1": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:standard", + "unitree-g1-basic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:basic", + "unitree-g1-shm": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:standard_with_shm", + "unitree-g1-agentic": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:agentic", + "unitree-g1-joystick": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:with_joystick", + "unitree-g1-full": "dimos.robot.unitree_webrtc.unitree_g1_blueprints:full_featured", "demo-osm": "dimos.mapping.osm.demo_osm:demo_osm", "demo-remapping": "dimos.robot.unitree_webrtc.demo_remapping:remapping", "demo-remapping-transport": "dimos.robot.unitree_webrtc.demo_remapping:remapping_and_transport", diff --git a/dimos/robot/unitree_webrtc/g1_joystick_module.py b/dimos/robot/unitree_webrtc/g1_joystick_module.py index 2c6a5e64e5..3a796d4011 100644 --- a/dimos/robot/unitree_webrtc/g1_joystick_module.py +++ b/dimos/robot/unitree_webrtc/g1_joystick_module.py @@ -179,3 +179,7 @@ def _update_display(self, twist) -> None: y_pos += 25 pygame.display.flip() + + +# Create blueprint function for easy instantiation +g1_joystick = G1JoystickModule.blueprint diff --git a/dimos/robot/unitree_webrtc/rosnav.py b/dimos/robot/unitree_webrtc/rosnav.py index bd91fafb90..ee0c5f9c77 100644 --- a/dimos/robot/unitree_webrtc/rosnav.py +++ b/dimos/robot/unitree_webrtc/rosnav.py @@ -25,6 +25,7 @@ logger = setup_logger("dimos.robot.unitree_webrtc.nav_bot", level=logging.INFO) +# TODO: Remove, deprecated class NavigationModule(Module): goal_pose: Out[PoseStamped] = None goal_reached: In[Bool] = None diff --git a/dimos/robot/unitree_webrtc/unitree_g1.py b/dimos/robot/unitree_webrtc/unitree_g1.py index fc148c54c3..633ba31e23 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1.py +++ b/dimos/robot/unitree_webrtc/unitree_g1.py @@ -35,6 +35,7 @@ from dimos.agents2.skills.ros_navigation import RosNavigation from dimos.agents2.spec import Model, Provider from dimos.core import In, Module, Out, rpc +from dimos.core.global_config import GlobalConfig from dimos.core.module_coordinator import ModuleCoordinator from dimos.core.resource import Resource from dimos.hardware.camera import zed @@ -83,7 +84,7 @@ class G1ConnectionModule(Module): """Simplified connection module for G1 - uses WebRTC for control.""" - movecmd: In[TwistStamped] = None + movecmd: In[Twist] = None odom_in: In[Odometry] = None odom_pose: Out[PoseStamped] = None @@ -91,10 +92,16 @@ class G1ConnectionModule(Module): connection_type: str = "webrtc" def __init__( - self, ip: str | None = None, connection_type: str = "webrtc", *args, **kwargs + self, + ip: str | None = None, + connection_type: str = "webrtc", + global_config: GlobalConfig | None = None, + *args, + **kwargs, ) -> None: - self.ip = ip - self.connection_type = connection_type + cfg = global_config or GlobalConfig() + self.ip = ip if ip is not None else cfg.robot_ip + self.connection_type = connection_type or cfg.unitree_connection_type self.connection = None Module.__init__(self, *args, **kwargs) @@ -128,9 +135,8 @@ def _publish_odom_pose(self, msg: Odometry) -> None: ) @rpc - def move(self, twist_stamped: TwistStamped, duration: float = 0.0) -> None: + def move(self, twist: Twist, duration: float = 0.0) -> None: """Send movement command to robot.""" - twist = Twist(linear=twist_stamped.linear, angular=twist_stamped.angular) self.connection.move(twist, duration) @rpc @@ -139,6 +145,9 @@ def publish_request(self, topic: str, data: dict): return self.connection.publish_request(topic, data) +connection = G1ConnectionModule.blueprint + + class UnitreeG1(Robot, Resource): """Unitree G1 humanoid robot.""" @@ -331,7 +340,7 @@ def _deploy_connection(self) -> None: self.connection = self._dimos.deploy(G1ConnectionModule, self.ip) # Configure LCM transports - self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", TwistStamped) + self.connection.movecmd.transport = core.LCMTransport("/cmd_vel", Twist) self.connection.odom_in.transport = core.LCMTransport("/state_estimation", Odometry) self.connection.odom_pose.transport = core.LCMTransport("/odom", PoseStamped) @@ -361,11 +370,12 @@ def _deploy_camera(self) -> None: def _deploy_visualization(self) -> None: """Deploy and configure visualization modules.""" - # Deploy WebSocket visualization module - self.websocket_vis = self._dimos.deploy(WebsocketVisModule, port=self.websocket_port) - self.websocket_vis.movecmd_stamped.transport = core.LCMTransport("/cmd_vel", TwistStamped) + # Deploy WebSocket visualization module - COMMENTED OUT DUE TO TRANSPORT ISSUES + # self.websocket_vis = self._dimos.deploy(WebsocketVisModule, port=self.websocket_port) + # self.websocket_vis.movecmd_stamped.transport = core.LCMTransport("/cmd_vel", TwistStamped) - # Note: robot_pose connection removed since odom was removed from G1ConnectionModule + # Connect odometry to websocket visualization + # self.websocket_vis.odom.transport = core.LCMTransport("/odom", PoseStamped) # Deploy Foxglove bridge self.foxglove_bridge = FoxgloveBridge( diff --git a/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py b/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py new file mode 100644 index 0000000000..f082ddd7ae --- /dev/null +++ b/dimos/robot/unitree_webrtc/unitree_g1_blueprints.py @@ -0,0 +1,173 @@ +#!/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. + +"""Blueprint configurations for Unitree G1 humanoid robot. + +This module provides pre-configured blueprints for various G1 robot setups, +from basic teleoperation to full autonomous agent configurations. +""" + +from dimos_lcm.sensor_msgs import CameraInfo + +from dimos.agents2.agent import llm_agent +from dimos.agents2.cli.human import human_input +from dimos.agents2.skills.navigation import navigation_skill +from dimos.constants import DEFAULT_CAPACITY_COLOR_IMAGE, DEFAULT_CAPACITY_DEPTH_IMAGE +from dimos.core.blueprints import autoconnect +from dimos.core.transport import LCMTransport, pSHMTransport +from dimos.hardware.camera import zed +from dimos.hardware.camera.module import camera_module +from dimos.hardware.camera.webcam import Webcam +from dimos.msgs.geometry_msgs import ( + PoseStamped, + Quaternion, + Transform, + Twist, + TwistStamped, + Vector3, +) +from dimos.msgs.nav_msgs import Odometry, Path +from dimos.msgs.sensor_msgs import Image, PointCloud2 +from dimos.msgs.std_msgs import Bool +from dimos.navigation.bt_navigator.navigator import ( + behavior_tree_navigator, +) +from dimos.navigation.frontier_exploration import ( + wavefront_frontier_explorer, +) +from dimos.navigation.global_planner import astar_planner +from dimos.navigation.local_planner.holonomic_local_planner import ( + holonomic_local_planner, +) +from dimos.navigation.rosnav.nav_bot import navigation_module +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.depth_module import depth_module +from dimos.robot.unitree_webrtc.g1_joystick_module import g1_joystick +from dimos.robot.unitree_webrtc.type.map import mapper +from dimos.robot.unitree_webrtc.unitree_g1 import connection +from dimos.robot.unitree_webrtc.unitree_g1_skill_container import g1_skills +from dimos.utils.monitoring import utilization +from dimos.web.websocket_vis.websocket_vis_module import websocket_vis + +# Basic configuration with navigation and visualization +basic = ( + autoconnect( + # Core connection module for G1 + connection(), + # Camera module + camera_module( + transform=Transform( + translation=Vector3(0.05, 0.0, 0.0), + rotation=Quaternion.from_euler(Vector3(0.0, 0.2, 0.0)), + frame_id="sensor", + child_frame_id="camera_link", + ), + hardware=lambda: Webcam( + camera_index=0, + frequency=15, + stereo_slice="left", + camera_info=zed.CameraInfo.SingleWebcam, + ), + ), + # SLAM and mapping + mapper(voxel_size=0.5, global_publish_interval=2.5), + # Navigation stack + astar_planner(), + holonomic_local_planner(), + behavior_tree_navigator(), + wavefront_frontier_explorer(), + navigation_module(), # G1-specific ROS navigation + # Visualization + websocket_vis(), + foxglove_bridge(), + ) + .global_config(n_dask_workers=4) + .transports( + { + # G1 uses Twist for movement commands + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + ("movecmd", Twist): LCMTransport("/cmd_vel", Twist), + # State estimation from ROS + ("state_estimation", Odometry): LCMTransport("/state_estimation", Odometry), + # Odometry output from ROSNavigationModule + ("odom_pose", PoseStamped): LCMTransport("/odom", PoseStamped), + # Navigation module topics from nav_bot + ("goal_req", PoseStamped): LCMTransport("/goal_req", PoseStamped), + ("goal_active", PoseStamped): LCMTransport("/goal_active", PoseStamped), + ("path_active", Path): LCMTransport("/path_active", Path), + ("pointcloud", PointCloud2): LCMTransport("/lidar", PointCloud2), + ("global_pointcloud", PointCloud2): LCMTransport("/map", PointCloud2), + # Original navigation topics for backwards compatibility + ("goal_pose", PoseStamped): LCMTransport("/goal_pose", PoseStamped), + ("goal_reached", Bool): LCMTransport("/goal_reached", Bool), + ("cancel_goal", Bool): LCMTransport("/cancel_goal", Bool), + # Camera topics (if camera module is added) + ("image", Image): LCMTransport("/g1/color_image", Image), + ("color_image", Image): LCMTransport("/g1/color_image", Image), + ("camera_info", CameraInfo): LCMTransport("/g1/camera_info", CameraInfo), + } + ) +) + +# Standard configuration with perception and memory +standard = autoconnect( + basic, + spatial_memory(), + object_tracking(frame_id="camera_link"), + utilization(), +).global_config(n_dask_workers=8) + +# Optimized configuration using shared memory for images +standard_with_shm = autoconnect( + standard.transports( + { + ("color_image", Image): pSHMTransport( + "/g1/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + ), + } + ), + foxglove_bridge( + shm_channels=[ + "/g1/color_image#sensor_msgs.Image", + ] + ), +) + +# Full agentic configuration with LLM and skills +agentic = autoconnect( + standard, + llm_agent(), + human_input(), + navigation_skill(), + g1_skills(), # G1-specific arm and movement mode skills +) + +# Configuration with joystick control for teleoperation +with_joystick = autoconnect( + basic, + g1_joystick(), # Pygame-based joystick control +) + +# Full featured configuration with everything +full_featured = autoconnect( + standard_with_shm, + llm_agent(), + human_input(), + navigation_skill(), + g1_skills(), + g1_joystick(), +) diff --git a/dimos/robot/unitree_webrtc/unitree_g1_skill_container.py b/dimos/robot/unitree_webrtc/unitree_g1_skill_container.py index 170b577c21..afca3339f5 100644 --- a/dimos/robot/unitree_webrtc/unitree_g1_skill_container.py +++ b/dimos/robot/unitree_webrtc/unitree_g1_skill_container.py @@ -229,3 +229,7 @@ def _execute_mode_command(self, data_value: int, name: str) -> str: error_msg = f"Failed to execute G1 mode {name}: {e}" logger.error(error_msg) return error_msg + + +# Create blueprint function for easy instantiation +g1_skills = UnitreeG1SkillContainer.blueprint