Python bindings error when adding two plants to a scene graph in pyDrake

Question

I'd like to have two plants in the same builder and scene graph. (I don't want them on the same plant because I'd like to separate their dynamics, but I'd like them to affect each other, hence keeping them on the same builder and scene graph.)

My implementation is the following:

from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
from pydrake.systems.framework import DiagramBuilder

builder = DiagramBuilder()
plant1, scene_graph  = AddMultibodyPlantSceneGraph(builder, 0.0)
plant2  = AddMultibodyPlantSceneGraph(builder, 0.0, scene_graph)

When I run this, I get the error:

Traceback (most recent call last):
  File "/filepath/2plants1scene.py", line 6, in <module>
    plant2  = AddMultibodyPlantSceneGraph(builder, 0.0, scene_graph)
RuntimeError: C++ object must be owned by pybind11 when attempting to release to C++

Is this a bindings issue? The documentation for AddMultibodyPlantSceneGraph makes it seem as though it can add plants to already existing scenes.

The error messages look similar to this issue from 2018: https://github.com/RobotLocomotion/drake/issues/8160

Thanks in advance for any ideas.

Answer 1

Is this a bindings issue?

Regarding your specific error message, you are trying to take one object (whose ownership is governed by unique_ptr<>) and trying to pass it off its owned data twice (or more).

From C++ API:
https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html#aac66563a5f3eb9e2041bd4fa8d438827 Note that the scene_graph argument is unique_ptr<>.

So, it's a bindings error insofar as the error message is kinda bad; however, it's more of a semantics issue w/ C++ API.

The documentation for AddMultibodyPlantSceneGraph makes it seem as though it can add plants to already existing scenes.

For reference, here is the core implementation of that method:
https://github.com/RobotLocomotion/drake/blob/v0.32.0/multibody/plant/multibody_plant.cc#L3346-L3370

For your use case, you should only add the SceneGraph to the DiagramBuilder once. Since you want to connect one SceneGraph to multiple MultibodyPlant instances, I would advise that you do not use AddMultibodyPlantSceneGraph, as that is sugar for a 1:1 pairing.

Instead, you should register and connect the SceneGraph manually; I think it would look something like this:

def register_plant_with_scene_graph(scene_graph, plant):
    plant.RegsterAsSourceForSceneGraph(scene_graph)
    builder.Connect(
        plant.get_geometry_poses_output_port(),
        scene_graph.get_source_pose_port(plant.get_source_id()),
    )
    builder.Connect(
        scene_graph.get_query_output_port(),
        plant.get_geometry_query_input_port(),
    )

builder = DiagramBuilder()
scene_graph = builder.AddSystem(SceneGraph())
plant_1 = builder.AddSystem(MultibodyPlant(time_step=0.0))
register_plant_with_scene_graph(scene_graph, plant_1)
plant_2 = builder.AddSystem(MultibodyPlant(time_step=0.0))
register_plant_with_scene_graph(scene_graph, plant_2)

As Sean warned above, you will need to be careful with this pairing.