feat(viser): overlay sim ground truth

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."""

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(

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."""
         ...

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:

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())

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")

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:

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: