PyDrake ComputePointPairPenetration () убивает ядро

Question

При вызове ComputePointPairPenetration() из QueryObject в Drake в Python в среде Jupyter Notebook ComputePointPairPenetration() надежно уничтожает ядро. Я не уверен, что это за причина, и не мог понять, как получить сообщение об ошибке.

Если это актуально, я запускаю pydrake локально на Ma c.

Вот соответствующий код:

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 исправил мою проблему :) Большое вам спасибо!

Для справки:

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

Вы создали локальную Context для scene_graph. Вместо этого вам нужен полный контекст диаграммы, чтобы порты были подключены правильно (например, scene_graph имеет входной порт, который принимает позы от MultibodyPlant). Таким образом, приведенное выше должно работать, если вы попросите Diagram создать Context, а затем запросите подконтекст SceneGraph для вызовов, которые у вас есть выше, вместо создания автономного SceneGraph контекста.

Это позволяет вам извлечь полностью связанный подконтекст:

scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context)

Answer 2

FTR Вот аналогичная формулировка в тесте Drake Python unittest:

TestPlant.test_scene_graph_queries

Обратите внимание, что здесь используется альтернативный маршрут (вместо diagram.GetMutableSubsystemContext из scene_graph.GetMyContextFromroot), а именно потому, что он также выполняет преобразование скалярного типа.

Если вам интересно преобразование скалярного типа (особенно, если вы собираетесь проводить оптимизацию, например, требуется AutoDiffXd), см .:

• Drake C ++ API: Обзор системного скалярного преобразования
• Drake Python API: pydrake.systems.scalar_conversion

Кроме того, вот примеры скалярного преобразования как MultibodyPlant, так и SceneGraph для тестирования InverseKinematics классов ограничений:

• inverse_kinematics.py: TestConstraints