How to construct a simulation in the pydrake/manipulation framework in python?

Question

As part of our project for the manipulation course on robotics, we want to simulate a robot system using the pydrake / manipulation libraries. We have been working on this for quite some time, and are struggling to get the basic things to work.

I think that the main difficulty for us, is that we don't know how different parts of the code interact to create a simulation environment (I give a concrete example below, but that is just the latest in a long series of seemingly small components, each of which took us several hours to understand).

So, the main question here is: is there a good resource explaining all the components that go into constructing a pydrake Diagram / Parser / station etc.?

Just to clarify, we have read chapter 2 of the manipulation course notes which explains some of the logic of how components in the diagram are connected, and completed the exercise of creating our own sdf. However, we still don't feel like we fully understand the nitty-gritty details of setting up a simulation environment from scratch. We tried looking at the many examples spread throughout the deepnote notebooks attached to the chapters of the manipulation course, but each of them constructs the simulator in a very different manner, often without detailed documentation over what each step represents, and there are many long-range effects (e.g., I was using the iiwa7_no_collision.sdf instead of the iiwa7_with_box_collisions.sdf and then my model did not detect collisions between the iiwa and the table; in retrospect, this might seem like an obvious cause for the problem, but there are tens of things going on in constructing a scenario and they all affect one another, making it hard to isolate the root causes of problems).

Is there a resource with one or more examples of a set of sdf files + dmd.yamls / python Parser based scene construction + diagram / scene_graph / plant / context building, that explains what each step in each of those files is in charge of?

To further illustrate the type of difficulties we are running into, here is an example of something we have been working on for the past few hours.

Our project uses an iiwa arm and several objects such as foam bricks, so we are initializing the system with a dmd.yaml file along the following lines. We want to be able to get and set the frame / pose of the foam bricks (down the line, we will use vision to find the position and the arm to move the bricks, but for testing ideas this would still be useful).

One approach would be to weld the foam_bricks to a certain frame (see foam_brick_2 below), which allows us to get its frame using plant.GetFrameByName, but has the disadvantage that it welds the brick in place. Another option would be to use plant.GetFreeBodyPose / plant.SetFreeBodyPose on foam_brick_1, but this doesn't work because foam_brick_1 is not defined as a body in the simulation. Each of the iiwa arm links is defined as both a frame and a body, but it is not clear what part of the code is responsible for setting this up, and how we can tell the plant that foam_brick_1 is also a frame / body.

directives:


- add_model:
    name: iiwa
    file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_with_box_collision.sdf
    default_joint_positions:
        iiwa_joint_1: [0]
        iiwa_joint_2: [0.1]
        iiwa_joint_3: [0]
        iiwa_joint_4: [-1.2]
        iiwa_joint_5: [0]
        iiwa_joint_6: [ 1.6]
        iiwa_joint_7: [0]
- add_weld:
    parent: world
    child: iiwa::iiwa_link_0
    X_PC:
        translation: [0.0, 0.0, 0.0]

- add_model:
    name: wsg
    file: package://drake/manipulation/models/wsg_50_description/sdf/schunk_wsg_50_with_tip.sdf

- add_weld:
    parent: iiwa::iiwa_link_7
    child: wsg::body
    X_PC:
        translation: [0, 0, 0.09]
        rotation: !Rpy { deg: [90, 0, 90]}

...

- add_model:
    name: foam_brick_1
    file: package://drake/examples/manipulation_station/models/061_foam_brick.sdf
    default_free_body_pose:
        base_link:
             translation: [0.43000000000000005, 0.0, 0.34]

- add_frame:
    name: foam_brick_2_origin
    X_PF:
        base_frame: world
        translation: [0.63, 0.2, 0.24]


- add_model:
    name: foam_brick_2
    file: package://drake/examples/manipulation_station/models/061_foam_brick.sdf

- add_weld:
    parent: foam_brick_2_origin
    child:  foam_brick_2::base_link  
...

We tried reading the relevant chapters in the manipulation course notes, and reading through several examples of notebooks from the course. We hoped to find an overarching theme, but the various implementations of simulations were so different from one another that it was hard to find which decisions affected which outcomes.

Answer 1

The reason for the high variance is that the manipulation notes are in the middle of their transition to what I hope is a cleaner / unified framework using MakeHardwareStation. The hardware station loads everything directly from yaml and should be your one-stop shop for setting up everything that is conceptually a model of the robot hardware (physics, low-level controllers, cameras, and visualization).

Any examples you've seen which call MakeManipulationStation are simply waiting for me to port them to MakeHardwareStation. In some simpler settings (e.g. the finger is a point, and I'm just trying to draw the contact forces between a point and a box), then I set up the diagram manually because I just need the plant and not all of the other features HardwareStation can provide. But I take the feedback, and will keep making things have a more consistent / uniform / well-documented feel! In the mean time, please try to build off the examples in those notes which use HardwareStation.

---

For your specific foam brick example, the recommended workflow would be to leave the brick as a floating base body (don't weld it to any frames), and use default_free_body_pose, as you've done, to set it's initial position. If you want to read the current pose of the brick, e.g. for your controller, then you can do that by evaluating the station output ports (e.g. the cheat ports) that give you body poses out.

If you are just trying to debug things and want to move objects around outside of the simulation workflow, then it's true that you can set/get via the plant API e.g.

plant = station.GetSubsystemByName("plant")
plant_context = plant.GetMySubsystemContextFromRoot(
   simulator.get_mutable_context())  # or similar
foam_brick_1 = plant.GetBodyByName("foam_brick_1")
X_FB1 = plant.GetFreeBodyPose(plant_context, foam_brick_1)

If you set the free-body pose in the Context, and want the updated pose to appear in the visualizer, then remember you'll need to publish the diagram's visualizers, e.g. with diagram.ForcedPublish(diagram_context).