Meta issue: easier way to interface controllers to MultibodyPlant

[edrumwri]

(This issue has arisen from discussions resulting from PR #12011).

We can imagine an easier way to interface controllers to MultibodyPlant, generally, than at present. The difficulty with the present approach only arises when only some bodies in a MBP are controlled.

Two anecdotal problems:

1. A MBP contains multiple model instances that are to be controlled as a single unit (e.g., Rick's planar example that consists of a single model duplicated twice more). The controllers (PID, inverse dynamics, etc.) do not "know" about model instances, and no ready procedure exists for demultiplexing the controller outputs to the actuator inputs.
2. At least some controllers (PID, inverse dynamics) output vectors in generalized-velocity-coordinates, which is the same dimension (at least for ID) as the actuation inputs. It's too easy to connect the control outputs to the wrong port. In my experience, the order of actuation has been identical to the order of generalized velocities, but only when a MBP has a single model instance.

Solving the problems above might require more than one approach (e.g., using typed input ports for (2) and some intermediate Systems for (1)).

We'll continue to gather information (problems and potential solutions) in this issue.

@rcory @SeanCurtis-TRI @amcastro-tri

[amcastro-tri]

IMO the best way to go with this is working on a specific example (with pseudocode allowed). @rcory, @edrumwri recently mentioned to me that you might have such an example?

[sherm1]

@rcory do you think this is still an issue? Or should we close it?

[rcory]

I think this is still a valid issue. For an example, see this method in planar_gripper_simulation which defines a local system which demultiplexes the inverse dynamics outputs to MBP actuator inputs. This seems like a common enough use case that perhaps MBP should support this natively?

[amcastro-tri]

I believe the problem you have @rcory could be solved with some improvements to the InverseDynamicsController. What I propose is:

1. New constructor's signature taking the full model of the plant and a model instance of the particular model to control.
2. We add an output port to the ID controller with exactly the right format MBP needs to consume. This might not be needed if we use the output generalized forces.

Then in your example you would not need the extra GeneralizedForceToActuationOrdering since the ID controller's output would wire directly to the plant of the whole world.

Thoughts?

[EricCousineau-TRI]

Per comment here: #13074 (comment)

I believe my prototype can help workaround some of these pain points - e.g. rather than trying to craft a whole bunch of model instances / coordinates for accessing portions of a plant, just reconstruct the plant (cheaply).

[amcastro-tri]

The last time I wrote here was "thoughts?". Wondering if the solution I described above would not only suffice but be appropriate and, its probably simpler?
That is, if I build an InverseDynamicsController with a full model of the world when I just want to control an arm in it, how can I expect to get the right output/inputs? that is why my suggestion.

[EricCousineau-TRI]

In my mind, I think the solution you described above is more complex; why write yet another overload in MBP / the controller to do the same computation, but only on a subset, add complexity to your testing code, deprecations, etc., when all you really need to do is just copy over a subset?

how can I expect to get the right output/inputs?

Is it just ordering? There can be ways to guarantee that through MBP's construction, or just adding a basic remapping on the port or whatever.

[amcastro-tri]

Is it just ordering? There can be ways to guarantee that through MBP's construction, or just adding a basic remapping on the port or whatever.

That's exactly right. IMO is not that simple (yes, I can hack the code myself), but clearly not simple for most users who have expressed this being the biggest pain point. Designing a general solution to extract a sub-plant and the remap, for a final product in Drake, will require some thought (totally worth it!).

But again, I just think we are asking too much from the InverseDynamicsController API, which was originally written to deal with a small robot model.

[EricCousineau-TRI]

But again, I just think we are asking too much from the InverseDynamicsController API, which was originally written to deal with a small robot model.

Can you expand on "asking too much"?

If you mean, "adding overload(s) for dealing with only a subset of the plant", then I am 100% on board with that statement!