Bugfix: Trajectory preview randomly times out in drake meshcat visualizer

[mustafab0]

Problem

preview_path() RPC randomly times out. Drake's Meshcat throws SystemExit (not Exception) when called from any thread other than its creator. RPC handlers run on a 50-worker thread pool, so many Meshcat call dies silently — no response sent, client sees timeout.

Issue: #

---

Solution

Route all Meshcat calls through a ThreadPoolExecutor(max_workers=1) that creates and owns the Meshcat instance. _on_viz_thread(fn, *args) submits work to this executor, with re-entrancy detection to avoid deadlocks when animate_path's nested calls are already on the viz thread.

Also reduced interpolation resolution from 0.02 to 0.1 for faster preview animations.

---

Breaking Changes

None

---

How to Test

1. Run a blueprint xarm-perception, coordinator-mock and manipulation_client.py
2. Open Meshcat in browser
3. Call plan_to_joints() then preview_path() via RPC
4. Yellow preview robot animates the path, live robot stays in place, no timeout

closes DIM-443

[greptile-apps]

Greptile Overview

Greptile Summary

This PR addresses intermittent preview_path() RPC timeouts by enforcing Drake Meshcat’s thread affinity: Meshcat is created on a dedicated single-worker and all Meshcat / MeshcatVisualizer calls are routed through _on_viz_thread(...), avoiding Drake’s SystemExit when invoked from non-creator threads.

In DrakeWorld, it also adds a separate “preview” (yellow/transparent) robot instance used for animation so the live robot state doesn’t move during previews. preview_path() now interpolates more coarsely (0.1 vs 0.02) to speed up animation.

Key integration point: ManipulationModule.preview_path() calls WorldMonitor.world.animate_path(), which now runs the full visualization publish loop on the Meshcat creator thread via the executor.

Confidence Score: 3/5

• This PR is close to mergeable but needs a Meshcat executor shutdown to avoid leaking threads in long-running/restarted processes.
• Main logic change (routing Meshcat calls to a dedicated creator thread) is coherent and consistently applied, but introducing a persistent ThreadPoolExecutor without a cleanup path is a concrete lifecycle bug that can accumulate threads or block clean shutdown when worlds/modules are recreated.
• dimos/manipulation/planning/world/drake_world.py

Important Files Changed

Filename	Overview
dimos/manipulation/manipulation_module.py	Reduces preview path interpolation resolution from 0.02 to 0.1 before calling world.animate_path(), speeding up visualization.
dimos/manipulation/planning/world/drake_world.py	Adds Meshcat thread-affinity routing via single-thread executor, introduces preview (ghost) robot instances, and updates obstacle/visualization calls to run on viz thread; must add executor shutdown to avoid thread leaks.

Sequence Diagram

sequenceDiagram
    participant Client as RPC Client
    participant RPC as ManipulationModule.preview_path()
    participant WM as WorldMonitor
    participant DW as DrakeWorld
    participant VEX as Viz ThreadPoolExecutor(1)
    participant Meshcat as Meshcat/MeshcatVisualizer

    Client->>RPC: preview_path(duration)
    RPC->>RPC: interpolate_path(resolution=0.1)
    RPC->>WM: world.animate_path(robot_id, path, duration)
    WM->>DW: animate_path(robot_id, path, duration)
    DW->>VEX: _on_viz_thread(_do_animate)
    VEX->>DW: _do_animate() (runs on creator thread)
    DW->>DW: show_preview(robot_id)
    DW->>VEX: _on_viz_thread(meshcat.SetProperty visible=true)
    loop each waypoint
        DW->>DW: _set_preview_positions(_plant_context,...)
        DW->>VEX: _on_viz_thread(meshcat_visualizer.ForcedPublish(viz_ctx))
        VEX->>Meshcat: ForcedPublish updates
    end
    DW->>DW: hide_preview(robot_id)
    DW->>VEX: _on_viz_thread(meshcat.SetProperty visible=false)
    DW->>VEX: _on_viz_thread(meshcat_visualizer.ForcedPublish(viz_ctx))
    VEX->>Meshcat: Final publish

Loading

[greptile-apps]

_{2 files reviewed, 1 comment}

_{Edit Code Review Agent Settings | Greptile}

[paul-nechifor]

Yes, looks good.

The only suggestion I'd have is to wrap this.

DrakeWorld doesn't care about the executor or the thread, it just needs to call meshcat, so it could be simpler from DrakeWorld's perspective if you'd have a SingleThreadedMeshcat (which contains all of this)

[mustafab0]

Wrapped the meshcat in a ThreadSafeMeshcat class.

Implementation is much more cleaner. Thanks !

[paul-nechifor]

Sleeping on the meshcat thread blocks other calls. You should sleep on the calling thread.

[mustafab0]

removed sleep from meshcat thread. sleep now lives in the calling thread

[paul-nechifor]

Why the resolution change?

[mustafab0]

The interpolation happens only for animation. As I was having too many timeouts, one theory was that the resolution was too fine. It doesn't seem to be the case. I can test to see if 0.2 is good enough.

But 0.1 seems to work great for preview, and less computational overhead.