PyDrake ComputePointPairPenetration() 杀死内核答案

【问题标题】：PyDrake ComputePointPairPenetration() kills kernelPyDrake ComputePointPairPenetration() 杀死内核
【发布时间】：2020-05-07 22:04:05
【问题描述】：

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

【问题讨论】：

多个问题：1.是否需要“运行模拟”或者没有它会崩溃？ 2.如果需要“运行模拟”，你能把那个代码也包括进去吗？ 3.你能详细说明robot.urdf和object.urdf中的几何形状吗？
感谢您的提问。崩溃不需要运行模拟。上面加上diagram = builder.Build()，其中# Run simulation..（在原始帖子中编辑）会重现问题。对于最小设置，移除机器人 urdf 仅留下对象 urdf 会观察到相同的崩溃。对象 urdf 是一个简单的球体，可以通过虚拟棱柱关节在 x 和 y 方向上移动。
看起来你为scene_graph创建了一个本地上下文。相反，我认为您需要完整的图表上下文，以便端口正确连接（例如，scene_graph 具有从 MultibodyPlant 接收姿势的输入端口）。因此，我认为如果您要求 Diagram 创建一个上下文，然后为上面的调用请求 SceneGraph 子上下文，而不是创建一个独立的 SceneGraph 上下文，则上述方法会起作用。
subcontext = scene_graph.GetMyContextFromRoot(diagram_context) 让您提取完全连接的子上下文。
@Sherm 实际上关注了代码。是的，您的问题是传递未连接的上下文。他应该在答案中输入它，以便更清楚地传播（提示、提示、提示）。

标签： drake

【解决方案1】：

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

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

scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context)

【讨论】：

【解决方案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

【讨论】：