Fixes up InverseDynamicsController to use the actuation matrix

[RussTedrake]

Clarifies that the control output of the inverse dynamics controller
is actuation, not generalized forces, and uses the actuation matrix to
achieve this. Previously, by connecting the output of the
InverseDynamicsController system to an actuation input port of the
MultibodyPlant, 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.

The generalized_force output port is still available, partly for
backwards compatibility, and because it's possible that there are
applications that prefer this.

This is a rewrite of PR #20971.
+@sherm1 for feature review, please (as the reviewer of #20971)
cc @EricCousineau-TRI who was involved in the first PR.
cc @siddancha

Note: I will follow-up with changes to Anzu as needed.

---

This change is 

[RussTedrake]

+@sherm1 for feature review, please?

[siddancha]

I'm not an official reviewer. But caught a couple of things (that might not turn out to be real issues).

Reviewable status: 2 unresolved discussions, LGTM missing from assignee sherm1(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @RussTedrake)

---

systems/controllers/inverse_dynamics_controller.h line 35 at r1 (raw file):

 * inverse dynamics computation.
 *
 * @note In the case where the plant is not fully actuated, then B⁻¹ is

Docs currently states that "this class assumes the robot is fully actuated" [1, 2]. Is that not true anymore since we're now using an actuation matrix that need not be full rank? Or we do we still need to assume the plant is fully actuated, in which case this comment is redundant?

---

systems/controllers/test/inverse_dynamics_controller_test.cc line 76 at r1 (raw file):

        robot_context == nullptr ? robot_plant.CreateDefaultContext()
                                 : robot_context->Clone();
    VectorX<double> expected_force = controllers_test::ComputeTorque(

Is it weird that we have "force" on the left hand side, and "ComputeTorque" on the right hand side?

[sherm1]

Feature pending a few minor comments
Release notes should specify breaking change since the existing port is affected.

+@sammy-tri for platform review, please

Reviewed 7 of 7 files at r1, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee sammy-tri(platform), missing label for release notes (waiting on @RussTedrake)

---

systems/controllers/test/inverse_dynamics_controller_test.cc line 76 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Is it weird that we have "force" on the left hand side, and "ComputeTorque" on the right hand side?

BTW consider spelling this out expected_generalized_forces to avoid said weirdness.

---

systems/controllers/test/inverse_dynamics_controller_test.cc line 78 at r1 (raw file):

    VectorX<double> expected_force = controllers_test::ComputeTorque(
        robot_plant, q, v, vd_d, workspace_context.get());
    Eigen::VectorXd expected_torque =

BTW and expected_actuation_forces here would be more clear.

[jwnimmer-tri]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee sammy-tri(platform), missing label for release notes (waiting on @RussTedrake)

---

systems/controllers/inverse_dynamics_controller.h line 165 at r1 (raw file):

   * Returns the output port for computed actuation/control.
   */
  const OutputPort<T>& get_output_port_control() const final {

BTW This function (which is virtual and specified by StateFeedbackControllerInterfaces) leaves a lot to be desired. I am surprised that this PR doesn't change anything in that base class, at the very least clarifying the base class documentation to clarify the units.

[RussTedrake]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee sammy-tri(platform)

---

systems/controllers/inverse_dynamics_controller.h line 35 at r1 (raw file):

Previously, siddancha (Siddharth Ancha) wrote…

Docs currently states that "this class assumes the robot is fully actuated" [1, 2]. Is that not true anymore since we're now using an actuation matrix that need not be full rank? Or we do we still need to assume the plant is fully actuated, in which case this comment is redundant?

Done. Good call. The class is still restricted to fully actuated (it uses that assumption in other places).

---

systems/controllers/inverse_dynamics_controller.h line 165 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW This function (which is virtual and specified by StateFeedbackControllerInterfaces) leaves a lot to be desired. I am surprised that this PR doesn't change anything in that base class, at the very least clarifying the base class documentation to clarify the units.

I agree. I'm not a fan of that interface, and could even imagine removing it completely.

As it stands right now, there are two general controllers that inherit from it (InverseDynamicsController and PidController). I haven't changed PidController, and probably can't (it's not MbP-aware). Which presents an interesting difficulty -- not all StateFeedbackControllers are MbP aware, and the "units" of generalized_force and actuation are the same... so the change here would not impact any documentation of "units". It's (typically) only the order of the outputs that are impacted.

We also have JointStiffnesControl, and other methods that do not yet inherit from this base. They are also not updated to use B inverse yet. I could decide to add the inheritence when I fix those implementations.

---

systems/controllers/test/inverse_dynamics_controller_test.cc line 76 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW consider spelling this out expected_generalized_forces to avoid said weirdness.

Done.

---

systems/controllers/test/inverse_dynamics_controller_test.cc line 78 at r1 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW and expected_actuation_forces here would be more clear.

Done.

[sherm1]

Reviewed 2 of 2 files at r2, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee sammy-tri(platform)

[sammy-tri]

Reviewed 7 of 7 files at r1.
Reviewable status: 3 unresolved discussions

[sammy-tri]

I think the blocking comments remaining can probably all be dismissed, but I'll let Russ make that call.

Reviewable status: 3 unresolved discussions