2

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.

adamconkey
  • 4,104
  • 5
  • 32
  • 61

1 Answers1

2
  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)
Xuchen Han
  • 326
  • 1
  • 6
  • Thank you, that example is helpful. Adding the model index for `SetPositions` was key, I didn't realize that was an option. I wasn't sure having two plants was the way to go but it seems that is pretty standard in the examples I've looked at. – adamconkey Sep 29 '22 at 17:14
  • At the moment, the C++ examples under the drake/examples/ directory is richer than the (growing but still limited) tutorials. You can probably find more useful tips there. – Xuchen Han Sep 29 '22 at 20:37