2

In calling ComputePointPairPenetration() from a QueryObject in Drake in Python in a Jupyter Notebook environment, ComputePointPairPenetration() reliably kills the kernel. I'm not sure what's causing it and couldn't figure out how to get any error message.

In case it's relevant I'm running pydrake locally on a Mac.

Here is relevant code:

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.00001)
file_name = FindResource("models/robot.urdf")
model = Parser(plant).AddModelFromFile(file_name)
file_name = FindResource("models/object.urdf")
object_model = Parser(plant).AddModelFromFile(file_name)
plant.Finalize()

diagram = builder.Build()

# Run simulation...

# Get geometry info from scene graph
context = scene_graph.AllocateContext()
q_obj = scene_graph.get_query_output_port().Eval(context)
q_obj.ComputePointPairPenetration()

Edit: @Sherm's comment fixed my problem :) Thank you so much!

For reference: 

diagram_context = diagram.CreateDefaultContext()
scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context) 
q_obj = scene_graph.get_query_output_port().Eval(scene_graph_context)
q_obj.ComputePointPairPenetration()
zliu
  • 143
  • 6
  • 2
    Multiple questions: 1.is "run simulation" required or does it crash without that? 2.If "run simulation" is required, can you include that code as well? 3. Can you elaborate on the geometries in `robot.urdf` and `object.urdf`? – Sean Curtis May 07 '20 at 22:16
  • 1
    Thank you for the question. Running simulation is not required for the crash. The above plus `diagram = builder.Build()` where `# Run simulation..` (as edited in the original post) is would reproduce the problem. For a minimum setting, removing the robot urdf only leaving the object urdf observes the same crash. The object urdf is a simple sphere that can move in the x and y directions through dummy prismatic joints. – zliu May 07 '20 at 22:28
  • 2
    It looks like you created a local Context for scene_graph. Instead I think you want the full diagram context so that the ports are connected up properly (e.g. scene_graph has an input port that receives poses from MultibodyPlant). So I think the above would work if you ask the Diagram to create a context, then ask for the SceneGraph subcontext for the calls you have above, rather than creating a standalone SceneGraph context. – Sherm May 07 '20 at 22:47
  • 2
    subcontext = scene_graph.GetMyContextFromRoot(diagram_context) lets you extract the fully-connected subcontext. – Sherm May 07 '20 at 22:52
  • 1
    @Sherm actually paid attention to the code. Yes, your problem is that of passing an unconnected context. He should type that up in an answer so that it's more clearly broadcast (hint, hint, hint). – Sean Curtis May 08 '20 at 17:09
  • 1
    Your wish is my command. Copied my comment to an answer, trolling for upvotes. – Sherm May 08 '20 at 21:10

2 Answers2

3

You created a local Context for scene_graph. Instead you want the full diagram context so that the ports are connected up properly (e.g. scene_graph has an input port that receives poses from MultibodyPlant). So the above should work if you ask the Diagram to create a Context, then ask for the SceneGraph subcontext for the calls you have above, rather than creating a standalone SceneGraph context.

This lets you extract the fully-connected subcontext:

scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context)
Eric Cousineau
  • 1,944
  • 14
  • 23
Sherm
  • 643
  • 4
  • 6
1

FTR Here's a similar formulation in a Drake Python unittest:

TestPlant.test_scene_graph_queries

Note that this takes an alternate route (using diagram.GetMutableSubsystemContext instead of scene_graph.GetMyContextFromroot), namely because it's doing scalar-type conversion as well.

If you're curious about scalar-type conversion (esp. if you're going to be doing optimization, e.g. needing AutoDiffXd), please see:

Additionally, here are examples of scalar-converting both MultibodyPlant and SceneGraph for testing InverseKinematics constraint classes:

Eric Cousineau
  • 1,944
  • 14
  • 23