Actuation coordinate ordering does not necessarily match ordering of velocity coordinates in MbP

[edrumwri]

The following discussion assumes that the number of actuation variables equals the number of velocity variables in MBP.

It is the case that the ordering of actuation variables may not match the ordering of velocity variables, at least (but not necessarily limited to) the case where the number of model instances is greater than one.

Rick discovered this counterintuitive behavior when attempting to use the inverse dynamics controller to control a simple plant comprised of three model instances (his code provided inline below). When connecting the output of the ID controller to a feature-in-progress that adds a single actuation port (for controlling all model instances) to MBP, the controls do not match those obtained by demultiplexing the ID control output and feeding the results into the model-instance-actuation ports of the MBP.

I've been able to work around this problem to keep Rick going, but only with an ugly kludge to the inverse dynamics controller. Observe the necessary changes to inverse_dynamics.cc starting at Line 82:

const VectorX<T> output_gv = plant.CalcInverseDynamics(
    *multibody_plant_context_, desired_vd, external_forces);
Eigen::VectorBlock<VectorX<T>> output_u = output->get_mutable_value();
for (multibody::ModelInstanceIndex i(0); i < plant.num_model_instances(); ++i) {
  plant.SetActuationInArray(i, plant.GetVelocitiesFromArray(i, output_gv),
                            &output_u);
}

Previously, we only had to do:

output->get_mutable_value() = plant.CalcInverseDynamics(*multibody_plant_context_, desired_vd, external_forces);

We'd like to PR the input actuation port (Rick and I working together), but the simple test that Rick devised breaks if I don't introduce this ugly kludge to inverse dynamics, assuming that it will take Alejandro a while to correct this architecture problem. Question for discussion: shall we submit the PR anyway? Inverse dynamics is kind of broken without it...

Rick's test code (including a re-mapping workaround) demonstrating the use case can be found in this branch.

To run do:
bazel run //examples/2d_gripper:2d_prismatic_gripper_simulation

cc @rcory @sherm1

[sherm1]

Per f2f discussions, with my own spin. Apparently users have two mostly-disjoint ways of thinking about multibody systems:

1. viewed as a collection of Model Instances, or
2. viewed as a single composite system, basically M v̇ = τ

Viewpoint 1 looks at quantities like force and velocities grouped conceptually by Model Instance, while viewpoint 2 sees only the underlying physics. There is no simple relationship between Model Instance groupings and the underlying physical (and computational) representation, which is chosen for efficiency and numerical accuracy, and subject to change with internal algorithm refinement. MBPlant provides utilities for mapping between the representations.

I think this makes good sense and we should make it easy to use either viewpoint. The problem @rcory found that @edrumwri reported above is due to incomplete support for these viewpoints. The inverse dynamics controller only supports viewpoint 2 (unified physical system), while MultibodyPlant supports only viewpoint 1 (per-model instance) actuation ports.

We can fix the immediate problem by teaching inverse dynamics about Model Instances and associated actuator orderings, or by teaching MultibodyPlant to accept full τ on some input port.

Drake users: please let us know your thoughts on this.

[amcastro-tri]

Thanks @rcory, @edrumwri and @sherm1 for working on this. I think supporting the two viewpoints would be nice and espcially since we already had a couple of stories in which people expect 2, at least for simple fully actuated models.

For the short term, I think it does make sense to teach the IDC to understand model instances as @edrumwri did. In my mind that does seem like the right abstraction and it was only a bug that we didn't do it like that in the first place.

Supporting viewpoint 1 might be doable once #10068 merges in and we review the internal MBP tree structure. As @sherm1 pointed out, even if we internally shuffle dofs user code written in terms of MBP APIs should not be affected.

[rcory]

As a side note related to Model Instances, I approached the implementation from viewpoint 2, where I was parsing the same sdf multiple times but expected the result to be a single composite model. The only reason I ended up with multiple model instances is because Parser::AddModelFromFile forces the user to create a new model instance for each parse, rather than providing the option of adding to an existing model instance, or preferably, in this case, allowing me to ignore model instances altogether.

We can fix the immediate problem by teaching inverse dynamics about Model Instances and associated actuator orderings, or by teaching MultibodyPlant to accept full τ on some input port.

@sherm1 I agree with the two options. For the first option, if we teach inverse dynamics controller about Model Instances and associated actuator ordering, then I think we'd also want inverse dynamics controller to provide multiple torque output ports (per model instance), each of which would directly connect to each corresponding MutlibodyPlant (model instance) actuation input port.

