Add meshcat paths for bodies without geometry and frames in the MeshcatVisualizer

[marip8]

Is your feature request related to a problem? Please describe.

Currently the MeshcatVisualizer system does not publish the transform of meshcat paths for bodies that do not contain geometry. This is problematic for plants spawned by URDF models where links are often created without geometry to represent important frames (e.g., camera frames, tool flange frames, etc.). I would like to attach various meshcat visualizations to these types of frames (e.g., coordinate triads, meshes, etc.), but the MeshcatVisualizer does not add them or update their positions using the query object.

Similarly, the MeshcatVisualizer does not add meshcat paths for Frame objects. As a result, users manually have to add meshcat paths for these Frames by getting the meshcat path to their associated bodies (assuming that body has geometry and was added by the MeshcatVisualizer, which is not always the case) and setting the transform via Frame::GetFixedPoseInBodyFrame.

Describe the solution you'd like

It would be very beneficial if the MeshcatVisualizer would:

1. Add an empty meshcat path for bodies without geometry and update the transform of that path using the query object as it does for bodies with geometry
2. Add an empty meshcat path for all Frames that are not body frames and set the transform relative their body using Frame::GetFixedPoseInBodyFrame

Describe alternatives you've considered

I've already written a Python function that adds all non-body frames to Meshcat. It works fine, but it's inconvenient that the MeshcatVisualizer doesn't take care of this by default.

def add_frames(plant: MultibodyPlant,
               meshcat: Meshcat,
               meshcat_ns: str):
    for i in range(plant.num_frames()):
        frame = plant.get_frame(FrameIndex(i))
        meshcat_path = f'{meshcat_ns}/{frame.body().body_frame().scoped_name().get_full().replace("::", "/")}'
        if not frame.is_body_frame():
            meshcat_path += f'/{frame.name()}'
            meshcat.SetTransform(meshcat_path, frame.GetFixedPoseInBodyFrame())

I am also about to add a custom LeafSystem very similar to MeshcatVisualizer that adds meshcat paths for bodies without geometry uses the query object to set their transforms. I think this will also work fine, but it's inconvenient that this isn't set by default

Additional context

N/A

[marip8]

For reference, here is a Python class that implements the requested behavior wrt. bodies without geometry:

@TemplateSystem.define("MeshcatVisualizerExtension")
def MeshcatVisualizerExtension_(T):
    class Impl(LeafSystem_[T]):
        def _construct(self,
                       meshcat: Meshcat,
                       params: MeshcatVisualizerParams,
                       converter=None):
            LeafSystem_[T].__init__(self, converter=converter)

            # Declare ports
            self.query_object_input_port = self.DeclareAbstractInputPort('query_object', Value[QueryObject]())

            self.DeclarePerStepPublishEvent(self.on_step)
            self.DeclareForcedPublishEvent(self.on_step)
            self.DeclareInitializationPublishEvent(self.on_init)

            self.meshcat = meshcat
            self.params = params

        def _construct_copy(self, other, converter=None):
            Impl._construct(self,
                            meshcat=other.meshcat,
                            params=other.params,
                            converter=converter)

        @staticmethod
        def AddToBuilder(builder: DiagramBuilder,
                         scene_graph: SceneGraph,
                         meshcat: Meshcat,
                         params: MeshcatVisualizerParams) -> LeafSystem:
            aspirational_name = f'meshcat_visualizer_extension({params.prefix})'
            visualizer = builder.AddSystem(MeshcatVisualizerExtension_[T](meshcat, params))
            if not builder.HasSubsystemNamed(aspirational_name):
                visualizer.set_name(aspirational_name)
            builder.Connect(scene_graph.get_query_output_port(), visualizer.query_object_input_port)
            return visualizer

        def on_init(self, _context: Context) -> EventStatus:
            if self.params.delete_on_initialization_event:
                self.meshcat.Delete()
            return EventStatus.Succeeded()

        def on_step(self, context: Context) -> EventStatus:
            query_object: QueryObject = self.query_object_input_port.Eval(context)
            inspector = query_object.inspector()
            for frame_id in inspector.GetAllFrameIds():
                if frame_id == inspector.world_frame_id():
                    continue

                geos = inspector.GetGeometries(frame_id, self.params.role)
                if len(geos) == 0:
                    name = inspector.GetName(frame_id).replace('::', '/')
                    self.meshcat.SetTransform(f'{self.params.prefix}/{name}', query_object.GetPoseInParent(frame_id), context.get_time())

            return EventStatus.Succeeded()

    return Impl

# Default instantiation
MeshcatVisualizerExtension = MeshcatVisualizerExtension_[float]

[RussTedrake]

+@joemasterjohn as component owner.