PyDrake ComputePointPairPenetration() 杀死内核

Question

在 Jupyter Notebook 环境中 Python 从 Drake 中的 QueryObject 调用 ComputePointPairPenetration() 时，ComputePointPairPenetration() 可靠地终止了内核。我不确定是什么原因造成的，也不知道如何获取任何错误消息。

如果相关，我在 Mac 上本地运行 pydrake。

相关代码如下：

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

编辑： @Sherm 的评论解决了我的问题:) 非常感谢！

供参考：

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

Answer 1

您为 scene_graph 创建了本地 Context。相反，您需要完整的图表上下文，以便正确连接端口（例如 scene_graph 有一个输入端口，可以从 MultibodyPlant 接收姿势）。因此，如果您要求 Diagram 创建一个 Context，然后为上面的调用请求 SceneGraph 子上下文，而不是创建一个独立的 SceneGraph上下文。

这让您可以提取完全连接的子上下文：

scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context)

Answer 2

FTR 这是 Drake Python 单元测试中的类似公式：

TestPlant.test_scene_graph_queries

请注意，这采用了替代路线（使用 diagram.GetMutableSubsystemContext 而不是 scene_graph.GetMyContextFromroot），即因为它也在进行标量类型转换。

如果您对标量类型转换感到好奇（尤其是如果您要进行优化，例如需要 AutoDiffXd），请参阅：

德雷克 C++ API：System Scalar Conversion Overview
德雷克 Python API: pydrake.systems.scalar_conversion

此外，这里是标量转换 MultibodyPlant 和 SceneGraph 以测试 InverseKinematics 约束的示例类:

inverse_kinematics.py: TestConstraints

PyDrake ComputePointPairPenetration() 杀死内核

PyDrake ComputePointPairPenetration() kills kernel

drake