diff --git a/dimos/manipulation/manipulation_module.py b/dimos/manipulation/manipulation_module.py index c8f96e8862..f9c4c102d2 100644 --- a/dimos/manipulation/manipulation_module.py +++ b/dimos/manipulation/manipulation_module.py @@ -191,6 +191,7 @@ class ManipulationModule(Module): # message is a complete map, so it replaces the obstacle rather than adding. voxel_map: In[PointCloud2] objects: In[list[DetObject]] + ground_truth_poses: In[dict[str, PoseStamped]] tf: Out[TFMessage] def __init__(self, **kwargs: Any) -> None: @@ -1324,6 +1325,12 @@ async def handle_objects(self, objects: list[DetObject]) -> None: if self._world_monitor is not None: self._world_monitor.on_objects(objects) + async def handle_ground_truth_poses(self, poses: dict[str, PoseStamped]) -> None: + """Forward sim-only truth poses to the configured visualization.""" + if self._world_monitor is None or self._world_monitor.visualization is None: + return + self._world_monitor.visualization.set_ground_truth_poses(poses, self.get_obstacles()) + @rpc def refresh_obstacles(self, min_duration: float = 0.0) -> int: """Sync cached perception objects into the planning world.""" diff --git a/dimos/manipulation/planning/monitor/test_world_monitor.py b/dimos/manipulation/planning/monitor/test_world_monitor.py index b8f3700887..6d6c6d4bcb 100644 --- a/dimos/manipulation/planning/monitor/test_world_monitor.py +++ b/dimos/manipulation/planning/monitor/test_world_monitor.py @@ -205,6 +205,13 @@ def remove_vis_obstacle(self, obstacle_id: str) -> None: def clear_vis_obstacles(self) -> None: self.calls.append(("clear_vis_obstacles",)) + def set_ground_truth_poses( + self, + poses: dict[str, PoseStamped], + belief: dict[str, PoseStamped], + ) -> None: + self.calls.append(("set_ground_truth_poses", poses, belief)) + def _robot_config() -> RobotModelConfig: return RobotModelConfig( diff --git a/dimos/manipulation/planning/spec/protocols.py b/dimos/manipulation/planning/spec/protocols.py index 7ad35dd8f3..cf6e4148b8 100644 --- a/dimos/manipulation/planning/spec/protocols.py +++ b/dimos/manipulation/planning/spec/protocols.py @@ -215,6 +215,14 @@ def clear_vis_obstacles(self) -> None: """Clear obstacle representations from the visualization.""" ... + def set_ground_truth_poses( + self, + poses: dict[str, PoseStamped], + belief: dict[str, PoseStamped], + ) -> None: + """Replace the sim-only truth overlay and its planner-belief comparison.""" + ... + def get_visualization_url(self) -> str | None: """Get visualization URL if enabled.""" ... diff --git a/dimos/manipulation/planning/world/drake_world.py b/dimos/manipulation/planning/world/drake_world.py index ef9f5f8105..512a959611 100644 --- a/dimos/manipulation/planning/world/drake_world.py +++ b/dimos/manipulation/planning/world/drake_world.py @@ -1137,6 +1137,14 @@ def clear_vis_obstacles(self) -> None: """Embedded Meshcat observes native WorldSpec obstacle mutations.""" return None + def set_ground_truth_poses( + self, + poses: dict[str, PoseStamped], + belief: dict[str, PoseStamped], + ) -> None: + """Ignore sim-only overlays in the legacy Meshcat backend.""" + return None + def get_visualization_url(self) -> str | None: """Get visualization URL if enabled.""" if self._meshcat is not None: diff --git a/dimos/manipulation/visualization/test_factory.py b/dimos/manipulation/visualization/test_factory.py index 315ec22bbe..c6ba001a9f 100644 --- a/dimos/manipulation/visualization/test_factory.py +++ b/dimos/manipulation/visualization/test_factory.py @@ -83,6 +83,13 @@ def remove_vis_obstacle(self, obstacle_id: str) -> None: def clear_vis_obstacles(self) -> None: return None + def set_ground_truth_poses( + self, + poses: dict[str, PoseStamped], + belief: dict[str, PoseStamped], + ) -> None: + return None + class FakeWorld: def load_model(self, config: RobotModelConfig) -> None: @@ -231,6 +238,13 @@ def remove_vis_obstacle(self, obstacle_id: str) -> None: def clear_vis_obstacles(self) -> None: self.visualization_calls.append(("clear_vis_obstacles",)) + def set_ground_truth_poses( + self, + poses: dict[str, PoseStamped], + belief: dict[str, PoseStamped], + ) -> None: + self.visualization_calls.append(("set_ground_truth_poses", poses, belief)) + def test_config_defaults_to_no_visualization() -> None: config = ManipulationModuleConfig(model=FakeWorld().get_model_config()) diff --git a/dimos/manipulation/visualization/viser/config.py b/dimos/manipulation/visualization/viser/config.py index 36ce7d06ea..fa7eacdb48 100644 --- a/dimos/manipulation/visualization/viser/config.py +++ b/dimos/manipulation/visualization/viser/config.py @@ -35,6 +35,10 @@ class ViserVisualizationConfig(BaseModel): panel_enabled: bool = Field( default=True, validation_alias=AliasChoices("panel_enabled", "viser_panel_enabled") ) + ground_truth_overlay: bool = Field( + default=False, + validation_alias=AliasChoices("ground_truth_overlay", "viser_ground_truth_overlay"), + ) poll_hz: float = Field(default=5.0, validation_alias=AliasChoices("poll_hz", "viser_poll_hz")) preview_duration: float = Field( default=3.0, validation_alias=AliasChoices("preview_duration", "viser_preview_duration") diff --git a/dimos/manipulation/visualization/viser/scene.py b/dimos/manipulation/visualization/viser/scene.py index 2ca5f6f2d4..58d4f116a4 100644 --- a/dimos/manipulation/visualization/viser/scene.py +++ b/dimos/manipulation/visualization/viser/scene.py @@ -95,6 +95,10 @@ OBSTACLE_FALLBACK_COLOR = (55, 190, 210) OBSTACLE_FALLBACK_OPACITY = 0.55 OBSTACLE_PROXY_COLOR = (255, 45, 25) +GROUND_TRUTH_NAMESPACE = "/manipulation/ground_truth" +GROUND_TRUTH_COLOR = (55, 210, 235) +GROUND_TRUTH_OPACITY = 0.22 +GROUND_TRUTH_RADIUS = 0.035 class RobotDisplayMode(StrEnum): @@ -114,7 +118,11 @@ class ViserManipulationScene: """Viser scene graph helpers for current robot, ghost robot, and path rendering.""" def __init__( - self, server: ViserServer, viser_urdf: type[ViserUrdf], preview_fps: float | None = None + self, + server: ViserServer, + viser_urdf: type[ViserUrdf], + preview_fps: float | None = None, + ground_truth_overlay: bool = False, ) -> None: self.server = server self.viser_urdf = viser_urdf @@ -140,6 +148,10 @@ def __init__( self._obstacles_visible = True self._obstacle_gui_handles: list[object] = [] self._obstacle_warning_handle: Any | None = None + self._ground_truth_enabled = ground_truth_overlay + self._ground_truth_visible = True + self._ground_truth_handles: dict[str, list[Any]] = {} + self._ground_truth_report_handle: Any | None = None self._closed = False self._ensure_obstacle_control() self._ensure_reference_grid() @@ -281,6 +293,107 @@ def clear_vis_obstacles(self) -> None: for obstacle_id in list(self._obstacle_handles): self.remove_vis_obstacle(obstacle_id) + def set_ground_truth_poses( + self, + poses: dict[str, PoseStamped], + belief: dict[str, PoseStamped], + ) -> list[tuple[str, str | None, float | None]]: + """Render faint sim-truth ghosts and update the truth-vs-belief table.""" + with self._scene_lock: + if self._closed or not self._ground_truth_enabled: + return [] + stale = set(self._ground_truth_handles) - set(poses) + for name in stale: + for handle in self._ground_truth_handles.pop(name): + self._remove_scene_handle(handle) + for name, pose in poses.items(): + position, wxyz = self._pose_components(pose) + handles = self._ground_truth_handles.get(name) + if handles is None: + path = f"{GROUND_TRUTH_NAMESPACE}/{name}" + handles = [ + self.server.scene.add_icosphere( + path, + radius=GROUND_TRUTH_RADIUS, + color=GROUND_TRUTH_COLOR, + opacity=GROUND_TRUTH_OPACITY, + wireframe=True, + position=position, + wxyz=wxyz, + visible=self._ground_truth_visible, + ), + self.server.scene.add_frame( + f"{path}/pose", + axes_length=0.06, + axes_radius=0.002, + position=position, + wxyz=wxyz, + visible=self._ground_truth_visible, + ), + self.server.scene.add_label( + f"{path}/label", + f"{name} (truth)", + position=(position[0], position[1], position[2] + 0.06), + visible=self._ground_truth_visible, + ), + ] + self._ground_truth_handles[name] = handles + else: + for handle in handles: + handle.position = position + handle.wxyz = wxyz + handles[-1].position = (position[0], position[1], position[2] + 0.06) + rows = self._truth_belief_rows(poses, belief) + if self._ground_truth_report_handle is not None: + self._ground_truth_report_handle.content = self._format_truth_report(rows) + return rows + + def set_ground_truth_visible(self, visible: bool) -> None: + with self._scene_lock: + self._ground_truth_visible = bool(visible) + for handles in self._ground_truth_handles.values(): + for handle in handles: + self._set_handle_visibility(handle, self._ground_truth_visible) + + @staticmethod + def _truth_belief_rows( + truth: dict[str, PoseStamped], belief: dict[str, PoseStamped] + ) -> list[tuple[str, str | None, float | None]]: + available = dict(belief) + rows: list[tuple[str, str | None, float | None]] = [] + for truth_name, truth_pose in sorted(truth.items()): + if not available: + rows.append((truth_name, None, None)) + continue + belief_name, belief_pose = min( + available.items(), + key=lambda item: np.linalg.norm( + item[1].position.to_numpy() - truth_pose.position.to_numpy() + ), + ) + delta = float( + np.linalg.norm(belief_pose.position.to_numpy() - truth_pose.position.to_numpy()) + ) + rows.append((truth_name, belief_name, delta)) + available.pop(belief_name) + return rows + + @staticmethod + def _format_truth_report(rows: Sequence[tuple[str, str | None, float | None]]) -> str: + lines = [ + "Ghost cyan = simulator truth; solid markers = planner belief.", + "", + "| Truth | Belief | Δ (m) |", + "|---|---|---:|", + ] + lines.extend( + f"| {truth} | {belief or '—'} | {delta:.4f} |" + if delta is not None + else f"| {truth} | — | — |" + for truth, belief, delta in rows + ) + return "\n".join(lines) + def show_obstacle_warning(self, message: str) -> None: """Expose a persistent renderer warning in the frontend when available.""" with self._scene_lock: @@ -299,8 +412,20 @@ def _ensure_obstacle_control(self) -> None: self._obstacle_gui_handles.append(folder) with folder: handle = self.server.gui.add_checkbox("manipulation.obstacles", initial_value=True) + if self._ground_truth_enabled: + truth_handle = self.server.gui.add_checkbox( + "sim ground truth", initial_value=True + ) + self._ground_truth_report_handle = self.server.gui.add_markdown( + "Waiting for simulator truth." + ) handle.on_update(lambda event: self.set_obstacles_visible(event.target.value)) self._obstacle_gui_handles.append(handle) + if self._ground_truth_enabled: + truth_handle.on_update( + lambda event: self.set_ground_truth_visible(event.target.value) + ) + self._obstacle_gui_handles.extend([truth_handle, self._ground_truth_report_handle]) except (AttributeError, TypeError): self._obstacle_gui_handles.clear() @@ -319,6 +444,20 @@ def _obstacle_pose( ), ) + @staticmethod + def _pose_components( + pose: PoseStamped, + ) -> tuple[tuple[float, float, float], tuple[float, float, float, float]]: + return ( + (float(pose.position.x), float(pose.position.y), float(pose.position.z)), + ( + float(pose.orientation.w), + float(pose.orientation.x), + float(pose.orientation.y), + float(pose.orientation.z), + ), + ) + @staticmethod def _obstacle_appearance(obstacle: Obstacle) -> tuple[tuple[int, int, int], float]: color = obstacle.color @@ -628,6 +767,10 @@ def close(self) -> None: self._obstacle_handles.clear() self._obstacles.clear() self._obstacle_render_failures.clear() + for handles in self._ground_truth_handles.values(): + for handle in handles: + self._remove_scene_handle(handle) + self._ground_truth_handles.clear() for key in list(self._handles): self._remove_handle(key) if self._grid_handle is not None: diff --git a/dimos/manipulation/visualization/viser/visualizer.py b/dimos/manipulation/visualization/viser/visualizer.py index e4b099510c..cc8cb8f199 100644 --- a/dimos/manipulation/visualization/viser/visualizer.py +++ b/dimos/manipulation/visualization/viser/visualizer.py @@ -15,6 +15,7 @@ from __future__ import annotations from contextlib import suppress +from threading import Lock from typing import TYPE_CHECKING from dimos.manipulation.visualization.viser.animation import ( @@ -75,18 +76,34 @@ def __init__( self._operator: object | None = None self._current_state: JointState | None = None self._model_config: RobotModelConfig | None = None + self._last_ground_truth_report: tuple[tuple[str, str | None, float | None], ...] = () + self._start_lock = Lock() self._closed = False def _ensure_started(self) -> None: if self._closed or self._runtime is not None: return + with self._start_lock: + if self._closed or self._runtime is not None: + return + self._start_runtime() + + def _start_runtime(self) -> None: + """Start Viser after the caller has serialized lazy initialization.""" runtime = ViserRuntime(self.config) scene: ViserManipulationScene | None = None gui: ViserPanelGui | None = None try: server = runtime.start() apply_dimos_theme(server) - scene = ViserManipulationScene(server, ViserUrdf) + if self.config.ground_truth_overlay: + scene = ViserManipulationScene( + server, + ViserUrdf, + ground_truth_overlay=True, + ) + else: + scene = ViserManipulationScene(server, ViserUrdf) gui = ( ViserPanelGui( server, @@ -216,6 +233,36 @@ def clear_vis_obstacles(self) -> None: if self._scene is not None: self._scene.clear_vis_obstacles() + def set_ground_truth_poses( + self, + poses: dict[str, PoseStamped], + belief: dict[str, PoseStamped], + ) -> None: + """Replace the live sim-truth ghosts and compare them with planner belief.""" + if self._closed or not self.config.ground_truth_overlay: + return + self._ensure_started() + if self._scene is None: + return + rows = tuple(self._scene.set_ground_truth_poses(poses, belief)) + rounded = tuple( + (truth, matched, None if delta is None else round(delta, 4)) + for truth, matched, delta in rows + ) + if rounded != self._last_ground_truth_report: + self._last_ground_truth_report = rounded + logger.info( + "Truth-vs-belief deltas", + deltas=[ + { + "truth": truth, + "belief": matched, + "delta_m": delta, + } + for truth, matched, delta in rounded + ], + ) + def update_state(self, frame: VisualizationStateFrame) -> None: """Update current robot render state from a pushed state frame.""" if self._closed: diff --git a/dimos/robot/manipulators/xarm/blueprints/simulation.py b/dimos/robot/manipulators/xarm/blueprints/simulation.py index 1d576733ee..896f6b0125 100644 --- a/dimos/robot/manipulators/xarm/blueprints/simulation.py +++ b/dimos/robot/manipulators/xarm/blueprints/simulation.py @@ -54,6 +54,7 @@ # as a ring instead of a roll. Keep a shape-word fallback for that view. "green ring", ] +XARM_ROOM_OBJECTS = ["bottle", "can", "cup", "tape", "marker", "box"] xarm_perception_sim = autoconnect( ManipulationModule.blueprint( @@ -90,7 +91,7 @@ ManipulationModule.blueprint( model=_xarm7_sim_model, planning_timeout=10.0, - visualization={"backend": "none"}, + visualization={"backend": "viser", "ground_truth_overlay": True}, ), ManipulationSkills.blueprint(), PickAndPlaceModule.blueprint(planning_frame="world"), @@ -104,6 +105,7 @@ # world->link7 edge and can make an otherwise valid scan unregistrable. "base_frame_id": "world", "reset_joint_positions": XARM_ROOM_SCAN_JOINTS, + "ground_truth_body_names": XARM_ROOM_OBJECTS, } ), ObjectSceneRegistrationModule.blueprint( diff --git a/dimos/simulation/engines/mujoco_sim_module.py b/dimos/simulation/engines/mujoco_sim_module.py index 87ff5a0a21..7d72f36e19 100644 --- a/dimos/simulation/engines/mujoco_sim_module.py +++ b/dimos/simulation/engines/mujoco_sim_module.py @@ -253,6 +253,8 @@ class MujocoSimModuleConfig(ModuleConfig, DepthCameraConfig): reset_joint_positions: list[float] | None = None headless: bool = False dof: int = 7 + ground_truth_body_names: list[str] = Field(default_factory=list) + ground_truth_fps: float = Field(default=2.0, gt=0.0) # Camera config (matches former MujocoCameraConfig). camera_name: str = "wrist_camera" @@ -332,6 +334,7 @@ class MujocoSimModule( # root. Published every step; consumers like the viser viewer use # this to translate the robot in world space. odom: Out[PoseStamped] + ground_truth_poses: Out[dict[str, PoseStamped]] tf: Out[TFMessage] def __init__(self, **kwargs: Any) -> None: @@ -348,6 +351,7 @@ def __init__(self, **kwargs: Any) -> None: self._camera_info_base: CameraInfo | None = None self._shm_ready_signaled = False self._latest_frame_ts: float | None = None + self._last_ground_truth_publish = 0.0 # IMU sensor slices into MjData.sensordata, resolved once at start. # None if the MJCF has no recognized IMU sensors (e.g. arm-only sims). @@ -778,6 +782,7 @@ def _publish_shm_and_lcm(self, engine: MujocoEngine) -> None: """ if self._sim_hooks is not None: self._sim_hooks.post_step(engine) + self._publish_ground_truth_poses(engine) shm = self._shm if shm is None: return @@ -847,6 +852,33 @@ def _publish_shm_and_lcm(self, engine: MujocoEngine) -> None: shm.signal_ready(num_joints=len(engine.joint_names), arm_joints=self.config.dof) self._shm_ready_signaled = True + def _publish_ground_truth_poses(self, engine: MujocoEngine) -> None: + names = self.config.ground_truth_body_names + if not names: + return + now = time.monotonic() + if now - self._last_ground_truth_publish < 1.0 / self.config.ground_truth_fps: + return + poses: dict[str, PoseStamped] = {} + for name in names: + body_id = mujoco.mj_name2id(engine.model, mujoco.mjtObj.mjOBJ_BODY, name) + if body_id < 0: + logger.warning("Ground-truth body not found", body_name=name) + continue + position = engine.data.xpos[body_id] + wxyz = engine.data.xquat[body_id] + poses[name] = PoseStamped( + ts=time.time(), + frame_id="world", + position=Vector3(float(position[0]), float(position[1]), float(position[2])), + orientation=Quaternion( + float(wxyz[1]), float(wxyz[2]), float(wxyz[3]), float(wxyz[0]) + ), + ) + self._last_ground_truth_publish = now + if poses: + self.ground_truth_poses.publish(poses) + def _build_camera_info(self) -> None: if self._engine is None: return