MeshcatVisualizer: How to visualize an optimizer's intermediate solutions in Meshcat

Question

Drake version: Dockerhub (1.20.0) Python 3.10.12, OS: Ubuntu 20.04

We would like to visualize a Kinematic Trajectory Optimizer's intermediate solutions while it is solving. Using: AddVisualizationCallback(visualization_callback, opt.prog().decision_variables()) works and passes the control points to the visualization_callback function and we can index in to get each control point's values (7 joint position states for the iiwa robot).

The issue we are having is, how do we visualize these values in Meshcat? It seems like MeshcatVisualizer is the right class to do so, but the documentation does not really show a straight forward way of visualizing the control point values we have or (preferably) adding a replica robot in meshcat to visualize the intermediate solutions.

Currently my visualization_callback function looks like this:

def visualization_callback(joint_positions):
    # Update the state of the Meshcat visualizer to match the optimizer's current configuration
    context = robot.CreateDefaultContext()

    # Append zeros for velocity
    joint_states = np.concatenate((joint_positions, np.zeros(len(joint_positions))))

    context.get_mutable_continuous_state().get_mutable_vector().SetFromVector(joint_states)
    meshcat_vis.Publish(context)

# In main function
meshcat = StartMeshcat()
global meshcat_vis
meshcat_vis = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)

Answer 1

Here is a code snippet (also posted in Drake#9728) that can be used to visualize a handful of robot poses directly. It can certainly be used in a visualization callback, though you might want to avoid calling SetObject and publishing the same geometry to the same meshcat path each time the visualization is called -- that could be slow.

def DrawRobot(query_object: QueryObject, meshcat_prefix: str, draw_world: bool = True):
    rgba = Rgba(0.7, 0.7, 0.7, 0.3)
    role = Role.kProximity
    # This is a minimal replication of the work done in MeshcatVisualizer.
    inspector = query_object.inspector()
    for frame_id in inspector.GetAllFrameIds():
        if frame_id == inspector.world_frame_id():
            if not draw_world:
                continue
            frame_path = meshcat_prefix
        else:
            frame_path = f"{meshcat_prefix}/{inspector.GetName(frame_id)}"
        frame_path.replace("::", "/")
        frame_has_any_geometry = False
        for geom_id in inspector.GetGeometries(frame_id, role):
            path = f"{frame_path}/{geom_id.get_value()}"
            path.replace("::", "/")
            meshcat.SetObject(path, inspector.GetShape(geom_id), rgba)
            meshcat.SetTransform(path, inspector.GetPoseInFrame(geom_id))
            frame_has_any_geometry = True

        if frame_has_any_geometry:
            X_WF = query_object.GetPoseInWorld(frame_id)
            meshcat.SetTransform(frame_path, X_WF)

...

  for i in range(1, num_to_draw):
      plant.SetPositions(plant_context, q[i])\n",
      query = scene_graph.get_query_output_port().Eval(scene_graph_context)\n",
      DrawRobot(query, f"{i}", False)