Search code examples
pythondrake

Structure of MultibodyPlant, SceneGraph, Context, and Simulator for pydrake simulation

I'm getting very confused trying to setup my simulation correctly in PyDrake. What I want is to have an actuated robot (with e.g. an InverseDynamicsController on it) together with an object in the scene that the robot will manipulate. However, I'm struggling to sort out how to create and use the MultibodyPlant, SceneGraph, Context, Simulator combination correctly.

Here is roughly what I've tried to do:

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
parser = Parser(plant, scene_graph)
# Add my robot 
robot = parser.AddModelFromFile(robot_urdf)
robot_base = plant.GetFrameByName('robot_base')
plant.WeldFrames(plant.world_frame(), robot_base)
# Add my object
parser.AddModelFromFile(FindResourceOrThrow("drake/my_object.urdf"))
plant.finalize()

# Add my controller
Kp = np.full(6, 100)
Ki = 2 * np.sqrt(Kp)
Kd = np.full(6, 1)
controller = builder.AddSystem(InverseDynamicsController(plant, Kp, Ki, Kd, False))
controller.set_name("sim_controller");
builder.Connect(plant.get_state_output_port(robot),
                controller.get_input_port_estimated_state())
builder.Connect(controller.get_output_port_control(),
                plant.get_actuation_input_port())

# Get the diagram, simulator, and contexts
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()
plant_context = plant.GetMyContextFromRoot(context)

However, this has some undesirable qualities. First, as long as I've added the object, then I get this error:

Failure at systems/controllers/inverse_dynamics_controller.cc:32 in SetUp(): condition 'num_positions == dim' failed.

Second, with the object added, the object pose becomes part of my InverseKinematics problem, and when I do SetPositions with plant_context, I have to set both my arm joints AND the pose of the object, when I feel like I should only be setting the robot's joint positions with SetPositions.

I realize I've done something wrong with this setup, and I'm just wondering what is the correct way to have an instance of Simulator that I can run simulations with that has both an actuated robot, and a manipulable object? Am I supposed to create multiple plants? Multiple contexts? Who shares what with who?

I'd really appreciate some advice on this, or a pointer to an example. Drake is great, but I struggle to find minimal examples that do what I want.

Solution
    1. Yes, you can add a separate MultibodyPlant for control. See https://github.com/RobotLocomotion/drake/blob/master/examples/planar_gripper/planar_gripper_simulation.cc for an example. The setup is similar to yours, though it's in C++. You can try mimicking the way the diagram is wired up there.
    2. When you do have two plants, you want to call SetPositions on the simulation plant (not the control plant). You can set only the robot positions by using ModelInstanceIndex.
    # Add my robot 
robot = parser.AddModelFromFile(robot_urdf)
...
plant.SetPositions(plant_context, robot, robot_positions)