How to find unique geometry across multiple scene_graph instances

Question

Hello, I was working on parallelizing inverse kinematics in Drake, and have encountered an issue: how do we refer to a geometry across parallel instances of SceneGraph? SceneGraph seems to keep a separate copy of GeometryIds for every different instance, and I wanted some advice on how to query for a specific geometry across all of these instances.

For example, when we run the following toy example that serially builds multiple scene graph instances and prints all associated GeometryIds,

import numpy as np

from pydrake.all import (
    DiagramBuilder, Parser,
    AddMultibodyPlantSceneGraph)

for _ in range(3):
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
    parser = Parser(plant=plant).AddModelsFromUrl(
        "package://drake/examples/multibody/cart_pole/cart_pole.sdf"
    )
    geometry_ids = scene_graph.model_inspector().GetAllGeometryIds()
    frame_ids = scene_graph.model_inspector().GetAllFrameIds()
    print(geometry_ids) # print all geometry ids 
    print(frame_ids) # print all frame ids
    body_idxs = []
    for frame_id in frame_ids:
        body_idxs.append(plant.GetBodyFromFrameId(frame_id).index())
    print(body_idxs) # print all body indices
    print('----')

the output is

[<GeometryId value=19>, <GeometryId value=23>, <GeometryId value=26>]
[<FrameId value=9>, <FrameId value=17>, <FrameId value=21>]
[BodyIndex(0), BodyIndex(1), BodyIndex(2)]
----
[<GeometryId value=44>, <GeometryId value=48>, <GeometryId value=51>]
[<FrameId value=9>, <FrameId value=42>, <FrameId value=46>]
[BodyIndex(0), BodyIndex(1), BodyIndex(2)]
----
[<GeometryId value=69>, <GeometryId value=73>, <GeometryId value=76>]
[<FrameId value=9>, <FrameId value=67>, <FrameId value=71>]
[BodyIndex(0), BodyIndex(1), BodyIndex(2)]
----

. Where it's possible to use BodyIndex to track a single body across different instances of plants, it seems I cannot do the same for GeometryIds. Is it possible for SceneGraph to tell me that 19, 44, and 69 are indeed the same geometry although they belong to different instances?

Things I've tried

The names associated with GeometryId is preserved across instances. For example, if I add the line

    for geometry_id in geometry_ids:
        print(scene_graph.model_inspector().GetName(geometry_id))

then they will consistently tell me that all three instances share the names

CartPole::cart_visual
CartPole::pole_point_mass
CartPole::pole_rod

However, I cannot seem to recover GeometryId back from these names without a FrameId, and the frame ids also differ across instances.

Another thought was relying on the ordering of the queries such as GetAllGeometryIds() to be consistent across different instances. Would this be true?

Solution

Your observation is correct, in a single Drake process, every newly instantiated geometry gets a globally unique geometry id. So, successively parsing the same model N times is going to produce a different set of ids in each SceneGraph instance. This is a property of the Identifier type.

Before I get into the details of what you'd have to do in this case to create the proper correlations, I want to dig into the question of, "Are you doing the right thing?"

Building N copies of the same diagram would largely be an anti-pattern for Drake. If the diagrams are the same but you're just doing parallel computations based on state, then the correct thing to do is to share the single diagram across threads, but to give each thread its own unique Context. (This would make your problem go away entirely.)

If you absolutely need unique copies of the same Diagram (although, I'd love to hear the reason for this), then cloning the Diagram (via copy.deepcopy() -- or System::Clone() for the C++ users out there) would be your friend. When cloned, SceneGraph copies the GeometryIds, so the clones would share ids.

Finally, if neither of the previous options work for you, you'll have to build your own map. The only ingredient your'e missing, as you pointed out, is getting the FrameId. The magical incantation for that is as follows:

some_body = plant.GetBodyByName("some_body_name")
geo_frame_id = plant.GetBodyFrameIdOrThrow(some_body.lndex())
geo_id = inspector.GetGeometryIdByName(geo_frame_id,
                                       Role.kIllustration, 
                                       "CartPole::cart_visual")

In other words, it all starts with the body index which will be shared across each of the plants (assuming they truly model the same system...which brings me back to the first question, maybe you just need multiple context?)