From 9bdc98b6a2b0362d185b5f76f8923ed721224544 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sat, 21 Feb 2026 11:48:34 +0000 Subject: [PATCH 1/8] fix(g1): add video stream and camera_info to G1SimConnection G1SimConnection was missing color_image and camera_info outputs, so MuJoCo-rendered frames never reached the rerun bridge. Mirrors the Go2 connection pattern: subscribes to video_stream(), publishes camera_info in a background thread, and adds camera_optical TF frame. --- dimos/robot/unitree/g1/sim.py | 51 ++++++++++++++++++++++++++++++++--- 1 file changed, 48 insertions(+), 3 deletions(-) diff --git a/dimos/robot/unitree/g1/sim.py b/dimos/robot/unitree/g1/sim.py index 4502e62a1..f5bec1acc 100644 --- a/dimos/robot/unitree/g1/sim.py +++ b/dimos/robot/unitree/g1/sim.py @@ -13,6 +13,7 @@ # limitations under the License. +from threading import Thread import time from typing import TYPE_CHECKING, Any @@ -27,7 +28,7 @@ Twist, Vector3, ) -from dimos.msgs.sensor_msgs import PointCloud2 +from dimos.msgs.sensor_msgs import CameraInfo, Image, PointCloud2 from dimos.robot.unitree.type.odometry import Odometry as SimOdometry from dimos.utils.logging_config import setup_logger @@ -37,12 +38,34 @@ logger = setup_logger() +def _camera_info_static() -> CameraInfo: + """Default camera intrinsics for G1 MuJoCo sim camera.""" + fx, fy, cx, cy = (819.553492, 820.646595, 625.284099, 336.808987) + width, height = (1280, 720) + + return CameraInfo( + frame_id="camera_optical", + height=height, + width=width, + distortion_model="plumb_bob", + D=[0.0, 0.0, 0.0, 0.0, 0.0], + K=[fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], + R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], + P=[fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], + binning_x=0, + binning_y=0, + ) + + class G1SimConnection(Module): cmd_vel: In[Twist] lidar: Out[PointCloud2] odom: Out[PoseStamped] + color_image: Out[Image] + camera_info: Out[CameraInfo] ip: str | None _global_config: GlobalConfig + _camera_info_thread: Thread | None = None def __init__( self, @@ -69,19 +92,33 @@ def start(self) -> None: self._disposables.add(Disposable(self.cmd_vel.subscribe(self.move))) self._disposables.add(self.connection.odom_stream().subscribe(self._publish_sim_odom)) self._disposables.add(self.connection.lidar_stream().subscribe(self.lidar.publish)) + self._disposables.add(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: 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=1.0) super().stop() + def _publish_camera_info_loop(self) -> None: + while True: + self.camera_info.publish(_camera_info_static()) + time.sleep(1.0) + def _publish_tf(self, msg: PoseStamped) -> None: self.odom.publish(msg) self.tf.publish(Transform.from_pose("base_link", msg)) - # Publish camera_link transform + # Publish camera_link and camera_optical transforms camera_link = Transform( translation=Vector3(0.05, 0.0, 0.6), rotation=Quaternion(0.0, 0.0, 0.0, 1.0), @@ -90,6 +127,14 @@ def _publish_tf(self, msg: PoseStamped) -> None: ts=time.time(), ) + camera_optical = Transform( + translation=Vector3(0.0, 0.0, 0.0), + rotation=Quaternion(-0.5, 0.5, -0.5, 0.5), + frame_id="camera_link", + child_frame_id="camera_optical", + ts=time.time(), + ) + map_to_world = Transform( translation=Vector3(0.0, 0.0, 0.0), rotation=Quaternion(0.0, 0.0, 0.0, 1.0), @@ -98,7 +143,7 @@ def _publish_tf(self, msg: PoseStamped) -> None: ts=time.time(), ) - self.tf.publish(camera_link, map_to_world) + self.tf.publish(camera_link, camera_optical, map_to_world) def _publish_sim_odom(self, msg: SimOdometry) -> None: self._publish_tf( From cb77ae19f69539ff47dbc071c5bcb1d9f08324ab Mon Sep 17 00:00:00 2001 From: spomichter Date: Sat, 21 Feb 2026 11:55:18 +0000 Subject: [PATCH 2/8] fix(g1): remove /g1/ topic prefix to match Go2 and rerun overrides The rerun visual_overrides from #1334 used world/camera_info and world/color_image (matching Go2), but the G1 transports used /g1/ prefixed topics. This mismatch meant the rerun bridge never applied the camera_info pinhole overlay. Remove the /g1/ prefix from color_image and camera_info transports in the primitive blueprint and the SHM blueprint. Now matches Go2 convention and the rerun overrides work correctly. --- .../unitree/g1/blueprints/perceptive/unitree_g1_shm.py | 4 ++-- .../g1/blueprints/primitive/uintree_g1_primitive_no_nav.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py index 5d2eefad9..5ee4d4c9d 100644 --- a/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py +++ b/dimos/robot/unitree/g1/blueprints/perceptive/unitree_g1_shm.py @@ -26,13 +26,13 @@ unitree_g1.transports( { ("color_image", Image): pSHMTransport( - "/g1/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE + "/color_image", default_capacity=DEFAULT_CAPACITY_COLOR_IMAGE ), } ), foxglove_bridge( shm_channels=[ - "/g1/color_image#sensor_msgs.Image", + "/color_image#sensor_msgs.Image", ] ), ) diff --git a/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py b/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py index 2ba267521..6b35579cc 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py @@ -118,9 +118,9 @@ ("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) - ("color_image", Image): LCMTransport("/g1/color_image", Image), - ("camera_info", CameraInfo): LCMTransport("/g1/camera_info", CameraInfo), + # Camera topics + ("color_image", Image): LCMTransport("/color_image", Image), + ("camera_info", CameraInfo): LCMTransport("/camera_info", CameraInfo), } ) ) From 1d0f78019db923eb297a1bc65c8e26ba2a912173 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sat, 21 Feb 2026 12:04:45 +0000 Subject: [PATCH 3/8] fix(g1): skip webcam camera_module in simulation mode In sim, G1SimConnection now provides video from MuJoCo. The webcam camera_module in the primitive was also publishing to color_image, causing interleaved webcam + MuJoCo frames. Conditionally skip camera_module when global_config.simulation is True. --- .../primitive/uintree_g1_primitive_no_nav.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py b/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py index 6b35579cc..36bf569f7 100644 --- a/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py +++ b/dimos/robot/unitree/g1/blueprints/primitive/uintree_g1_primitive_no_nav.py @@ -76,9 +76,8 @@ case _: _with_vis = autoconnect() -uintree_g1_primitive_no_nav = ( +_camera = ( autoconnect( - _with_vis, camera_module( transform=Transform( translation=Vector3(0.05, 0.0, 0.6), # height of camera on G1 robot @@ -93,6 +92,15 @@ camera_info=zed.CameraInfo.SingleWebcam, ), ), + ) + if not global_config.simulation + else autoconnect() +) + +uintree_g1_primitive_no_nav = ( + autoconnect( + _with_vis, + _camera, voxel_mapper(voxel_size=0.1), cost_mapper(), wavefront_frontier_explorer(), From 4af3c863d788127a5e1a0b1a2bb98abf5e541b0a Mon Sep 17 00:00:00 2001 From: spomichter Date: Sat, 21 Feb 2026 12:13:30 +0000 Subject: [PATCH 4/8] fix(g1): use correct MuJoCo camera intrinsics and add thread exit condition MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Replace hardcoded 1280x720 Go2 intrinsics with computed values from MuJoCo constants (320x240, FOV=45°) matching MujocoConnection pattern - Add _stop_event to camera_info loop for clean shutdown --- dimos/robot/unitree/g1/sim.py | 34 +++++++++++++++++++++------------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/dimos/robot/unitree/g1/sim.py b/dimos/robot/unitree/g1/sim.py index f5bec1acc..ceb0493ba 100644 --- a/dimos/robot/unitree/g1/sim.py +++ b/dimos/robot/unitree/g1/sim.py @@ -13,6 +13,7 @@ # limitations under the License. +import threading from threading import Thread import time from typing import TYPE_CHECKING, Any @@ -38,22 +39,26 @@ logger = setup_logger() -def _camera_info_static() -> CameraInfo: - """Default camera intrinsics for G1 MuJoCo sim camera.""" - fx, fy, cx, cy = (819.553492, 820.646595, 625.284099, 336.808987) - width, height = (1280, 720) +def _compute_sim_camera_info() -> CameraInfo: + """Compute camera intrinsics from MuJoCo sim camera parameters.""" + import math + + from dimos.simulation.mujoco.constants import VIDEO_CAMERA_FOV, VIDEO_HEIGHT, VIDEO_WIDTH + + fovy = math.radians(VIDEO_CAMERA_FOV) + f = VIDEO_HEIGHT / (2 * math.tan(fovy / 2)) + cx = VIDEO_WIDTH / 2.0 + cy = VIDEO_HEIGHT / 2.0 return CameraInfo( frame_id="camera_optical", - height=height, - width=width, + height=VIDEO_HEIGHT, + width=VIDEO_WIDTH, distortion_model="plumb_bob", D=[0.0, 0.0, 0.0, 0.0, 0.0], - K=[fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], + K=[f, 0.0, cx, 0.0, f, cy, 0.0, 0.0, 1.0], R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], - P=[fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], - binning_x=0, - binning_y=0, + P=[f, 0.0, cx, 0.0, 0.0, f, cy, 0.0, 0.0, 0.0, 1.0, 0.0], ) @@ -77,6 +82,7 @@ def __init__( self._global_config = cfg self.ip = ip if ip is not None else self._global_config.robot_ip self.connection: MujocoConnection | None = None + self._stop_event = threading.Event() super().__init__(*args, **kwargs) @rpc @@ -102,6 +108,7 @@ def start(self) -> None: @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(): @@ -109,9 +116,10 @@ def stop(self) -> None: super().stop() def _publish_camera_info_loop(self) -> None: - while True: - self.camera_info.publish(_camera_info_static()) - time.sleep(1.0) + info = _compute_sim_camera_info() + while not self._stop_event.is_set(): + self.camera_info.publish(info) + self._stop_event.wait(1.0) def _publish_tf(self, msg: PoseStamped) -> None: self.odom.publish(msg) From 921cc9cd3620a457d54792caed861d5068e04e32 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sat, 21 Feb 2026 12:23:20 +0000 Subject: [PATCH 5/8] fix(sim): increase MuJoCo video render resolution to 1280x720 --- dimos/simulation/mujoco/constants.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dimos/simulation/mujoco/constants.py b/dimos/simulation/mujoco/constants.py index 4e3501153..2d706387a 100644 --- a/dimos/simulation/mujoco/constants.py +++ b/dimos/simulation/mujoco/constants.py @@ -15,8 +15,8 @@ from pathlib import Path # Video/Camera constants -VIDEO_WIDTH = 320 -VIDEO_HEIGHT = 240 +VIDEO_WIDTH = 1280 +VIDEO_HEIGHT = 720 VIDEO_CAMERA_FOV = 45 # MuJoCo default FOV for head_camera (degrees) DEPTH_CAMERA_FOV = 160 From e89669e598f58127a4a8861600165a15ca503f85 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sat, 21 Feb 2026 12:29:18 +0000 Subject: [PATCH 6/8] fix(g1): revert MuJoCo resolution, use Go2 camera intrinsics for rerun MuJoCo framebuffer can't exceed 640 width without XML config. Reverted render resolution to 320x240. Use the same 1280x720 camera intrinsics as Go2 for rerun display scaling (matches Go2 sim behavior exactly). --- dimos/robot/unitree/g1/sim.py | 26 +++++++++++--------------- dimos/simulation/mujoco/constants.py | 4 ++-- 2 files changed, 13 insertions(+), 17 deletions(-) diff --git a/dimos/robot/unitree/g1/sim.py b/dimos/robot/unitree/g1/sim.py index ceb0493ba..6888ae74a 100644 --- a/dimos/robot/unitree/g1/sim.py +++ b/dimos/robot/unitree/g1/sim.py @@ -39,26 +39,22 @@ logger = setup_logger() -def _compute_sim_camera_info() -> CameraInfo: - """Compute camera intrinsics from MuJoCo sim camera parameters.""" - import math - - from dimos.simulation.mujoco.constants import VIDEO_CAMERA_FOV, VIDEO_HEIGHT, VIDEO_WIDTH - - fovy = math.radians(VIDEO_CAMERA_FOV) - f = VIDEO_HEIGHT / (2 * math.tan(fovy / 2)) - cx = VIDEO_WIDTH / 2.0 - cy = VIDEO_HEIGHT / 2.0 +def _camera_info_static() -> CameraInfo: + """Camera intrinsics for rerun visualization (matches Go2 convention).""" + fx, fy, cx, cy = (819.553492, 820.646595, 625.284099, 336.808987) + width, height = (1280, 720) return CameraInfo( frame_id="camera_optical", - height=VIDEO_HEIGHT, - width=VIDEO_WIDTH, + height=height, + width=width, distortion_model="plumb_bob", D=[0.0, 0.0, 0.0, 0.0, 0.0], - K=[f, 0.0, cx, 0.0, f, cy, 0.0, 0.0, 1.0], + K=[fx, 0.0, cx, 0.0, fy, cy, 0.0, 0.0, 1.0], R=[1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0], - P=[f, 0.0, cx, 0.0, 0.0, f, cy, 0.0, 0.0, 0.0, 1.0, 0.0], + P=[fx, 0.0, cx, 0.0, 0.0, fy, cy, 0.0, 0.0, 0.0, 1.0, 0.0], + binning_x=0, + binning_y=0, ) @@ -116,7 +112,7 @@ def stop(self) -> None: super().stop() def _publish_camera_info_loop(self) -> None: - info = _compute_sim_camera_info() + info = _camera_info_static() while not self._stop_event.is_set(): self.camera_info.publish(info) self._stop_event.wait(1.0) diff --git a/dimos/simulation/mujoco/constants.py b/dimos/simulation/mujoco/constants.py index 2d706387a..4e3501153 100644 --- a/dimos/simulation/mujoco/constants.py +++ b/dimos/simulation/mujoco/constants.py @@ -15,8 +15,8 @@ from pathlib import Path # Video/Camera constants -VIDEO_WIDTH = 1280 -VIDEO_HEIGHT = 720 +VIDEO_WIDTH = 320 +VIDEO_HEIGHT = 240 VIDEO_CAMERA_FOV = 45 # MuJoCo default FOV for head_camera (degrees) DEPTH_CAMERA_FOV = 160 From f21d37b20d31a7a5c1f54172bc194e812fb9a73f Mon Sep 17 00:00:00 2001 From: spomichter Date: Sat, 21 Feb 2026 12:36:04 +0000 Subject: [PATCH 7/8] fix(sim): increase MuJoCo render resolution to 1280x720 Set offscreen framebuffer size in model XML to support the higher resolution. Previously failed because MuJoCo's default offscreen buffer is only 640 wide. --- dimos/simulation/mujoco/constants.py | 4 ++-- dimos/simulation/mujoco/model.py | 9 +++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/dimos/simulation/mujoco/constants.py b/dimos/simulation/mujoco/constants.py index 4e3501153..2d706387a 100644 --- a/dimos/simulation/mujoco/constants.py +++ b/dimos/simulation/mujoco/constants.py @@ -15,8 +15,8 @@ from pathlib import Path # Video/Camera constants -VIDEO_WIDTH = 320 -VIDEO_HEIGHT = 240 +VIDEO_WIDTH = 1280 +VIDEO_HEIGHT = 720 VIDEO_CAMERA_FOV = 45 # MuJoCo default FOV for head_camera (degrees) DEPTH_CAMERA_FOV = 160 diff --git a/dimos/simulation/mujoco/model.py b/dimos/simulation/mujoco/model.py index bc309b730..1e2b3b9c5 100644 --- a/dimos/simulation/mujoco/model.py +++ b/dimos/simulation/mujoco/model.py @@ -113,6 +113,15 @@ def get_model_xml(robot: str, scene_xml: str) -> str: map_elem.set("znear", "0.01") map_elem.set("zfar", "10000") + # Set offscreen framebuffer size to support higher render resolutions + global_elem = visual.find("global") + if global_elem is None: + global_elem = ET.SubElement(visual, "global") + from dimos.simulation.mujoco.constants import VIDEO_HEIGHT, VIDEO_WIDTH + + global_elem.set("offwidth", str(VIDEO_WIDTH)) + global_elem.set("offheight", str(VIDEO_HEIGHT)) + _add_person_object(root) return ET.tostring(root, encoding="unicode") From 6233235332e432ea4eef235aab4375e14a4ba890 Mon Sep 17 00:00:00 2001 From: spomichter Date: Sat, 21 Feb 2026 12:42:25 +0000 Subject: [PATCH 8/8] Revert "fix(sim): increase MuJoCo render resolution to 1280x720" This reverts commit f21d37b20d31a7a5c1f54172bc194e812fb9a73f. --- dimos/simulation/mujoco/constants.py | 4 ++-- dimos/simulation/mujoco/model.py | 9 --------- 2 files changed, 2 insertions(+), 11 deletions(-) diff --git a/dimos/simulation/mujoco/constants.py b/dimos/simulation/mujoco/constants.py index 2d706387a..4e3501153 100644 --- a/dimos/simulation/mujoco/constants.py +++ b/dimos/simulation/mujoco/constants.py @@ -15,8 +15,8 @@ from pathlib import Path # Video/Camera constants -VIDEO_WIDTH = 1280 -VIDEO_HEIGHT = 720 +VIDEO_WIDTH = 320 +VIDEO_HEIGHT = 240 VIDEO_CAMERA_FOV = 45 # MuJoCo default FOV for head_camera (degrees) DEPTH_CAMERA_FOV = 160 diff --git a/dimos/simulation/mujoco/model.py b/dimos/simulation/mujoco/model.py index 1e2b3b9c5..bc309b730 100644 --- a/dimos/simulation/mujoco/model.py +++ b/dimos/simulation/mujoco/model.py @@ -113,15 +113,6 @@ def get_model_xml(robot: str, scene_xml: str) -> str: map_elem.set("znear", "0.01") map_elem.set("zfar", "10000") - # Set offscreen framebuffer size to support higher render resolutions - global_elem = visual.find("global") - if global_elem is None: - global_elem = ET.SubElement(visual, "global") - from dimos.simulation.mujoco.constants import VIDEO_HEIGHT, VIDEO_WIDTH - - global_elem.set("offwidth", str(VIDEO_WIDTH)) - global_elem.set("offheight", str(VIDEO_HEIGHT)) - _add_person_object(root) return ET.tostring(root, encoding="unicode")