Is there any example of using `DirectCollocation` for `MultibodyPlant`?

[ommmid]

I would like to use the DirectCollocation for Kuka robot in the Shelves scene. What I have found is we have to pass the diagram into DirectCollocation not the plant but it would be very helpful to see an example.

Also, I have found this old example too but it is not working based on the current code.

[RussTedrake]

Yes. My underactuated course notes has an updated Direct Collocation notebook which has an example of the dircol with the cart-pole system which was intentionally authored using MultibodyPlant to show how that workflow works.

[ommmid]

Cool, thanks for the direction. Just used that to run it for Kuka but it produces an error saying the plant's query input port is not connected. Here is the code to reproduce the error

[RussTedrake]

The reason your example is failing but the cartpole was succeeding is because your example has collision geometry (which requires a scenegraph). I didn't mean to lay a trap by exploiting that fact with my example; I've gone ahead and updated my cartpole example to pass the diagram to DirectCollocation instead, which is the more general approach. Note that I've also updated it to use RobotDiagram, which automatically adds the plant and scene_graph, which is newer sugar in Drake.

Note that if you do have collision geometry and your collision geometry makes a contact, then you probably do not want to be handling that with DirectCollocation, but with a method that can reason about the contact dynamics (as discussed in the contact chapters of my notes).

[ommmid]

Ah, OK that works now. I guess I figured what my problem was looking at RobotDiagramBuilder code: When I was passing diagram to DirectCollocation the input port of the diagram was not set correctly and I would get error. I mean I had some attempts to make input port for the diagram using build.connect() but that connects input-to-output not input(of the subsystem)-to(input of the diagram).

I was using the following workflow per this suggestion.:

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0) 
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(diagram_context)

But this did not work for me because this workflow does not know that it should connect the input of the plant to the input of the diagram. So I was trapped between passing the plant to DirectCollocation which needed the scene graph but as soon as I would pass diagam (to involve the scene graph), there was no input port that DirectCollocation could use.

Looking at RobotDiagramBuilder I noticed ExportOutput/ExportInput functionanlity that declares the output/input of a subsystem as the output/input port of the whole diagram. And that solved my problem. Thanks for quick replies, now I have better understanding of the system/diagram workflow.