drakemeshcat

How do I visualize coordinate frames in Drake using Meshcat?


How can I view coordinate frames from Drake in Meschcat? I only found this related question about Drake Visualizer but had trouble finding the equivalent code for Meshcat.

I have made some progress and got a coordinate frame at the origin using

    AddFrameTriadIllustration(scene_graph=scene_graph, plant=plant, frame_id=plant.world_frame_id())

However, I want a coordinate frame displayed at my robot's end effector. I'm having trouble determining how to obtain a frame id that's not the world_frame_id.


Solution

  • To visualize a frame that's offset from a SceneGraph frame, call pydrake.visualization.AddFrameTriadIllustration. If the base frame is the world frame, then the X_FT you provide would be the world pose of what you're trying to visualize.

    Here's a link to a sample usage to visualize all of the existing frames.

    In case the link disappears, the snippet is reproduced below.

    # https://github.com/RobotLocomotion/drake/blob/v1.19.0/bindings/pydrake/visualization/_model_visualizer.py#L257-L274
    if self._visualize_frames:
        # Find all the frames and draw them.
        # The frames are drawn using the parsed length.
        # The world frame is drawn thicker than the rest.
        inspector = self._builder.scene_graph().model_inspector()
        for frame_id in inspector.GetAllFrameIds():
            world_id = self._builder.scene_graph().world_frame_id()
            radius = self._triad_radius * (
                3 if frame_id == world_id else 1
                )
            AddFrameTriadIllustration(
                plant=self._builder.plant(),
                scene_graph=self._builder.scene_graph(),
                frame_id=frame_id,
                length=self._triad_length,
                radius=radius,
                opacity=self._triad_opacity,
            )