CODEWITHSUNDEEP
drake
zliu
zliu

Reputation: 153

Drake: Why are there multiple geometryId for the same link in a MultibodyPlant?

I have 6 real links (named left distal, left proximal, right distal, right proximal, palm, and ball) + 2 dummy links (so the ball can move freely in x-y plane) in my URDFs. I want to know which geometryId corresponds to which link so I can check for contact. However, there are far more geometryId registered than links in the system. So I try to print out the body names through the geometryIds and found multiple geometryIds with the same body name. Then from Drake's render_multibody_plant.ipynb tutorial I saw this line geometry_label = inspector.GetPerceptionProperties(geometry_id).GetProperty("label", "id") so I printed the same for my geometryIds. However, some of them returns None. When I print out int(geometry_label) for those that are not None, I get exactly 6 numbers (but multiple of the same number for some of the links!). I don't understand what those extra geometry_ids are for and how to find the geometryIds that I actually care for.

Here is relevant code: 

builder = DiagramBuilder()
# plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.00001)
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.)
file_name = FindResource("models/myhand.urdf")
gripper_model = Parser(plant).AddModelFromFile(file_name)
file_name = FindResource("models/sphere.urdf")
object_model = Parser(plant).AddModelFromFile(file_name)
scene_graph_context = scene_graph.AllocateContext()
plant.Finalize()
plant.set_name('hand')


controller = ConstantVectorSource([-7, 7])
torque_system = builder.AddSystem(controller)
builder.Connect(torque_system.get_output_port(0), plant.get_actuation_input_port())

# Setup visualization
T_xy = np.array([[ 1., 0., 0., 0.], [ 0., 1., 0., 0.], [ 0., 0., 0., 1.]])
T_xz = np.array([[ 1., 0., 0., 0.], [ 0., 0., 1., 0.], [ 0., 0., 0., 1.]])
visualizer = builder.AddSystem(
    PlanarSceneGraphVisualizer(scene_graph, T_VW=T_xy, xlim=[-0.3, 0.3], ylim=[-0.3, 0.3], show=plt_is_interactive))

builder.Connect(scene_graph.get_pose_bundle_output_port(),
                visualizer.get_input_port(0))


diagram = builder.Build()

diagram_context = diagram.CreateDefaultContext()
scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context) 
plant_context = plant.GetMyContextFromRoot(diagram_context) 

# Set up a simulator to run this diagram
simulator = Simulator(diagram)
context = simulator.get_mutable_context()

# simulate from zero to duration
simulator.Initialize()


# Simulate
duration = 0.5 if get_ipython() else 0.1 # sets a shorter duration during testing
context.SetTime(0.0)

AdvanceToAndVisualize(simulator, visualizer, duration)

query_object = scene_graph.get_query_output_port().Eval(scene_graph_context)
inspector = query_object.inspector()


for geometry_id in inspector.GetAllGeometryIds():
    body = plant.GetBodyFromFrameId(inspector.GetFrameId(geometry_id))
    print(body.name())

    geometry_label = inspector.GetPerceptionProperties(
        geometry_id)
    if geometry_label != None:
        print(int(geometry_label.GetProperty("label", "id")))

Here's output to the print statement: 

ball
ball
6
right_distal
right_distal
5
right_distal
5
left_distal
left_distal
4
left_distal
4
right_proximal
right_proximal
3
right_proximal
3
left_proximal
left_proximal
2
left_proximal
2
palm
palm
1

Note: running PyDrake on a Jupyter Notebook

Upvotes: 2

Views: 157

Answers (2)

Sean Curtis
Sean Curtis

Reputation: 1923

Specifically, each link can register collision and visual geometry. Even if the two geometries specify shapes with identical parameters, those shapes are not consolidated to a single geometry. So, you'll get one GeometryId for each collision and visual geometry associated with a link.

Those declared as visual in your URDF will have non-None PerceptionProperties (and IllutrationProperties). But their ProximityProperties will be None. Conversely, those declared as collision geometries will have non-None ProximityProperties but None for the other two types.

Upvotes: 3

Russ Tedrake
Russ Tedrake

Reputation: 5533

I would expect to have a separate geometry ID for each visual element in your body (not one per body). Does your URDF have multiple visual elements in each body?

The "Body frames and SceneGraph frames" documentation for MultibodyPlant appears to have the answer you're looking for:

Given a GeometryId, SceneGraph cannot report what body it is affixed to. It can only report the SceneGraph alias frame F. But the following idiom can report the body:

const MultibodyPlant<T>& plant = ...;
const SceneGraphInspector<T>& inspector =  ...;
const GeometryId g_id = id_from_some_query;
const FrameId f_id = inspector.GetFrameId(g_id);
const Body<T>* body = plant.GetBodyFromFrameId(f_id);

See documentation of geometry::SceneGraphInspector on where to get an inspector.

Upvotes: 2

Related Questions