For the second option, if we teach MultibodyPlant to accept full τ on some input port, I think @jwnimmer-tri's slack comment makes sense, i.e., have the full τ be ordered by velocity index coming out of the inverse dynamics controller, and let MultibodyPlant's input port re-arrange it as it sees fit for internal use.

The one remaining question in my mind is, what is the expected ordering for the reference state input port of inverse dynamics controller (which isn't currently documented)? Perhaps it is up to the user to call SetPositionFromArray and SetVelocitiesFromArray when creating this reference state input, which I feel should at least be included in the inverse dynamics controller documentation for reference.

[amcastro-tri]

Perhaps it is up to the user to call SetPositionFromArray and SetVelocitiesFromArray when creating this reference state input, which I feel should at least be included in the inverse dynamics controller documentation for reference.

I think that's a great idea.

[edrumwri]

@sherm1 I agree with the two options. For the first option, if we teach inverse dynamics controller about Model Instances and associated actuator ordering, then I think we'd also want inverse dynamics controller to provide multiple torque output ports (per model instance), each of which would directly connect to each corresponding MutlibodyPlant (model instance) actuation input port.

This is fine, but since we're revisiting this architecture, we should strive for consistency. MBP exports all continuous state from one port. So it would seem that continuous state should then be output per model instance as well.

[sherm1]

Per f2fs, I believe we have converged for now on just adding a τ (generalized force) input port to MBP ordered identically to generalized velocities. The particular ordering is not to be specified, just that the ordering is consistent (with mass matrix, Jacobians, etc.). The API already provides nice access routines for finding and setting particular entries.

No change is required to the ID Controller with this scheme. The Model Instance-oriented actuator ports to MBP remain and continue to function; anything fed into the τ port is additive.

[tri-ltyyu]

This will be obsolete once the other feature is done.

[sherm1]

@rcory @amcastro-tri can we close this now?

[rcory]

There are a couple things that I think should be addressed before closing this issue.

Inverse Dynamics documentation:

The one remaining question in my mind is, what is the expected ordering for the reference state input port of inverse dynamics controller (which isn't currently documented)? Perhaps it is up to the user to call SetPositionFromArray and SetVelocitiesFromArray when creating this reference state input, which I feel should at least be included in the inverse dynamics controller documentation for reference.

Additionally, InverseDynamicsController states that its output is "torque", while MultibodyPlant::get_applied_generalized_force_input_port() talks about "tau". I think for consistency the notation on both should be the same (maybe reference System Dynamics)? Also, might be a good idea to call out in the documentation for InverseDynamicsController that its output port is to be connected to MultibodyPlant::get_applied_generalized_force_input_port() and not MultibodyPlant::get_actuation_input_port, since for fully actuated systems these BasicVector ports would be exactly the same size and easily swapped by mistake (I've done this myself, and have seen others do it as well).

[sammy-tri]

Did you mean MultibodyPlant::get_applied_generalized_force_input_port?

I started to write something longer a number of times and then (I hope) realized why I was getting confused... which is that the uses of InverseDynamicsController which I typically encounter always use MultibodyPlant::get_actuation_input_port, since the controller is only generating torques for one model instance in a different, larger plant, but I don't think that's relevant to the use case being discussed here.

In terms of documentation changes, we should probably try not to confuse users of either configuration.

[rcory]

@sammy-tri Yes, I did mean MultibodyPlant::get_applied_generalized_force_input_port (I've updated my above comment too).

Let me attempt to clarify what I understand to be the problem, and perhaps we can converge on what changes in documentation would be warranted. The system dynamics are given as:
M(q)v̇ + C(q, v)v = τ = Bu, where τ represents the generalized forces vector, B the actuator selector matrix, and u the actuation vector. The output of InverseDynamicsController is τ. My understanding is that τ is ordered according to velocity index ordering, and u is ordered according to actuator ordering. For InverseDynamicsController which only supports fully actuated systems we have τ.size() == u.size(). Therefore we have two options: 1) We convert generalized forces τ to actuation values u via the actuator selector matrix B, where the relationship u = B.Inverse() * τ holds. Then u can then be fed directly into MultibodyPlant::get_acutation_input_port(). Or, option 2) we directly feed τ (the output of IDC) into MultibodyPlant::get_applied_generalized_force_input_port.

The fact that you are feeding the output of IDC (τ) into MultibodyPlant::get_actuation_input_port() (u) and it still works seems to be lucky, because you have a single model instance in your control plant? If you were to have two model instances in your control plant, I believe the ordering τ and u would differ, and this wiring wouldn't work anymore. At least., that's what I've seen.

[jwnimmer-tri]

I wonder if #22315 fully resolved this issue?