MbP actuation/state (non-)ordering makes correct code unlikely

[ggould-tri]

This is a long-simmering gripe of mine that @rpoyner-tri asked me to scribe here. Apologies if this duplicates an existing issue. It clearly relates to #15462, #13509, and #10546, but is not the same as any of them.

It is my firm belief that MBP callers that do not have a list of joint names or a MatrixGain system -- that is, nearly all of them -- are defective, probably without ever realizing it.

The ordering of MBP's actuation inputs and state outputs is neither documented nor guaranteed. It is therefore impossible to correctly consume directly; most users either maintain a model MBP that matches it (InverseDynamicsController, e.g.) or else simply unsoundly rely on knowing its undocumented behaviour ahead of time (JacoCommandSender, e.g.). This latter case applies to the large number of cases where MBP inputs/outputs are attached to LCM messages are all defective (typically with compensating errors on both ends of the communication link; FYI @sammy-tri per our recent discussion) although this is hard to prove as few of our LCM types document their joint orders.

A small number of applications use the correct idiom, enumerating the relevant dofs manually and calling the selector methods (allegro_hand::GetControlPortMapping, e.g.; there is also a more detailed example in downstream code that I can point out on request). It is instructive to read how the allegro simulation correctly operates the plant: The MBP's state selector matrices are mapped into the gains of the controller and into MatrixGain systems attached to the MBP's ports.

If downstream users are to write good, correct code, we should provide an affordance for them, either adding to MBP or putting some helper functions around it to create ports whose vectors have a specified order by attaching the required MatrixGain systems for them.

An alternative of course would be to document a guaranteed element order for all of these ports. My understanding is that that would be worse than API-level solutions for performance reasons.

[rpoyner-tri]

\cc @amcastro-tri @sherm1 for any thoughts you may have.

\cc @jwnimmer-tri shall we add this to the Adoption backlog?

[jwnimmer-tri]

An alternative of course would be to document a guaranteed element order for all of these ports.

Well, that's the first thing we should attempt in any case. (That's a job for @amcastro-tri or @joemasterjohn.) In other words, are there any promises that we can document today (and maintain in the future) about the order of anything? We can start with that, and then figure out what extra sugar we want beyond that.

We have helpers like MbP::SetActuationInArray() and MbP::GetPositionsFromArray already available, in the case where the user wants per-model-instance operations. For those per-model ports, can we promise anything in the specific case of linear chains of links (i.e., a tree with branching factor == 1)? That is a very common case, and seems unlikely to change (it's the same under BFS and DFS)?

[ggould-tri]

I agree that that would work, although I still think that sugar for accessing substates via a list of names would probably be better: Making code and sdf agree on orders is likely to become ever more difficult for users over time as larger and more varied scenes become less and less hand-editable and thus harder to keep in sync with code -- remember that we're still pretending that xml is a human-editable source format here! Finding orphaned bits of joint and frame shrapnel lying around in sdf/urdf files is becoming quite common in our more complex scenes.

A bit of wrapper sugar like MakePartialStatePort(MultibodyPlant, std::vector<std::tuple<ModelInstanceIndex, std::string, int>>, DiagramBuilder) (or Joint instead of std::string if you prefer) and similar MakePartialActuationPort that slaps the appropriate selector-MatrixGain on the appropriate port and returns its other end really seems simpler to me. Then as long as nobody ever used an MBP port directly we could keep Dynamics' freedom to muck around in state orders and ensure that users call their shots wrt models, joints, and actuators rather than risking brittle dependency on topo-traversals of references in xml documents.

[xuchenhan-tri]

@rpoyner-tri Do you have plans to work on this for the adoption project?
@sherm1 @amcastro-tri @joemasterjohn Any insights to share on this one?

[sherm1]

Could this be addressed by just good examples & tutorials? Or is the current feature set just too awful?

[ggould-tri]

Certainly having correct examples in Drake would be great. But imo the correct usage idiom in its full glory (ie attaching a MatrixGain system to every nontrivial mbp input or output that you use) is probably not going to make your tutorial good.

[JoseBarreiros-TRI]

Certainly having correct examples in Drake would be great. But imo the correct usage idiom in its full glory (ie attaching a MatrixGain system to every nontrivial mbp input or output that you use) is probably not going to make your tutorial good.

Agreed 100%. As a new Drake user, it took me a few hours to debug the issue generated by the wrong actuation/state mapping for my controller (17426). I believe MbP should provide a user-friendly way (e.g naming) of accessing the states and the controller APIs should have an option to select the joint actuators that is controlling. Imo, the use of input and output_porjection matrix (as in the PID) is overcomplicated

[jwnimmer-tri]

Recent updates:

• JointActuator::input_start() provides the offset of that actuator in the MbP overall actuation input port.
• Joint::position_suffix() (ditto velocity) provides for a name of the position which the Python MakeNamedViewPositions can use to view a positions vector using human-readable coordinate names.

[jwnimmer-tri]

... and those are good enough. Everything is documented now.