Fixup InverseDynamics to use the actuation matrix.

[RussTedrake]

Clarifies that the output of the inverse dynamics controller is actuation input, not generalized forces, and uses the actuation matrix to achieve this. Previously, by connecting the output of the InverseDynamics system to an actuation input port, we implicitly assumed that the actuator matrix was the identity matrix. If the actuators were in a different order than the joints (generalized forces), then our method produced the wrong actuator inputs.

cc @Michaelszeng

---

This change is 

[RussTedrake]

systems/controllers/inverse_dynamics.cc line 34 at r1 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Easy to do. What do you think about even moving it into MultibodyPlant? e.g.

  /// Creates the inverse of the actuation matrix B directly (without requiring
  /// an explicit inverse calculation). See MakeActuationMatrix().
  /// @throws std::exception if B is not a square and invertible matrix.
  Eigen::SparseMatrix<double> MakeActuationMatrixInverse() const;

(note the double, not T. I think MakeActuationMatrix should not return MatrixX<T>... since it does not depend on the context there is no reasonable way for the output to be anything but double.)

I've pulled that out into #20975.

[RussTedrake]

Previously, RussTedrake (Russ Tedrake) wrote…

Thanks. Yes, we have quite a mess here. Let's try to get it right.

1. the name InverseDynamics suggests to me that the result of the computation is indeed generalized forces.
2. any controller based on inverse dynamics needs to map to the actuation vector.
3. We do have a second class InverseDynamicsController. I could imagine adding a MatrixGain on the end of that system, and leaving InverseDynamics alone. I cannot see any justification for anything called a controller outputting generalized forces directly, instead of an actuator input. I don't even see offering multiple output ports as the right long term solution.

rollout_trajectory_in_sim.py uses InverseDynamicsController, so that doesn't avoid the break. actuator_net_with_mbp_simulation.py uses JointStiffnessController, so I don't think that's immediately in danger. (but I'll be coming for that one soon!)

@AdityaBhat-TRI if you're around. @EricCousineau-TRI wdyt?

MatrixGain isa LinearSystem, and does not support SparseMatrix out of the box. It will require some plumbing to enable that...

[EricCousineau-TRI]

For backwards compatibility, I think some form of deprecation would be nice (e.g. add the correct port to Inverse Dynamics Controllers, deprecate old generalized force form), but I also don't want to delay the more proper form.

I cannot see any justification for anything called a controller outputting generalized forces directly [...]

When testing spatial / SE(3) controller objectives and coupling, it was helpful to debug the objective by simply actuating a floating object. (I am not sure if this is the best justification, but do want to ask about this workflow.) In this case, I had tried both applied external forces and generalized forces, and found generalized forces a bit easier to deal with in terms of generic abstraction.

What would have been the correct way to wire up an inverse dynamics controller in this case? (do we have a way of specifying actuators for floating bases for testing simplified controllers?)