Fixup JointStiffnessController to output actuation instead of general…

…ized_force (RobotLocomotion#22329)

Follows RobotLocomotion#22315, which implemented the same fix for InverseDynamicsController.

Previously there was a tacit assumption that the actuators were
declared in the same order as the joints. This PR resolves that, and
(to make this clear) renames the output port from `generalized_force`
to `actuation`.

bindings/pydrake/systems/BUILD.bazel

@@ -486,6 +486,7 @@ drake_py_unittest(
     deps = [
         ":controllers_py",
         ":test_util_py",
+        "//bindings/pydrake/common/test_utilities:deprecation_py",
         "//bindings/pydrake/examples",
         "//bindings/pydrake/multibody:parsing_py",
         "//bindings/pydrake/multibody:plant_py",

bindings/pydrake/systems/controllers_py.cc

@@ -150,26 +150,33 @@ PYBIND11_MODULE(controllers, m) {
   {
     using Class = JointStiffnessController<double>;
     constexpr auto& cls_doc = doc.JointStiffnessController;
-    py::class_<Class, LeafSystem<double>>(
-        m, "JointStiffnessController", cls_doc.doc)
-        .def(py::init<const MultibodyPlant<double>&,
-                 const Eigen::Ref<const Eigen::VectorXd>&,
-                 const Eigen::Ref<const Eigen::VectorXd>&>(),
-            py::arg("plant"), py::arg("kp"), py::arg("kd"),
-            // Keep alive, reference: `self` keeps `robot` alive.
-            py::keep_alive<1, 2>(), cls_doc.ctor.doc)
+    py::class_<Class, LeafSystem<double>> cls(
+        m, "JointStiffnessController", cls_doc.doc);
+    cls.def(py::init<const MultibodyPlant<double>&,
+                const Eigen::Ref<const Eigen::VectorXd>&,
+                const Eigen::Ref<const Eigen::VectorXd>&>(),
+           py::arg("plant"), py::arg("kp"), py::arg("kd"),
+           // Keep alive, reference: `self` keeps `robot` alive.
+           py::keep_alive<1, 2>(), cls_doc.ctor.doc)
         .def("get_input_port_estimated_state",
             &Class::get_input_port_estimated_state, py_rvp::reference_internal,
             cls_doc.get_input_port_estimated_state.doc)
         .def("get_input_port_desired_state",
             &Class::get_input_port_desired_state, py_rvp::reference_internal,
             cls_doc.get_input_port_desired_state.doc)
-        .def("get_output_port_generalized_force",
-            &Class::get_output_port_generalized_force,
-            py_rvp::reference_internal,
-            cls_doc.get_output_port_generalized_force.doc)
+        .def("get_output_port_actuation", &Class::get_output_port_actuation,
+            py_rvp::reference_internal, cls_doc.get_output_port_actuation.doc)
         .def("get_multibody_plant", &Class::get_multibody_plant,
             py_rvp::reference_internal, cls_doc.get_multibody_plant.doc);
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+    cls.def("get_output_port_generalized_force",
+        WrapDeprecated(cls_doc.get_output_port_generalized_force.doc_deprecated,
+            &Class::get_output_port_generalized_force),
+        py_rvp::reference_internal,
+        cls_doc.get_output_port_generalized_force.doc_deprecated);
+#pragma GCC diagnostic pop
   }
 
   {

bindings/pydrake/systems/test/controllers_test.py

@@ -5,7 +5,7 @@
 
 import numpy as np
 
-from pydrake.common import FindResourceOrThrow
+from pydrake.common.test_utilities.deprecation import catch_drake_warnings
 from pydrake.examples import PendulumPlant
 from pydrake.multibody.tree import MultibodyForces
 from pydrake.multibody.plant import MultibodyPlant
@@ -128,8 +128,12 @@ def test_joint_stiffness_controller(self):
         self.assertEqual(controller.get_input_port_estimated_state().size(),
                          14)
         self.assertEqual(controller.get_input_port_desired_state().size(), 14)
-        self.assertEqual(controller.get_output_port_generalized_force().size(),
+        self.assertEqual(controller.get_output_port_actuation().size(),
                          7)
+
+        with catch_drake_warnings(expected_count=1) as w:
+            controller.get_output_port_generalized_force()
+
         self.assertIsInstance(controller.get_multibody_plant(), MultibodyPlant)
 
     def test_inverse_dynamics(self):

systems/controllers/BUILD.bazel

@@ -202,7 +202,6 @@ drake_cc_googletest(
 drake_cc_googletest(
     name = "joint_stiffness_controller_test",
     data = [
-        "//multibody/benchmarks/acrobot:models",
         "@drake_models//:iiwa_description",
     ],
     deps = [

systems/controllers/joint_stiffness_controller.cc

@@ -35,6 +35,7 @@ JointStiffnessController<T>::JointStiffnessController(
 
   DRAKE_DEMAND(kp.size() == num_q);
   DRAKE_DEMAND(kd.size() == num_q);
+  Binv_ = plant_->MakeActuationMatrixPseudoinverse();
 
   input_port_index_estimated_state_ =
       this->DeclareInputPort("estimated_state", kVectorValued, num_states)
@@ -48,13 +49,24 @@ JointStiffnessController<T>::JointStiffnessController(
   // other dependencies. Specifying this here is also essential so that that
   // GetDirectFeedthrough won't attempt to cast to Symbolic for evaluation
   // (which would fail if the plant is not owned).
-  output_port_index_force_ =
+  output_port_index_actuation_ =
       this->DeclareVectorOutputPort(
-              "generalized_force", num_q,
-              &JointStiffnessController<T>::CalcOutputForce,
+              "actuation", num_q,
+              &JointStiffnessController<T>::CalcOutputActuation,
               {this->all_input_ports_ticket()})
           .get_index();
 
+  auto& output_port_force = this->DeclareVectorOutputPort(
+          "generalized_force", num_q,
+          &JointStiffnessController<T>::CalcOutputForce,
+          {this->all_input_ports_ticket()});
+  output_port_index_force_ = output_port_force.get_index();
+  this->DeprecateOutputPort(
+      output_port_force,
+      "Use the 'actuation' output port instead, which multiplies the "
+      "generalized force by B⁻¹ to be consumed by MultibodyPlant's actuation "
+      "input port.");
+
   auto plant_context = plant_->CreateDefaultContext();
 
   // Declare cache entry for the multibody plant context.
@@ -117,6 +129,7 @@ void JointStiffnessController<T>::CalcMultibodyForces(
   plant_->CalcForceElementsContribution(plant_context, cache_value);
 }
 
+// This method should be removed with the removal of the deprecated output port.
 template <typename T>
 void JointStiffnessController<T>::CalcOutputForce(
     const Context<T>& context, BasicVector<T>* output) const {
@@ -157,6 +170,46 @@ void JointStiffnessController<T>::CalcOutputForce(
   output->get_mutable_value() = tau;
 }
 
+template <typename T>
+void JointStiffnessController<T>::CalcOutputActuation(
+    const Context<T>& context, BasicVector<T>* output) const {
+  const int num_q = plant_->num_positions();
+
+  const auto& plant_context =
+      this->get_cache_entry(plant_context_cache_index_)
+          .template Eval<Context<T>>(context);
+
+  // These include gravity.
+  const auto& applied_forces =
+      this->get_cache_entry(applied_forces_cache_index_)
+          .template Eval<MultibodyForces<T>>(context);
+
+  // Compute inverse dynamics with zero generalized accelerations.
+  // ID(q, v, v̇)  = M(q)v̇ + C(q, v)v - tau_app
+  // So with v̇ = 0 we get:
+  // ID(q, v, 0) = C(q,v)v - tau_app(q).
+  VectorX<T> tau = plant_->CalcInverseDynamics(
+      plant_context, VectorX<T>::Zero(num_q), /* vdot = 0 */
+      applied_forces);
+
+  // Subtract off C(q,v)v
+  // Note: we do not simply set v=0 because we want to be able to cancel the
+  // contribution from damping forces.
+  VectorX<T> Cv(num_q);
+  plant_->CalcBiasTerm(plant_context, &Cv);
+  tau -= Cv;
+
+  // Add in the stiffness terms.
+  const VectorX<T>& x = get_input_port_estimated_state().Eval(context);
+  const VectorX<T>& x_d = get_input_port_desired_state().Eval(context);
+
+  tau += (kp_.array() * (x_d.head(num_q) - x.head(num_q)).array() +
+          kd_.array() * (x_d.tail(num_q) - x.tail(num_q)).array())
+             .matrix();
+
+  output->get_mutable_value() = Binv_ * tau;
+}
+
 }  // namespace controllers
 }  // namespace systems
 }  // namespace drake

systems/controllers/joint_stiffness_controller.h

@@ -18,16 +18,16 @@ namespace controllers {
 /**
  * Implements a joint-space stiffness controller of the form
  * <pre>
- *   τ_control = −τ_g(q) − τ_app + kp⊙(q_d − q) + kd⊙(v_d − v)
+ *   τ_control = B⁻¹[−τ_g(q) − τ_app + kp⊙(q_d − q) + kd⊙(v_d − v)]
  * </pre>
  * where `Kp` and `Kd` are the joint stiffness and damping coefficients,
  * respectively, `τ_g(q)` is the vector of generalized forces due to gravity,
  * and `τ_app` contains applied forces from force elements added to the
  * multibody model (this can include damping, springs, etc. See
- * MultibodyPlant::CalcForceElementsContribution()).  `q_d` and `v_d` are the
- * desired (setpoint) values for the multibody positions and velocities,
- * respectively. `kd` and `kp` are taken as vectors, and ⊙ represents
- * elementwise multiplication.
+ * MultibodyPlant::CalcForceElementsContribution()). B⁻¹ is the inverse of the
+ * actuation matrix. `q_d` and `v_d` are the desired (setpoint) values for the
+ * multibody positions and velocities, respectively. `kd` and `kp` are taken as
+ * vectors, and ⊙ represents elementwise multiplication.
  *
  * The goal of this controller is to produce a closed-loop dynamics that
  * resembles a spring-damper dynamics at the joints around the setpoint:
@@ -48,7 +48,7 @@ namespace controllers {
  * - estimated_state
  * - desired_state
  * output_ports:
- * - generalized_force
+ * - actuation
  * @endsystem
  *
  * Note that the joint impedance control as implemented on Kuka's iiwa and
@@ -129,10 +129,21 @@ class JointStiffnessController final : public LeafSystem<T> {
   /**
    * Returns the output port for the generalized forces implementing the
    * control. */
+  DRAKE_DEPRECATED("2025-04-01",
+                   "Use get_output_port_actuation() instead, which multiplies "
+                   "the generalized force by B⁻¹ to be consumed by "
+                   "MultibodyPlant's actuation input port.")
   const OutputPort<T>& get_output_port_generalized_force() const {
     return this->get_output_port(output_port_index_force_);
   }
 
+  /**
+   * Returns the output port implementing the control in the form (and order)
+   * expected for the plant's actuation input port. */
+  const OutputPort<T>& get_output_port_actuation() const {
+    return this->get_output_port(output_port_index_actuation_);
+  }
+
   /**
    * Returns a constant pointer to the MultibodyPlant used for control.
    */
@@ -150,9 +161,12 @@ class JointStiffnessController final : public LeafSystem<T> {
 
   template <typename> friend class JointStiffnessController;
 
+  // Calculator for the deprecated output port.
+  void CalcOutputForce(const Context<T>& context, BasicVector<T>* force) const;
+
   // This is the calculator method for the output port.
-  void CalcOutputForce(const Context<T>& context,
-                       BasicVector<T>* force) const;
+  void CalcOutputActuation(const Context<T>& context,
+                           BasicVector<T>* force) const;
 
   // Methods for updating cache entries.
   void SetMultibodyContext(const Context<T>&, Context<T>*) const;
@@ -164,9 +178,11 @@ class JointStiffnessController final : public LeafSystem<T> {
 
   int input_port_index_estimated_state_{0};
   int input_port_index_desired_state_{0};
+  int output_port_index_actuation_{0};
   int output_port_index_force_{0};
 
   Eigen::VectorXd kp_, kd_;
+  Eigen::SparseMatrix<double> Binv_;
 
   drake::systems::CacheIndex applied_forces_cache_index_;
   drake::systems::CacheIndex plant_context_cache_index_;

systems/controllers/test/joint_stiffness_controller_test.cc

@@ -11,17 +11,36 @@ namespace systems {
 namespace controllers {
 namespace {
 
+using Eigen::Matrix2d;
 using Eigen::Vector2d;
 using Eigen::Vector4d;
 using Eigen::VectorXd;
 using multibody::MultibodyPlant;
 
 GTEST_TEST(JointStiffnessControllerTest, SimpleDoublePendulum) {
+  std::string xml = R"""(
+<mujoco model="test">
+  <worldbody>
+    <body name="upper_arm" pos="0 0 2">
+      <joint name="shoulder" type="hinge" axis="0 1 0" damping="0.1"/>
+      <geom name="upper_arm" fromto="0 0 0 0 0 1" size="0.05" type="capsule" mass="1"/>
+      <body name="lower_arm" pos="0 0 1">
+        <joint name="elbow" type="hinge" axis="0 1 0" damping="0.1"/>
+        <geom name="lower_arm" fromto="0 0 0 0 0 1" size="0.049" type="capsule" mass="1"/>
+      </body>
+    </body>
+  </worldbody>
+   <actuator>
+    <!-- intentionally list these in reverse order -->
+    <motor name="elbow" joint="elbow"/>
+    <motor name="shoulder" joint="shoulder"/>
+  </actuator>
+</mujoco>
+)""";
+
   DiagramBuilder<double> builder;
   auto plant = builder.AddSystem<MultibodyPlant>(0.0);
-  multibody::Parser(plant).AddModelsFromUrl(
-      "package://drake/multibody/benchmarks/acrobot/double_pendulum.urdf");
-  plant->WeldFrames(plant->world_frame(), plant->GetFrameByName("base"));
+  multibody::Parser(plant).AddModelsFromString(xml, ".xml");
   plant->Finalize();
 
   Vector2d kp{0.3, 0.4}, kd{0.1, 0.2};
@@ -48,14 +67,25 @@ GTEST_TEST(JointStiffnessControllerTest, SimpleDoublePendulum) {
   // We expect the controller to cancel gravity and damping, and add the
   // stiffness terms.
   const double kDamping = 0.1;  // must match double_pendulum.urdf
-  VectorXd tau_expected = -plant->CalcGravityGeneralizedForces(plant_context) +
-                          kDamping * x.tail<2>() +
-                          (kp.array() * (x_d.head<2>() - x.head<2>()).array() +
-                           kd.array() * (x_d.tail<2>() - x.tail<2>()).array())
-                              .matrix();
-  VectorXd tau = controller->get_output_port().Eval(controller_context);
-
+  const VectorXd tau_expected =
+      -plant->CalcGravityGeneralizedForces(plant_context) +
+      kDamping * x.tail<2>() +
+      (kp.array() * (x_d.head<2>() - x.head<2>()).array() +
+       kd.array() * (x_d.tail<2>() - x.tail<2>()).array())
+          .matrix();
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+  const VectorXd tau =
+      controller->get_output_port_generalized_force().Eval(controller_context);
   EXPECT_TRUE(CompareMatrices(tau, tau_expected, 1e-14));
+#pragma GCC diagnostic pop
+
+  const Matrix2d Binv =
+      (Matrix2d() << 0, 1, 1, 0)
+          .finished();  // actuators were registered in reverse order.
+  const VectorXd u =
+      controller->get_output_port_actuation().Eval(controller_context);
+  EXPECT_TRUE(CompareMatrices(u, Binv * tau_expected, 1e-14));
 }
 
 GTEST_TEST(JointStiffnessControllerTest, ScalarConversion) {