Custom meshcat contact visualizer not drawing contact forces correctly

Question

I have a TrajectoryVisualizer object that includes a diagram and its context, used for visualizing trajectories and the contact forces along the trajectories.

class TrajectoryVisualizer:
    def __init__(self,):
        self.diagram = make_diagram_with_plant_and_scene_graph_and_visualizer_and_contact_visualizer() 
        self.meshcat # Meshcat object
        self.contact_vis  # ContactVisualizer object
        self.meshcat_vis  # MeshcatVisualizer object
        self.plant  # MultibodyPlant object

        self.context = self.diagram.CreateDefaultContext()
        self.context_meshcat = self.meshcat_vis.GetMyMutableContextFromRoot(
            self.context
        )
        self.context_contact_vis = (
            self.contact_vis.GetMyMutableContextFromRoot(self.context)
        )
        self.context_plant = self.plant.GetMyMutableContextFromRoot(self.context)

    def publish_trajectory(
        self,
        h: float,
        q_knots: np.ndarray,
        contact_results_list: List[ContactResults],
    ):
        """
        q_knots: (T + 1, n_q) array.
        contact_results_list: (T,) list.
        For 1 <= i <= T, contact_results_list[i - 1] is the contact forces at
         configuration q_knots[i].
        """
        assert len(q_knots) == len(contact_results_list) + 1


        self.meshcat_vis.DeleteRecording()
        self.meshcat_vis.StartRecording(False)

        for i, t in enumerate(np.arange(len(q_knots)) * h):
            self.context.SetTime(t)
            self.plant.SetPositions(self.context_plant)
            self.meshcat_vis.ForcedPublish(self.context_meshcat)

            if i > 0:
                # Contact forces
                self.contact_vis.GetInputPort("contact_results").FixValue(
                    self.context_contact_vis,
                    AbstractValue.Make(contact_results_list[i - 1]),
                )
                self.contact_vis.ForcedPublish(self.context_contact_vis)

        self.meshcat_vis.StopRecording()
        self.meshcat_vis.PublishRecording()

However, I realized that if I delete the contact forces, and try to draw the the same trajectory, the contact forces do not show up anymore:

vis = TrajectoryVisualizer()
vis.publish_trajectory(h, q_knots, contact_results_list)  # drawing for the first time, ok.
vis.meshcat.Delete("contact_forces")
vis.publish_trajectory(h, q_knots, contact_results_list)  # contact forces do not show up anymore.

I have two questions:

1. Is the failure due to caching? If so, how do I fix it?
2. A possible solution is to create a new context every time I call vis.publish_trajectory(...), but my impression has always been that Context can be expensive to construct, which is why it is constructed only once in the constructor of TrajectoryVisualizer . Is my understanding correct?

I believe creating a new context would work, but want to find out the most efficient way to visualize.

Answer 1

Instead of calling general function meshcat.Delete("contact_forces") to clear the contact visualization, call the purpose-built contact_visualizer.Delete() instead. The ContactVisualizer owns the entire /contact_forces/... path tree in Meshcat. You are not supposed to be manually screwing with that tree outside of its control.

Specifically -- for speed, the ContactVisualizer uses an internal cache of whether or not a contact pair / surface is being shown. If you manually change the Meshcat paths that are owned by the ContactVisualizer without telling it, the visualization will no longer work correctly.

---

As an alternative to contact_visualizer.Delete(), another way to reset would be to ensure that the delete_on_initialization_event option is enabled in the params (it is enabled by default), and then just re-initialize the self.context each time instead of calling Delete(). This would be more future-proof against any visualization systems that gain re-initialization logic down the road, and is the best way to ensure you're starting from a clean slate each time.