From a342cd2f3f75f1653841c67701a4ac98be89b25c Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Thu, 30 Apr 2026 13:03:57 -0400 Subject: [PATCH 01/18] Attempting to solve issuse 1842 From 10de38f3ebd8b8523447d517cab09901bdc33356 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sat, 9 May 2026 21:36:16 -0400 Subject: [PATCH 02/18] File added -- inspired from unitree_g1_basic_sim and drone_basic. Uses ReplanningAStar (Same as g1) for planning -- yet to be tested --- .../drone/blueprints/basic/drone_basic_sim.py | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 dimos/robot/drone/blueprints/basic/drone_basic_sim.py diff --git a/dimos/robot/drone/blueprints/basic/drone_basic_sim.py b/dimos/robot/drone/blueprints/basic/drone_basic_sim.py new file mode 100644 index 0000000000..4a2c287933 --- /dev/null +++ b/dimos/robot/drone/blueprints/basic/drone_basic_sim.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 + +""" Basic Drone Sim Blueprint with sim connection and visualization""" + +from typing import TYPE_CHECKING + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner +from dimos.robot.drone.blueprints.primitive.drone_primitive_no_nav import ( + drone_primitive_no_nav, +) +from dimos.robot.drone.sim import DroneSimConnection + +drone_basic_sim = autoconnect( + drone_primitive_no_nav, + DroneSimConnection.blueprint(), + ReplanningAStarPlanner.blueprint(), +) + +__all__ = ["drone_basic_sim"] \ No newline at end of file From ba0101c443929ebe318f31b6d7d5b7adcac066c6 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sat, 9 May 2026 22:58:50 -0400 Subject: [PATCH 03/18] Done till creating rerun_config, most of the stuff remains the same, drone_rerun_blueprint instead of g1 -- Inspired from unitree_g1_primitive_no_nav --- .../primitive/drone_primitive_no_nav.py | 87 +++++++++++++++++++ 1 file changed, 87 insertions(+) create mode 100644 dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py diff --git a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py new file mode 100644 index 0000000000..2502ce469d --- /dev/null +++ b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 + +""" Minimal Drone Stack without navigation. An attempt to create a minimal stack""" + +from typing import Any + +from dimos_lcm.sensor_msgs import CameraInfo + +from dimos.core.coordination.blueprints import autoconnect +from dimos.core.global_config import global_config +from dimos.core.transport import LCMTransport +from dimos.hardware.sensors.camera.module import CameraModule +from dimos.hardware.sensors.camera.webcam import Webcam +from dimos.hardware.sensors.camera.zed import compat as zed +from dimos.mapping.costmapper import CostMapper +from dimos.mapping.voxels import VoxelGridMapper +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.nav_msgs.Odometry import Odometry +from dimos.msgs.nav_msgs.Path import Path +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.msgs.std_msgs.Bool import Bool +from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( + WavefrontFrontierExplorer, +) +from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule + +def _convert_camera_info(camera_info: Any) -> Any: + return camera_info.to_rerun( + image_topic="/world/color_image", + optical_frame="camera_optical", + ) + +def _convert_navigation_costmap(grid: Any) -> Any: + return grid.to_rerun( + colormap="Accent", + z_offset=0.015, + opacity=0.2, + background="#484981", + ) + +def _static_base_link(rr: Any) -> list[Any]: + return [ + rr.Boxes3D( + half_sizes=[0.2, 0.15, 0.75], + colors=[(0, 255, 127)], + fill_mode="MajorWireframe", + ), + rr.Transform3D(parent_frame="tf#/base_link"), + ] + +def _drone_rerun_blueprint() -> Any: + """Same as the G1 rerun blueprint but with a different base link visualization.""" + import rerun as rr + import rerun.blueprint as rrb + + return rrb.Blueprint( + rrb.Horizontal( + rrb.Spatial2DView(origin="world/color_image", name="Camera"), + rrb.Spatial3DView( + origin="world", + name="3D", + background=rrb.Background(kind="SolidColor", color=[0, 0, 0]), + line_grid=rrb.LineGrid3D( + plane=rr.components.Plane3D.XY.with_distance(0.5), + ), + ), + column_shares=[1, 2], + ), + ) + +rerun_config = { + "blueprint": _drone_rerun_blueprint(), + "pubsubs": [LCM()], + "visual_override": { + "world/camera_info": _convert_camera_info, + "world/navigation_costmap": _convert_navigation_costmap, + }, + "static": { + "world/tf/base_link": _static_base_link, + } +} \ No newline at end of file From 4f37ced0860ccb7b09562dc79ee482dffa3da0cb Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sat, 9 May 2026 23:44:44 -0400 Subject: [PATCH 04/18] Rest of the things remain same as g1 as well -- maybe voxel is not needed, will decided to remove later --- .../primitive/drone_primitive_no_nav.py | 79 ++++++++++++++++++- 1 file changed, 78 insertions(+), 1 deletion(-) diff --git a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py index 2502ce469d..5e89103d1d 100644 --- a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py +++ b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py @@ -84,4 +84,81 @@ def _drone_rerun_blueprint() -> Any: "static": { "world/tf/base_link": _static_base_link, } -} \ No newline at end of file +} + +if global_config.viewer == "foxglove": + from dimos.robot.foxglove_bridge import FoxgloveBridge + + _with_vis = autoconnect(FoxgloveBridge.blueprint()) +elif global_config.viewer.startswith("rerun"): + from dimos.visualization.rerun.bridge import RerunBridgeModule, _resolve_viewer_mode + + _with_vis = autoconnect( + RerunBridgeModule.blueprint(viewer_mode=_resolve_viewer_mode((), **rerun_config)), + ) +else: + _with_vis = autoconnect() + +def _create_webcam() -> Webcam: + return Webcam( + camera_index=0, + fps=15, + stereo_slice="left", + camera_info=zed.CameraInfo.SingleWebcam, + ) + +_camera = ( + autoconnect( + CameraModule.blueprint( + 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=_create_webcam + ), + ) + if not global_config.simulation + else autoconnect() +) + +#Note: Maybe Voxel and Lidar might not be needed + +drone_primitive_no_nav = ( + autoconnect( + _with_vis, + _camera, + VoxelGridMapper.blueprint(), + CostMapper.blueprint(), + WavefrontFrontierExplorer.blueprint(), + #Visualization + WebsocketVisModule.blueprint(), + ) + .global_config(n_workers=4, robot_model="drone") + .transports( + { + # Movement command + ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), + # State Estimation from ROS + ("state_estimation", Odometry): LCMTransport("/state_estimation", Odometry), + # Odometry output from ROSNavigationModule + ("odom", 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 + ("color_image", Image): LCMTransport("/color_image", Image), + ("camera_info", CameraInfo): LCMTransport("/camera_info", CameraInfo), + } + ) +) + +__all__ = ["drone_primitive_no_nav"] \ No newline at end of file From 14c1995dbed92fe06567d4a099deaebf4b0f6fdd Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 00:14:53 -0400 Subject: [PATCH 05/18] Added the imports, making the required files -- MujocoConnection and SimOdometry --- dimos/robot/drone/sim.py | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 dimos/robot/drone/sim.py diff --git a/dimos/robot/drone/sim.py b/dimos/robot/drone/sim.py new file mode 100644 index 0000000000..773a9ec271 --- /dev/null +++ b/dimos/robot/drone/sim.py @@ -0,0 +1,25 @@ +import threading +from threading import Thread +import time +from typing import Any + +from pydantic import Field +from reactivex.disposable import Disposable + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.core import rpc +from dimos.core.module import ModuleConfig +from dimos.core.stream import In, Out +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Transform import Transform +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +from dimos.robot.drone.connection_module import DroneConnectionModule +from dimos.robot.drone.mujoco_connection import MujocoConnection +from dimos.robot.drone.type.odometry import Odometry as SimOdometry +from dimos.utils.logging_config import setup_logger From e66cd00448cc4c712ea2cc4b9409fe7ddda18e84 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 00:20:42 -0400 Subject: [PATCH 06/18] Also needs Odometry in the imports -- creating that now... --- dimos/robot/drone/mujoco_connection.py | 45 ++++++++++++++++++++++++++ 1 file changed, 45 insertions(+) create mode 100644 dimos/robot/drone/mujoco_connection.py diff --git a/dimos/robot/drone/mujoco_connection.py b/dimos/robot/drone/mujoco_connection.py new file mode 100644 index 0000000000..79145742b1 --- /dev/null +++ b/dimos/robot/drone/mujoco_connection.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 + +import atexit +import base64 +from collections.abc import Callable +import functools +import json +import pickle +import subprocess +import sys +import threading +import time +from typing import Any, TypeVar +import weakref + +import numpy as np +from numpy.typing import NDArray +from reactivex import Observable +from reactivex.abc import ObserverBase, SchedulerBase +from reactivex.disposable import Disposable + +from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT +from dimos.core.global_config import GlobalConfig +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Twist import Twist +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo +from dimos.msgs.sensor_msgs.Image import Image +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + +from dimos.robot.drone.type.odometry import Odometry + +from dimos.simulation.mujoco.constants import ( + LAUNCHER_PATH, + LIDAR_FPS, + VIDEO_CAMERA_FOV, + VIDEO_FPS, + VIDEO_HEIGHT, + VIDEO_WIDTH, +) + +from dimos.simulation.mujoco.shared_memory import ShmWriter +from dimos.utils.data import get_data +from dimos.utils.logging_config import setup_logger + From e4e9cf917945d33f5f7b8dae9cc99f911eef1399 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 00:25:53 -0400 Subject: [PATCH 07/18] odometry needs timeseries to be created... --- dimos/robot/drone/type/odometry.py | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 dimos/robot/drone/type/odometry.py diff --git a/dimos/robot/drone/type/odometry.py b/dimos/robot/drone/type/odometry.py new file mode 100644 index 0000000000..ffba11b160 --- /dev/null +++ b/dimos/robot/drone/type/odometry.py @@ -0,0 +1,9 @@ +from typing import Literal, TypedDict + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3 +from dimos.robot.drone.type.timeseries import ( + Timestamped, +) +from dimos.types.timestamped import to_timestamp \ No newline at end of file From 2c7b6aa5fe9bc59fa1e5a54e9a3482f5c5b86997 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 01:01:14 -0400 Subject: [PATCH 08/18] realised while referring that a new Odometry is not required -- removing the file, updating imports, continuing mujoco_connection for drone --- dimos/robot/drone/mujoco_connection.py | 2 +- dimos/robot/drone/sim.py | 2 +- dimos/robot/drone/type/odometry.py | 9 --------- 3 files changed, 2 insertions(+), 11 deletions(-) delete mode 100644 dimos/robot/drone/type/odometry.py diff --git a/dimos/robot/drone/mujoco_connection.py b/dimos/robot/drone/mujoco_connection.py index 79145742b1..6bab1be3cc 100644 --- a/dimos/robot/drone/mujoco_connection.py +++ b/dimos/robot/drone/mujoco_connection.py @@ -28,7 +28,7 @@ from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.robot.drone.type.odometry import Odometry +from dimos.robot.unitree.type.odometry import Odometry as SimOdometry from dimos.simulation.mujoco.constants import ( LAUNCHER_PATH, diff --git a/dimos/robot/drone/sim.py b/dimos/robot/drone/sim.py index 773a9ec271..3b65f9a63d 100644 --- a/dimos/robot/drone/sim.py +++ b/dimos/robot/drone/sim.py @@ -21,5 +21,5 @@ from dimos.robot.drone.connection_module import DroneConnectionModule from dimos.robot.drone.mujoco_connection import MujocoConnection -from dimos.robot.drone.type.odometry import Odometry as SimOdometry +from dimos.robot.unitree.type.odometry import Odometry as SimOdometry from dimos.utils.logging_config import setup_logger diff --git a/dimos/robot/drone/type/odometry.py b/dimos/robot/drone/type/odometry.py deleted file mode 100644 index ffba11b160..0000000000 --- a/dimos/robot/drone/type/odometry.py +++ /dev/null @@ -1,9 +0,0 @@ -from typing import Literal, TypedDict - -from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.robot.drone.type.timeseries import ( - Timestamped, -) -from dimos.types.timestamped import to_timestamp \ No newline at end of file From e5c4ff3f96a492bba515b2754ebc5cc1f10d6050 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 01:37:08 -0400 Subject: [PATCH 09/18] Mujoco_connection -- trying to use the one in unitree directly as well -- will create one later if required --- dimos/robot/drone/mujoco_connection.py | 45 -------------------------- 1 file changed, 45 deletions(-) delete mode 100644 dimos/robot/drone/mujoco_connection.py diff --git a/dimos/robot/drone/mujoco_connection.py b/dimos/robot/drone/mujoco_connection.py deleted file mode 100644 index 6bab1be3cc..0000000000 --- a/dimos/robot/drone/mujoco_connection.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env python3 - -import atexit -import base64 -from collections.abc import Callable -import functools -import json -import pickle -import subprocess -import sys -import threading -import time -from typing import Any, TypeVar -import weakref - -import numpy as np -from numpy.typing import NDArray -from reactivex import Observable -from reactivex.abc import ObserverBase, SchedulerBase -from reactivex.disposable import Disposable - -from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT -from dimos.core.global_config import GlobalConfig -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo -from dimos.msgs.sensor_msgs.Image import Image -from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - -from dimos.robot.unitree.type.odometry import Odometry as SimOdometry - -from dimos.simulation.mujoco.constants import ( - LAUNCHER_PATH, - LIDAR_FPS, - VIDEO_CAMERA_FOV, - VIDEO_FPS, - VIDEO_HEIGHT, - VIDEO_WIDTH, -) - -from dimos.simulation.mujoco.shared_memory import ShmWriter -from dimos.utils.data import get_data -from dimos.utils.logging_config import setup_logger - From 4e6dfb4cde6f5fdf3769ac9fc43c4781b975ef1a Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 01:37:48 -0400 Subject: [PATCH 10/18] using same Mujocu connection as unitree -- might change later if required --- dimos/robot/drone/sim.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/drone/sim.py b/dimos/robot/drone/sim.py index 3b65f9a63d..66bd00ae32 100644 --- a/dimos/robot/drone/sim.py +++ b/dimos/robot/drone/sim.py @@ -20,6 +20,6 @@ from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.robot.drone.connection_module import DroneConnectionModule -from dimos.robot.drone.mujoco_connection import MujocoConnection +from dimos.robot.unitree.mujoco_connection import MujocoConnection from dimos.robot.unitree.type.odometry import Odometry as SimOdometry from dimos.utils.logging_config import setup_logger From 8ad91447e6d5080bfe104f40ecd16f659c592337 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 17:27:33 -0400 Subject: [PATCH 11/18] Basic -- rerun io starts with camera and 3D -- bugs are there, solving now. Ran test all blueprints for populating all_blueprints. Added the bitcraze_crazyflie_2 in model and a new class in policy --- dimos/robot/all_blueprints.py | 3 + .../primitive/drone_primitive_no_nav.py | 4 +- dimos/robot/drone/sim.py | 77 +++++++++++++++++++ dimos/simulation/mujoco/model.py | 8 +- dimos/simulation/mujoco/policy.py | 13 ++++ 5 files changed, 102 insertions(+), 3 deletions(-) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index 96ce1c6784..e8a8bb1903 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -48,6 +48,8 @@ "demo-skill": "dimos.agents.skills.demo_skill:demo_skill", "drone-agentic": "dimos.robot.drone.blueprints.agentic.drone_agentic:drone_agentic", "drone-basic": "dimos.robot.drone.blueprints.basic.drone_basic:drone_basic", + "drone-basic-sim": "dimos.robot.drone.blueprints.basic.drone_basic_sim:drone_basic_sim", + "drone-primitive-no-nav": "dimos.robot.drone.blueprints.primitive.drone_primitive_no_nav:drone_primitive_no_nav", "dual-xarm6-planner": "dimos.manipulation.blueprints:dual_xarm6_planner", "keyboard-teleop-piper": "dimos.robot.manipulators.piper.blueprints:keyboard_teleop_piper", "keyboard-teleop-xarm6": "dimos.robot.manipulators.xarm.blueprints:keyboard_teleop_xarm6", @@ -117,6 +119,7 @@ "detection3-d-module": "dimos.perception.detection.module3D.Detection3DModule", "drone-camera-module": "dimos.robot.drone.camera_module.DroneCameraModule", "drone-connection-module": "dimos.robot.drone.connection_module.DroneConnectionModule", + "drone-sim-connection": "dimos.robot.drone.sim.DroneSimConnection", "drone-tracking-module": "dimos.robot.drone.drone_tracking_module.DroneTrackingModule", "embedding-memory": "dimos.memory.embedding.EmbeddingMemory", "emitter-module": "dimos.utils.demo_image_encoding.EmitterModule", diff --git a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py index 5e89103d1d..99c425b763 100644 --- a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py +++ b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py @@ -75,7 +75,7 @@ def _drone_rerun_blueprint() -> Any: ) rerun_config = { - "blueprint": _drone_rerun_blueprint(), + "blueprint": _drone_rerun_blueprint, "pubsubs": [LCM()], "visual_override": { "world/camera_info": _convert_camera_info, @@ -94,7 +94,7 @@ def _drone_rerun_blueprint() -> Any: from dimos.visualization.rerun.bridge import RerunBridgeModule, _resolve_viewer_mode _with_vis = autoconnect( - RerunBridgeModule.blueprint(viewer_mode=_resolve_viewer_mode((), **rerun_config)), + RerunBridgeModule.blueprint(viewer_mode=_resolve_viewer_mode(), **rerun_config), ) else: _with_vis = autoconnect() diff --git a/dimos/robot/drone/sim.py b/dimos/robot/drone/sim.py index 66bd00ae32..184f36af1b 100644 --- a/dimos/robot/drone/sim.py +++ b/dimos/robot/drone/sim.py @@ -23,3 +23,80 @@ from dimos.robot.unitree.mujoco_connection import MujocoConnection from dimos.robot.unitree.type.odometry import Odometry as SimOdometry from dimos.utils.logging_config import setup_logger + +logger = setup_logger() + +class DroneSimConfig(ModuleConfig): + ip: str = Field(default_factory=lambda m: m["g"].robot_ip) + +class DroneSimConnection(DroneConnectionModule): + config: DroneSimConfig + cmd_vel: In[Twist] + lidar: Out[PointCloud2] + odom: Out[PoseStamped] + color_image: Out[Image] + camera_info: Out[CameraInfo] + connection: MujocoConnection | None = None + _camera_info_thread: Thread | None = None + + def __init__(self, **kwargs: Any) -> None: + super().__init__(**kwargs) + self._stop_event = threading.Event() + + @rpc + def start(self) -> None: + super().start() + + from dimos.robot.unitree.mujoco_connection import MujocoConnection + + self.connection = MujocoConnection(self.config.g) + assert self.connection is not None + self.connection.start() + + self.register_disposable(Disposable(self.cmd_vel.subscribe(self.move))) + self.register_disposable(self.connection.odom_stream().subscribe(self._publish_sim_odom)) + self.register_disposable(self.connection.lidar_stream().subscribe(self.lidar.publish)) + self.register_disposable(self.connection.video_stream().subscribe(self.color_image.publish)) + + self._camera_info_thread = Thread( + target=self._publish_camera_info_loop, + daemon=True, + ) + self._camera_info_thread.start() + + @rpc + def stop(self) -> None: + self._stop_event.set() + assert self.connection is not None + self.connection.stop() + if self._camera_info_thread and self._camera_info_thread.is_alive(): + self._camera_info_thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) + super().stop() + + def _publish_camera_info_loop(self) -> None: + assert self.connection is not None + info = self.connection.camera_info_static + while not self._stop_event.is_set(): + self.camera_info.publish(info) + self._stop_event.wait(1.0) + + def _publish_sim_odom(self, msg: SimOdometry) -> None: + self._publish_tf( + PoseStamped( + ts=msg.ts, + frame_id=msg.frame_id, + position=msg.position, + orientation=msg.orientation, + ) + ) + + @rpc + def move(self, twist: Twist, duration: float = 0.0) -> None: + assert self.connection is not None + self.connection.move(twist, duration) + + @rpc + def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: + logger.info(f"Publishing request to topic: {topic} with data: {data}") + assert self.connection is not None + return self.connection.publish_request(topic, data) \ No newline at end of file diff --git a/dimos/simulation/mujoco/model.py b/dimos/simulation/mujoco/model.py index bc309b7307..b4372c9503 100644 --- a/dimos/simulation/mujoco/model.py +++ b/dimos/simulation/mujoco/model.py @@ -27,7 +27,7 @@ from dimos.mapping.occupancy.extrude_occupancy import generate_mujoco_scene from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.simulation.mujoco.input_controller import InputController -from dimos.simulation.mujoco.policy import G1OnnxController, Go1OnnxController, OnnxController +from dimos.simulation.mujoco.policy import G1OnnxController, Go1OnnxController, OnnxController, DroneController from dimos.utils.data import get_data @@ -52,6 +52,10 @@ def get_assets() -> dict[str, bytes]: mjx_env.update_assets(assets, person_dir, "*.obj") mjx_env.update_assets(assets, person_dir, "*.png") + # From: https://github.com/google-deepmind/mujoco_menagerie/tree/main/bitcraze_crazyflie_2 + mjx_env.update_assets(assets, mjx_env.MENAGERIE_PATH / "bitcraze_crazyflie_2") + mjx_env.update_assets(assets, mjx_env.MENAGERIE_PATH / "bitcraze_crazyflie_2" / "assets") + return assets @@ -90,6 +94,8 @@ def load_model( policy: OnnxController = Go1OnnxController(**params) case "unitree_g1": policy = G1OnnxController(**params, drift_compensation=[-0.18, 0.0, -0.09]) + case "bitcraze_crazyflie_2": + policy = DroneController(**params) case _: raise ValueError(f"Unknown robot policy: {robot}") diff --git a/dimos/simulation/mujoco/policy.py b/dimos/simulation/mujoco/policy.py index 0a792baf1a..d78cd3196a 100644 --- a/dimos/simulation/mujoco/policy.py +++ b/dimos/simulation/mujoco/policy.py @@ -154,3 +154,16 @@ def get_obs(self, model: mujoco.MjModel, data: mujoco.MjData) -> np.ndarray[Any, def _post_control_update(self) -> None: phase_tp1 = self._phase + self._phase_dt self._phase = np.fmod(phase_tp1 + np.pi, 2 * np.pi) - np.pi + +class DroneController(): + def __init__( + self, + input_controller: InputController, + ) -> None: + self._input_controller = input_controller + + def get_obs(self, model: mujoco.MjModel, data: mujoco.MjData) -> np.ndarray[Any, Any]: + + # Check data.sensor names to determine which drone model is being used + command = self._input_controller.get_command() + return command.astype(np.float32) \ No newline at end of file From 3c6264a7e5f69b62f7d1c28dabfc74de32f35fbd Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 21:14:56 -0400 Subject: [PATCH 12/18] Jumping over , the connection Module for now and directly using Module.start(self) -- need to verify if this is temporary solution, refactoring might be needed --- dimos/robot/drone/sim.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dimos/robot/drone/sim.py b/dimos/robot/drone/sim.py index 184f36af1b..c13c4f30fa 100644 --- a/dimos/robot/drone/sim.py +++ b/dimos/robot/drone/sim.py @@ -8,7 +8,7 @@ from dimos.constants import DEFAULT_THREAD_JOIN_TIMEOUT from dimos.core.core import rpc -from dimos.core.module import ModuleConfig +from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped from dimos.msgs.geometry_msgs.Quaternion import Quaternion @@ -45,7 +45,7 @@ def __init__(self, **kwargs: Any) -> None: @rpc def start(self) -> None: - super().start() + Module.start(self) from dimos.robot.unitree.mujoco_connection import MujocoConnection @@ -71,7 +71,7 @@ def stop(self) -> None: self.connection.stop() if self._camera_info_thread and self._camera_info_thread.is_alive(): self._camera_info_thread.join(timeout=DEFAULT_THREAD_JOIN_TIMEOUT) - super().stop() + Module.stop(self) def _publish_camera_info_loop(self) -> None: assert self.connection is not None From 43de2a036c02a726f5c49d9fc43bbd2d8b68c787 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Sun, 10 May 2026 22:00:10 -0400 Subject: [PATCH 13/18] Starting to make this drone specific -- smaller drone --- .../robot/drone/blueprints/primitive/drone_primitive_no_nav.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py index 99c425b763..c347d8c5fb 100644 --- a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py +++ b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py @@ -47,7 +47,7 @@ def _convert_navigation_costmap(grid: Any) -> Any: def _static_base_link(rr: Any) -> list[Any]: return [ rr.Boxes3D( - half_sizes=[0.2, 0.15, 0.75], + half_sizes=[0.05, 0.05, 0.02], colors=[(0, 255, 127)], fill_mode="MajorWireframe", ), From 3e285b049bfb170f961a84c929a474cd43e518df Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Mon, 11 May 2026 00:05:47 -0400 Subject: [PATCH 14/18] No Camera or WaveFront Explorer in Drone -- commented --- .../primitive/drone_primitive_no_nav.py | 90 +++++++++---------- 1 file changed, 45 insertions(+), 45 deletions(-) diff --git a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py index c347d8c5fb..c23aab04bb 100644 --- a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py +++ b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py @@ -9,24 +9,24 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport -from dimos.hardware.sensors.camera.module import CameraModule -from dimos.hardware.sensors.camera.webcam import Webcam -from dimos.hardware.sensors.camera.zed import compat as zed +# from dimos.hardware.sensors.camera.module import CameraModule +# from dimos.hardware.sensors.camera.webcam import Webcam +# from dimos.hardware.sensors.camera.zed import compat as zed from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels import VoxelGridMapper from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform +# from dimos.msgs.geometry_msgs.Quaternion import Quaternion +# from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 -from dimos.msgs.nav_msgs.Odometry import Odometry -from dimos.msgs.nav_msgs.Path import Path +# from dimos.msgs.geometry_msgs.Vector3 import Vector3 +# from dimos.msgs.nav_msgs.Odometry import Odometry +# from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 -from dimos.msgs.std_msgs.Bool import Bool -from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( - WavefrontFrontierExplorer, -) +# from dimos.msgs.std_msgs.Bool import Bool +# from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( +# WavefrontFrontierExplorer, +# ) from dimos.protocol.pubsub.impl.lcmpubsub import LCM from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule @@ -99,61 +99,61 @@ def _drone_rerun_blueprint() -> Any: else: _with_vis = autoconnect() -def _create_webcam() -> Webcam: - return Webcam( - camera_index=0, - fps=15, - stereo_slice="left", - camera_info=zed.CameraInfo.SingleWebcam, - ) - -_camera = ( - autoconnect( - CameraModule.blueprint( - 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=_create_webcam - ), - ) - if not global_config.simulation - else autoconnect() -) +# def _create_webcam() -> Webcam: +# return Webcam( +# camera_index=0, +# fps=15, +# stereo_slice="left", +# camera_info=zed.CameraInfo.SingleWebcam, +# ) + +# _camera = ( +# autoconnect( +# CameraModule.blueprint( +# 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=_create_webcam +# ), +# ) +# if not global_config.simulation +# else autoconnect() +# ) #Note: Maybe Voxel and Lidar might not be needed drone_primitive_no_nav = ( autoconnect( _with_vis, - _camera, + # _camera, VoxelGridMapper.blueprint(), CostMapper.blueprint(), - WavefrontFrontierExplorer.blueprint(), + # WavefrontFrontierExplorer.blueprint(), #Visualization WebsocketVisModule.blueprint(), ) - .global_config(n_workers=4, robot_model="drone") + .global_config(n_workers=4, robot_model="drone", mujoco_room="empty") .transports( { # Movement command ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), # State Estimation from ROS - ("state_estimation", Odometry): LCMTransport("/state_estimation", Odometry), + # ("state_estimation", Odometry): LCMTransport("/state_estimation", Odometry), # Odometry output from ROSNavigationModule ("odom", 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), + # ("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), + # ("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), + # ("goal_reached", Bool): LCMTransport("/goal_reached", Bool), + # ("cancel_goal", Bool): LCMTransport("/cancel_goal", Bool), # Camera topics ("color_image", Image): LCMTransport("/color_image", Image), ("camera_info", CameraInfo): LCMTransport("/camera_info", CameraInfo), From ced090d6366cf37fa54d2a15c27e8b092d3a120b Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Mon, 11 May 2026 00:59:51 -0400 Subject: [PATCH 15/18] Drone basic sim working -- height fixe for now -- removed commented code -- added drone to mujoco_process -- fixed Drone Controller policy --- .../primitive/drone_primitive_no_nav.py | 38 ------------------- dimos/simulation/mujoco/mujoco_process.py | 6 +++ dimos/simulation/mujoco/policy.py | 20 ++++++++-- 3 files changed, 22 insertions(+), 42 deletions(-) diff --git a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py index c23aab04bb..b2ef648a4d 100644 --- a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py +++ b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py @@ -99,39 +99,11 @@ def _drone_rerun_blueprint() -> Any: else: _with_vis = autoconnect() -# def _create_webcam() -> Webcam: -# return Webcam( -# camera_index=0, -# fps=15, -# stereo_slice="left", -# camera_info=zed.CameraInfo.SingleWebcam, -# ) - -# _camera = ( -# autoconnect( -# CameraModule.blueprint( -# 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=_create_webcam -# ), -# ) -# if not global_config.simulation -# else autoconnect() -# ) - -#Note: Maybe Voxel and Lidar might not be needed - drone_primitive_no_nav = ( autoconnect( _with_vis, - # _camera, VoxelGridMapper.blueprint(), CostMapper.blueprint(), - # WavefrontFrontierExplorer.blueprint(), #Visualization WebsocketVisModule.blueprint(), ) @@ -140,20 +112,10 @@ def _drone_rerun_blueprint() -> Any: { # Movement command ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), - # State Estimation from ROS - # ("state_estimation", Odometry): LCMTransport("/state_estimation", Odometry), # Odometry output from ROSNavigationModule ("odom", 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 ("color_image", Image): LCMTransport("/color_image", Image), ("camera_info", CameraInfo): LCMTransport("/camera_info", CameraInfo), diff --git a/dimos/simulation/mujoco/mujoco_process.py b/dimos/simulation/mujoco/mujoco_process.py index 2644dddd36..85b83eff9c 100755 --- a/dimos/simulation/mujoco/mujoco_process.py +++ b/dimos/simulation/mujoco/mujoco_process.py @@ -63,6 +63,8 @@ def get_command(self) -> NDArray[Any]: self._command[0] = linear[0] # forward/backward self._command[1] = linear[1] # left/right self._command[2] = angular[2] # rotation + else: + self._command[:] = 0 result: NDArray[Any] = self._command.copy() return result @@ -75,6 +77,8 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: robot_name = config.robot_model or "unitree_go1" if robot_name == "unitree_go2": robot_name = "unitree_go1" + if robot_name == "drone": + robot_name = "bitcraze_crazyflie_2" controller = MockController(shm) model, data = load_model(controller, robot=robot_name, scene_xml=load_scene_xml(config)) @@ -87,6 +91,8 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: z = 0.3 case "unitree_g1": z = 0.8 + case "bitcraze_crazyflie_2": + z = 0.5 case _: z = 0 diff --git a/dimos/simulation/mujoco/policy.py b/dimos/simulation/mujoco/policy.py index d78cd3196a..d2bc5829df 100644 --- a/dimos/simulation/mujoco/policy.py +++ b/dimos/simulation/mujoco/policy.py @@ -155,15 +155,27 @@ def _post_control_update(self) -> None: phase_tp1 = self._phase + self._phase_dt self._phase = np.fmod(phase_tp1 + np.pi, 2 * np.pi) - np.pi +_DRONE_HOVER_THRUST = 0.26487 # From cf2.xml file +_DRONE_MOMENT_SCALE = 0.005 + class DroneController(): def __init__( self, input_controller: InputController, + **kwargs: Any, ) -> None: self._input_controller = input_controller - + def get_obs(self, model: mujoco.MjModel, data: mujoco.MjData) -> np.ndarray[Any, Any]: - - # Check data.sensor names to determine which drone model is being used command = self._input_controller.get_command() - return command.astype(np.float32) \ No newline at end of file + return command.astype(np.float32) + + def get_control(self, model: mujoco.MjModel, data: mujoco.MjData) -> None: + command = self._input_controller.get_command() + forward = float(command[0]) + lateral = float(command[1]) + yaw = float(command[2]) + data.ctrl[0] = _DRONE_HOVER_THRUST + data.ctrl[1] = lateral * _DRONE_MOMENT_SCALE # x_moment (roll) + data.ctrl[2] = -forward * _DRONE_MOMENT_SCALE # y_moment (pitch) + data.ctrl[3] = -yaw * _DRONE_MOMENT_SCALE # z_moment (yaw) \ No newline at end of file From 58357f908ecf10c49c09b738740b77b267a97afa Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Mon, 11 May 2026 05:12:28 -0400 Subject: [PATCH 16/18] vis moved to vis_module in main merge -- making drone sim work adter merge (same as g1) --- .../primitive/drone_primitive_no_nav.py | 15 ++------------- 1 file changed, 2 insertions(+), 13 deletions(-) diff --git a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py index b2ef648a4d..fa01bc9694 100644 --- a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py +++ b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py @@ -28,6 +28,7 @@ # WavefrontFrontierExplorer, # ) from dimos.protocol.pubsub.impl.lcmpubsub import LCM +from dimos.visualization.vis_module import vis_module from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule def _convert_camera_info(camera_info: Any) -> Any: @@ -76,7 +77,6 @@ def _drone_rerun_blueprint() -> Any: rerun_config = { "blueprint": _drone_rerun_blueprint, - "pubsubs": [LCM()], "visual_override": { "world/camera_info": _convert_camera_info, "world/navigation_costmap": _convert_navigation_costmap, @@ -86,18 +86,7 @@ def _drone_rerun_blueprint() -> Any: } } -if global_config.viewer == "foxglove": - from dimos.robot.foxglove_bridge import FoxgloveBridge - - _with_vis = autoconnect(FoxgloveBridge.blueprint()) -elif global_config.viewer.startswith("rerun"): - from dimos.visualization.rerun.bridge import RerunBridgeModule, _resolve_viewer_mode - - _with_vis = autoconnect( - RerunBridgeModule.blueprint(viewer_mode=_resolve_viewer_mode(), **rerun_config), - ) -else: - _with_vis = autoconnect() +_with_vis = vis_module(viewer_backend=global_config.viewer, rerun_config=rerun_config) drone_primitive_no_nav = ( autoconnect( From c782e76e802893211884a5561434e25c272aee12 Mon Sep 17 00:00:00 2001 From: Shreyas Raorane Date: Thu, 14 May 2026 00:51:31 -0400 Subject: [PATCH 17/18] camera and lidar camera is not available in cf2 -- was getting around ut by addind to to xml -- using original xml which comes with mujoco playground and adding additional checks instead --- dimos/simulation/mujoco/model.py | 9 +++++++-- dimos/simulation/mujoco/mujoco_process.py | 8 ++++---- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/dimos/simulation/mujoco/model.py b/dimos/simulation/mujoco/model.py index b4372c9503..13ff432ef2 100644 --- a/dimos/simulation/mujoco/model.py +++ b/dimos/simulation/mujoco/model.py @@ -80,9 +80,14 @@ def load_model( n_substeps = round(ctrl_dt / sim_dt) model.opt.timestep = sim_dt + if robot == "cf2": + keyframe_name = "hover" + else: # For unitree_go1 and unitree_g1 -- default + keyframe_name = "home" + params = { "policy_path": (_get_data_dir() / f"{robot}_policy.onnx").as_posix(), - "default_angles": np.array(model.keyframe("home").qpos[7:]), + "default_angles": np.array(model.keyframe(keyframe_name).qpos[7:]), "n_substeps": n_substeps, "action_scale": 0.5, "input_controller": input_device, @@ -94,7 +99,7 @@ def load_model( policy: OnnxController = Go1OnnxController(**params) case "unitree_g1": policy = G1OnnxController(**params, drift_compensation=[-0.18, 0.0, -0.09]) - case "bitcraze_crazyflie_2": + case "cf2": policy = DroneController(**params) case _: raise ValueError(f"Unknown robot policy: {robot}") diff --git a/dimos/simulation/mujoco/mujoco_process.py b/dimos/simulation/mujoco/mujoco_process.py index 85b83eff9c..4717847b3b 100755 --- a/dimos/simulation/mujoco/mujoco_process.py +++ b/dimos/simulation/mujoco/mujoco_process.py @@ -78,7 +78,7 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: if robot_name == "unitree_go2": robot_name = "unitree_go1" if robot_name == "drone": - robot_name = "bitcraze_crazyflie_2" + robot_name = "cf2" controller = MockController(shm) model, data = load_model(controller, robot=robot_name, scene_xml=load_scene_xml(config)) @@ -91,7 +91,7 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: z = 0.3 case "unitree_g1": z = 0.8 - case "bitcraze_crazyflie_2": + case "cf2": z = 0.5 case _: z = 0 @@ -160,14 +160,14 @@ def _run_simulation(config: GlobalConfig, shm: ShmReader) -> None: current_time = time.time() # Video rendering - if current_time - last_video_time >= video_interval: + if camera_id != -1 and current_time - last_video_time >= video_interval: rgb_renderer.update_scene(data, camera=camera_id, scene_option=scene_option) pixels = rgb_renderer.render() shm.write_video(pixels) last_video_time = current_time # Lidar/depth rendering - if current_time - last_lidar_time >= lidar_interval: + if lidar_camera_id != -1 and current_time - last_lidar_time >= lidar_interval: # Render all depth cameras depth_renderer.update_scene(data, camera=lidar_camera_id, scene_option=scene_option) depth_front = depth_renderer.render() From 6fa1acd533a362c8a6ed6424ed12175179a032cf Mon Sep 17 00:00:00 2001 From: "autofix-ci[bot]" <114827586+autofix-ci[bot]@users.noreply.github.com> Date: Thu, 14 May 2026 22:14:35 +0000 Subject: [PATCH 18/18] [autofix.ci] apply automated fixes --- .../drone/blueprints/basic/drone_basic_sim.py | 7 ++---- .../primitive/drone_primitive_no_nav.py | 22 +++++++++++++------ dimos/robot/drone/sim.py | 11 ++++------ dimos/simulation/mujoco/model.py | 11 +++++++--- dimos/simulation/mujoco/policy.py | 18 ++++++++------- 5 files changed, 39 insertions(+), 30 deletions(-) diff --git a/dimos/robot/drone/blueprints/basic/drone_basic_sim.py b/dimos/robot/drone/blueprints/basic/drone_basic_sim.py index 4a2c287933..3834e133b5 100644 --- a/dimos/robot/drone/blueprints/basic/drone_basic_sim.py +++ b/dimos/robot/drone/blueprints/basic/drone_basic_sim.py @@ -1,11 +1,8 @@ #!/usr/bin/env python3 -""" Basic Drone Sim Blueprint with sim connection and visualization""" - -from typing import TYPE_CHECKING +"""Basic Drone Sim Blueprint with sim connection and visualization""" from dimos.core.coordination.blueprints import autoconnect -from dimos.core.global_config import global_config from dimos.navigation.replanning_a_star.module import ReplanningAStarPlanner from dimos.robot.drone.blueprints.primitive.drone_primitive_no_nav import ( drone_primitive_no_nav, @@ -18,4 +15,4 @@ ReplanningAStarPlanner.blueprint(), ) -__all__ = ["drone_basic_sim"] \ No newline at end of file +__all__ = ["drone_basic_sim"] diff --git a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py index fa01bc9694..a5870f3981 100644 --- a/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py +++ b/dimos/robot/drone/blueprints/primitive/drone_primitive_no_nav.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -""" Minimal Drone Stack without navigation. An attempt to create a minimal stack""" +"""Minimal Drone Stack without navigation. An attempt to create a minimal stack""" from typing import Any @@ -9,34 +9,39 @@ from dimos.core.coordination.blueprints import autoconnect from dimos.core.global_config import global_config from dimos.core.transport import LCMTransport + # from dimos.hardware.sensors.camera.module import CameraModule # from dimos.hardware.sensors.camera.webcam import Webcam # from dimos.hardware.sensors.camera.zed import compat as zed from dimos.mapping.costmapper import CostMapper from dimos.mapping.voxels import VoxelGridMapper from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + # from dimos.msgs.geometry_msgs.Quaternion import Quaternion # from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist + # from dimos.msgs.geometry_msgs.Vector3 import Vector3 # from dimos.msgs.nav_msgs.Odometry import Odometry # from dimos.msgs.nav_msgs.Path import Path from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 + # from dimos.msgs.std_msgs.Bool import Bool # from dimos.navigation.frontier_exploration.wavefront_frontier_goal_selector import ( # WavefrontFrontierExplorer, # ) -from dimos.protocol.pubsub.impl.lcmpubsub import LCM from dimos.visualization.vis_module import vis_module from dimos.web.websocket_vis.websocket_vis_module import WebsocketVisModule + def _convert_camera_info(camera_info: Any) -> Any: return camera_info.to_rerun( image_topic="/world/color_image", optical_frame="camera_optical", ) + def _convert_navigation_costmap(grid: Any) -> Any: return grid.to_rerun( colormap="Accent", @@ -45,6 +50,7 @@ def _convert_navigation_costmap(grid: Any) -> Any: background="#484981", ) + def _static_base_link(rr: Any) -> list[Any]: return [ rr.Boxes3D( @@ -55,6 +61,7 @@ def _static_base_link(rr: Any) -> list[Any]: rr.Transform3D(parent_frame="tf#/base_link"), ] + def _drone_rerun_blueprint() -> Any: """Same as the G1 rerun blueprint but with a different base link visualization.""" import rerun as rr @@ -75,6 +82,7 @@ def _drone_rerun_blueprint() -> Any: ), ) + rerun_config = { "blueprint": _drone_rerun_blueprint, "visual_override": { @@ -83,7 +91,7 @@ def _drone_rerun_blueprint() -> Any: }, "static": { "world/tf/base_link": _static_base_link, - } + }, } _with_vis = vis_module(viewer_backend=global_config.viewer, rerun_config=rerun_config) @@ -93,13 +101,13 @@ def _drone_rerun_blueprint() -> Any: _with_vis, VoxelGridMapper.blueprint(), CostMapper.blueprint(), - #Visualization + # Visualization WebsocketVisModule.blueprint(), ) .global_config(n_workers=4, robot_model="drone", mujoco_room="empty") .transports( - { - # Movement command + { + # Movement command ("cmd_vel", Twist): LCMTransport("/cmd_vel", Twist), # Odometry output from ROSNavigationModule ("odom", PoseStamped): LCMTransport("/odom", PoseStamped), @@ -112,4 +120,4 @@ def _drone_rerun_blueprint() -> Any: ) ) -__all__ = ["drone_primitive_no_nav"] \ No newline at end of file +__all__ = ["drone_primitive_no_nav"] diff --git a/dimos/robot/drone/sim.py b/dimos/robot/drone/sim.py index c13c4f30fa..0e65a1c255 100644 --- a/dimos/robot/drone/sim.py +++ b/dimos/robot/drone/sim.py @@ -1,6 +1,5 @@ -import threading +import threading from threading import Thread -import time from typing import Any from pydantic import Field @@ -11,14 +10,10 @@ from dimos.core.module import Module, ModuleConfig from dimos.core.stream import In, Out from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped -from dimos.msgs.geometry_msgs.Quaternion import Quaternion -from dimos.msgs.geometry_msgs.Transform import Transform from dimos.msgs.geometry_msgs.Twist import Twist -from dimos.msgs.geometry_msgs.Vector3 import Vector3 from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 - from dimos.robot.drone.connection_module import DroneConnectionModule from dimos.robot.unitree.mujoco_connection import MujocoConnection from dimos.robot.unitree.type.odometry import Odometry as SimOdometry @@ -26,9 +21,11 @@ logger = setup_logger() + class DroneSimConfig(ModuleConfig): ip: str = Field(default_factory=lambda m: m["g"].robot_ip) + class DroneSimConnection(DroneConnectionModule): config: DroneSimConfig cmd_vel: In[Twist] @@ -99,4 +96,4 @@ def move(self, twist: Twist, duration: float = 0.0) -> None: def publish_request(self, topic: str, data: dict[str, Any]) -> dict[Any, Any]: logger.info(f"Publishing request to topic: {topic} with data: {data}") assert self.connection is not None - return self.connection.publish_request(topic, data) \ No newline at end of file + return self.connection.publish_request(topic, data) diff --git a/dimos/simulation/mujoco/model.py b/dimos/simulation/mujoco/model.py index 13ff432ef2..d8d14537a9 100644 --- a/dimos/simulation/mujoco/model.py +++ b/dimos/simulation/mujoco/model.py @@ -27,7 +27,12 @@ from dimos.mapping.occupancy.extrude_occupancy import generate_mujoco_scene from dimos.msgs.nav_msgs.OccupancyGrid import OccupancyGrid from dimos.simulation.mujoco.input_controller import InputController -from dimos.simulation.mujoco.policy import G1OnnxController, Go1OnnxController, OnnxController, DroneController +from dimos.simulation.mujoco.policy import ( + DroneController, + G1OnnxController, + Go1OnnxController, + OnnxController, +) from dimos.utils.data import get_data @@ -82,8 +87,8 @@ def load_model( if robot == "cf2": keyframe_name = "hover" - else: # For unitree_go1 and unitree_g1 -- default - keyframe_name = "home" + else: # For unitree_go1 and unitree_g1 -- default + keyframe_name = "home" params = { "policy_path": (_get_data_dir() / f"{robot}_policy.onnx").as_posix(), diff --git a/dimos/simulation/mujoco/policy.py b/dimos/simulation/mujoco/policy.py index d2bc5829df..ac97091125 100644 --- a/dimos/simulation/mujoco/policy.py +++ b/dimos/simulation/mujoco/policy.py @@ -155,14 +155,16 @@ def _post_control_update(self) -> None: phase_tp1 = self._phase + self._phase_dt self._phase = np.fmod(phase_tp1 + np.pi, 2 * np.pi) - np.pi -_DRONE_HOVER_THRUST = 0.26487 # From cf2.xml file + +_DRONE_HOVER_THRUST = 0.26487 # From cf2.xml file _DRONE_MOMENT_SCALE = 0.005 -class DroneController(): + +class DroneController: def __init__( - self, - input_controller: InputController, - **kwargs: Any, + self, + input_controller: InputController, + **kwargs: Any, ) -> None: self._input_controller = input_controller @@ -176,6 +178,6 @@ def get_control(self, model: mujoco.MjModel, data: mujoco.MjData) -> None: lateral = float(command[1]) yaw = float(command[2]) data.ctrl[0] = _DRONE_HOVER_THRUST - data.ctrl[1] = lateral * _DRONE_MOMENT_SCALE # x_moment (roll) - data.ctrl[2] = -forward * _DRONE_MOMENT_SCALE # y_moment (pitch) - data.ctrl[3] = -yaw * _DRONE_MOMENT_SCALE # z_moment (yaw) \ No newline at end of file + data.ctrl[1] = lateral * _DRONE_MOMENT_SCALE # x_moment (roll) + data.ctrl[2] = -forward * _DRONE_MOMENT_SCALE # y_moment (pitch) + data.ctrl[3] = -yaw * _DRONE_MOMENT_SCALE # z_moment (yaw)