diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index 15c37186ac..014da1b32b 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -28,6 +28,7 @@ def _get_all_numbers(s: str) -> list[float]: class GlobalConfig(BaseSettings): robot_ip: str | None = None + robot_ips: str | None = None simulation: bool = False replay: bool = False viewer_backend: ViewerBackend = "rerun-web" @@ -46,6 +47,7 @@ class GlobalConfig(BaseSettings): planner_strategy: NavigationStrategy = "simple" planner_robot_speed: float | None = None dtop: bool = False + obstacle_avoidance: bool = True model_config = SettingsConfigDict( env_file=".env", diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 6026572388..d02abd61b9 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -57,6 +57,7 @@ "mid360-fastlio": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio", "mid360-fastlio-voxels": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels", "mid360-fastlio-voxels-native": "dimos.hardware.sensors.lidar.fastlio2.fastlio_blueprints:mid360_fastlio_voxels_native", + "phone-go2-fleet-teleop": "dimos.teleop.phone.blueprints:phone_go2_fleet_teleop", "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", @@ -77,6 +78,7 @@ "unitree-go2-agentic-ollama": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_agentic_ollama:unitree_go2_agentic_ollama", "unitree-go2-basic": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic:unitree_go2_basic", "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection", + "unitree-go2-fleet": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_fleet:unitree_go2_fleet", "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros", "unitree-go2-spatial": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_spatial:unitree_go2_spatial", "unitree-go2-temporal-memory": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_temporal_memory:unitree_go2_temporal_memory", @@ -107,6 +109,7 @@ "g1-sim-connection": "dimos.robot.unitree.g1.sim", "g1-skills": "dimos.robot.unitree.g1.skill_container", "go2-connection": "dimos.robot.unitree.go2.connection", + "go2-fleet-connection": "dimos.robot.unitree.go2.fleet_connection", "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", diff --git a/dimos/robot/unitree/connection.py b/dimos/robot/unitree/connection.py index f3f8ffaafb..ff73d922ee 100644 --- a/dimos/robot/unitree/connection.py +++ b/dimos/robot/unitree/connection.py @@ -277,6 +277,17 @@ def lowstate_stream(self) -> Observable[LowStateMsg]: def standup(self) -> bool: return bool(self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandUp"]})) + def balance_stand(self) -> bool: + return bool( + self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["BalanceStand"]}) + ) + + def set_obstacle_avoidance(self, enabled: bool = True) -> None: + self.publish_request( + RTC_TOPIC["OBSTACLES_AVOID"], + {"api_id": 1001, "parameter": {"enable": int(enabled)}}, + ) + def liedown(self) -> bool: return bool( self.publish_request(RTC_TOPIC["SPORT_MOD"], {"api_id": SPORT_CMD["StandDown"]}) diff --git a/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_fleet.py b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_fleet.py new file mode 100644 index 0000000000..015cfcdba4 --- /dev/null +++ b/dimos/robot/unitree/go2/blueprints/basic/unitree_go2_fleet.py @@ -0,0 +1,39 @@ +#!/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. + +"""Blueprint for Go2 fleet — multiple Go2 robots controlled together. + +Usage: + ROBOT_IPS=10.0.0.102,10.0.0.209 dimos run unitree-go2-fleet +""" + +from dimos.core.blueprints import autoconnect +from dimos.protocol.service.system_configurator import ClockSyncConfigurator +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import with_vis +from dimos.robot.unitree.go2.fleet_connection import go2_fleet_connection +from dimos.web.websocket_vis.websocket_vis_module import websocket_vis + +unitree_go2_fleet = ( + autoconnect( + with_vis, + go2_fleet_connection(), + websocket_vis(), + ) + .global_config(n_workers=4, robot_model="unitree_go2") + .configurators(ClockSyncConfigurator()) +) + +__all__ = ["unitree_go2_fleet"] diff --git a/dimos/robot/unitree/go2/connection.py b/dimos/robot/unitree/go2/connection.py index 4c7d4755fb..8baeae41f1 100644 --- a/dimos/robot/unitree/go2/connection.py +++ b/dimos/robot/unitree/go2/connection.py @@ -60,6 +60,8 @@ def video_stream(self) -> Observable: ... # type: ignore[type-arg] def move(self, twist: Twist, duration: float = 0.0) -> bool: ... def standup(self) -> bool: ... def liedown(self) -> bool: ... + def balance_stand(self) -> bool: ... + def set_obstacle_avoidance(self, enabled: bool = True) -> None: ... def publish_request(self, topic: str, data: dict) -> dict: ... # type: ignore[type-arg] @@ -81,6 +83,20 @@ def _camera_info_static() -> CameraInfo: ) +def make_connection(ip: str | None, cfg: GlobalConfig) -> Go2ConnectionProtocol: + connection_type = cfg.unitree_connection_type + + if ip in ("fake", "mock", "replay") or connection_type == "replay": + return ReplayConnection() + elif ip == "mujoco" or connection_type == "mujoco": + from dimos.robot.unitree.mujoco_connection import MujocoConnection + + return MujocoConnection(cfg) + else: + assert ip is not None, "IP address must be provided" + return UnitreeWebRTCConnection(ip) + + class ReplayConnection(UnitreeWebRTCConnection): dir_name = "unitree_go2_bigoffice" @@ -108,6 +124,12 @@ def standup(self) -> bool: def liedown(self) -> bool: return True + def balance_stand(self) -> bool: + return True + + def set_obstacle_avoidance(self, enabled: bool = True) -> None: + pass + @simple_mcache def lidar_stream(self): # type: ignore[no-untyped-def] lidar_store = TimedSensorReplay(f"{self.dir_name}/lidar") # type: ignore[var-annotated] @@ -181,18 +203,7 @@ def __init__( # type: ignore[no-untyped-def] self._global_config = cfg ip = ip if ip is not None else self._global_config.robot_ip - - connection_type = self._global_config.unitree_connection_type - - if ip in ["fake", "mock", "replay"] or connection_type == "replay": - self.connection = ReplayConnection() - elif ip == "mujoco" or connection_type == "mujoco": - from dimos.robot.unitree.mujoco_connection import MujocoConnection - - self.connection = MujocoConnection(self._global_config) - else: - assert ip is not None, "IP address must be provided" - self.connection = UnitreeWebRTCConnection(ip) + self.connection = make_connection(ip, self._global_config) Module.__init__(self, *args, **kwargs) @@ -229,6 +240,10 @@ def onimage(image: Image) -> None: self._camera_info_thread.start() self.standup() + time.sleep(3) + self.connection.balance_stand() + self.connection.set_obstacle_avoidance(self._global_config.obstacle_avoidance) + # self.record("go2_bigoffice") @rpc @@ -337,4 +352,4 @@ def deploy(dimos: ModuleCoordinator, ip: str, prefix: str = "") -> "ModuleProxy" return connection -__all__ = ["GO2Connection", "deploy", "go2_connection"] +__all__ = ["GO2Connection", "deploy", "go2_connection", "make_connection"] diff --git a/dimos/robot/unitree/go2/fleet_connection.py b/dimos/robot/unitree/go2/fleet_connection.py new file mode 100644 index 0000000000..4dd2be2984 --- /dev/null +++ b/dimos/robot/unitree/go2/fleet_connection.py @@ -0,0 +1,140 @@ +# 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. + +"""Go2 Fleet Connection - manage multiple Go2 robots as a fleet""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +from dimos.core.core import rpc +from dimos.core.global_config import GlobalConfig, global_config +from dimos.robot.unitree.go2.connection import ( + GO2Connection, + Go2ConnectionProtocol, + make_connection, +) +from dimos.utils.logging_config import setup_logger + +if TYPE_CHECKING: + from dimos.msgs.geometry_msgs import Twist + +logger = setup_logger() + + +class Go2FleetConnection(GO2Connection): + """Inherits all single-robot behaviour from GO2Connection for the primary + (first) robot. Additional robots only receive broadcast commands + (move, standup, liedown, publish_request). + """ + + def __init__( + self, + ips: list[str] | None = None, + cfg: GlobalConfig = global_config, + *args: object, + **kwargs: object, + ) -> None: + if not ips: + raw = cfg.robot_ips + if not raw: + raise ValueError( + "No IPs provided. Pass ips= or set ROBOT_IPS (e.g. ROBOT_IPS=10.0.0.102,10.0.0.209)" + ) + ips = [ip.strip() for ip in raw.split(",") if ip.strip()] + self._extra_ips = ips[1:] + self._extra_connections: list[Go2ConnectionProtocol] = [] + super().__init__(ips[0], cfg, *args, **kwargs) + + @rpc + def start(self) -> None: + self._extra_connections.clear() + for ip in self._extra_ips: + conn = make_connection(ip, self._global_config) + conn.start() + self._extra_connections.append(conn) + + # Parent starts primary robot, subscribes sensors, calls standup() on all + super().start() + for conn in self._extra_connections: + conn.balance_stand() + conn.set_obstacle_avoidance(self._global_config.obstacle_avoidance) + + @rpc + def stop(self) -> None: + # one robot's error should not prevent others from stopping + for conn in self._extra_connections: + try: + conn.liedown() + except Exception as e: + logger.error(f"Error lying down fleet Go2: {e}") + try: + conn.stop() + except Exception as e: + logger.error(f"Error stopping fleet Go2: {e}") + self._extra_connections.clear() + super().stop() + + @property + def _all_connections(self) -> list[Go2ConnectionProtocol]: + return [self.connection, *self._extra_connections] + + @rpc + def move(self, twist: Twist, duration: float = 0.0) -> bool: + results: list[bool] = [] + for conn in self._all_connections: + try: + results.append(conn.move(twist, duration)) + except Exception as e: + logger.error(f"Fleet move failed: {e}") + results.append(False) + return all(results) + + @rpc + def standup(self) -> bool: + results: list[bool] = [] + for conn in self._all_connections: + try: + results.append(conn.standup()) + except Exception as e: + logger.error(f"Fleet standup failed: {e}") + results.append(False) + return all(results) + + @rpc + def liedown(self) -> bool: + results: list[bool] = [] + for conn in self._all_connections: + try: + results.append(conn.liedown()) + except Exception as e: + logger.error(f"Fleet liedown failed: {e}") + results.append(False) + return all(results) + + @rpc + def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: + """Publish a request to all robots, return primary's response.""" + for conn in self._extra_connections: + try: + conn.publish_request(topic, data) + except Exception as e: + logger.error(f"Fleet publish_request failed: {e}") + return self.connection.publish_request(topic, data) + + +go2_fleet_connection = Go2FleetConnection.blueprint + + +__all__ = ["Go2FleetConnection", "go2_fleet_connection"] diff --git a/dimos/robot/unitree/mujoco_connection.py b/dimos/robot/unitree/mujoco_connection.py index f998ae1dd9..36673ecb3e 100644 --- a/dimos/robot/unitree/mujoco_connection.py +++ b/dimos/robot/unitree/mujoco_connection.py @@ -229,6 +229,12 @@ def standup(self) -> bool: def liedown(self) -> bool: return True + def balance_stand(self) -> bool: + return True + + def set_obstacle_avoidance(self, enabled: bool = True) -> None: + pass + def get_video_frame(self) -> NDArray[Any] | None: if self.shm_data is None: return None diff --git a/dimos/teleop/phone/blueprints.py b/dimos/teleop/phone/blueprints.py index 6328af8612..6e68e726e3 100644 --- a/dimos/teleop/phone/blueprints.py +++ b/dimos/teleop/phone/blueprints.py @@ -15,6 +15,7 @@ from dimos.core.blueprints import autoconnect from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import unitree_go2_basic +from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_fleet import unitree_go2_fleet from dimos.teleop.phone.phone_extensions import simple_phone_teleop_module # Simple phone teleop (mobile base axis filtering + cmd_vel output) @@ -28,5 +29,11 @@ unitree_go2_basic, ) +# Phone teleop wired to Go2 fleet — twist commands sent to all robots +phone_go2_fleet_teleop = autoconnect( + simple_phone_teleop_module(), + unitree_go2_fleet, +) + -__all__ = ["phone_go2_teleop", "simple_phone_teleop"] +__all__ = ["phone_go2_fleet_teleop", "phone_go2_teleop", "simple_phone_teleop"]