PyDrake ComputePointPairPenetration() kills kernel

问题

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()

回答1:

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)

回答2:

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:

• Drake C++ API: System Scalar Conversion Overview
• Drake Python API: pydrake.systems.scalar_conversion

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

• inverse_kinematics.py: TestConstraints

来源：https://stackoverflow.com/questions/61668510/pydrake-computepointpairpenetration-kills-kernel