From c3ec1eb804c2a9b90f0e7899eb4e5d8fd88bd1e9 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Wed, 18 Dec 2024 08:11:14 -0500 Subject: [PATCH] Fixup JointStiffnessController to output actuation instead of generalized_force Follows #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 | 1 + bindings/pydrake/systems/controllers_py.cc | 31 ++++++---- .../pydrake/systems/test/controllers_test.py | 8 ++- systems/controllers/BUILD.bazel | 1 - .../controllers/joint_stiffness_controller.cc | 59 ++++++++++++++++++- .../controllers/joint_stiffness_controller.h | 32 +++++++--- .../test/joint_stiffness_controller_test.cc | 50 ++++++++++++---- 7 files changed, 146 insertions(+), 36 deletions(-) diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index c42c5a3c9434..9d89cd8a810b 100644 --- a/bindings/pydrake/systems/BUILD.bazel +++ b/bindings/pydrake/systems/BUILD.bazel @@ -459,6 +459,7 @@ drake_py_unittest( ], deps = [ ":controllers_py", + "//bindings/pydrake/common/test_utilities:deprecation_py", "//bindings/pydrake/examples", "//bindings/pydrake/multibody:parsing_py", "//bindings/pydrake/multibody:plant_py", diff --git a/bindings/pydrake/systems/controllers_py.cc b/bindings/pydrake/systems/controllers_py.cc index ca5482b5d02e..0a85e5a93d47 100644 --- a/bindings/pydrake/systems/controllers_py.cc +++ b/bindings/pydrake/systems/controllers_py.cc @@ -150,26 +150,33 @@ PYBIND11_MODULE(controllers, m) { { using Class = JointStiffnessController; constexpr auto& cls_doc = doc.JointStiffnessController; - py::class_>( - m, "JointStiffnessController", cls_doc.doc) - .def(py::init&, - const Eigen::Ref&, - const Eigen::Ref&>(), - 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_> cls( + m, "JointStiffnessController", cls_doc.doc); + cls.def(py::init&, + const Eigen::Ref&, + const Eigen::Ref&>(), + 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 } { diff --git a/bindings/pydrake/systems/test/controllers_test.py b/bindings/pydrake/systems/test/controllers_test.py index de96b1b28bbd..74423370dea3 100644 --- a/bindings/pydrake/systems/test/controllers_test.py +++ b/bindings/pydrake/systems/test/controllers_test.py @@ -4,7 +4,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 @@ -126,8 +126,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): diff --git a/systems/controllers/BUILD.bazel b/systems/controllers/BUILD.bazel index 63fe8207e939..4836ff2fcb73 100644 --- a/systems/controllers/BUILD.bazel +++ b/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 = [ diff --git a/systems/controllers/joint_stiffness_controller.cc b/systems/controllers/joint_stiffness_controller.cc index 50b7380d828b..886a065dd35c 100644 --- a/systems/controllers/joint_stiffness_controller.cc +++ b/systems/controllers/joint_stiffness_controller.cc @@ -35,6 +35,7 @@ JointStiffnessController::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::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::CalcOutputForce, + "actuation", num_q, + &JointStiffnessController::CalcOutputActuation, {this->all_input_ports_ticket()}) .get_index(); + auto& output_port_force = this->DeclareVectorOutputPort( + "generalized_force", num_q, + &JointStiffnessController::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::CalcMultibodyForces( plant_->CalcForceElementsContribution(plant_context, cache_value); } +// This method should be removed with the removal of the deprecated output port. template void JointStiffnessController::CalcOutputForce( const Context& context, BasicVector* output) const { @@ -157,6 +170,46 @@ void JointStiffnessController::CalcOutputForce( output->get_mutable_value() = tau; } +template +void JointStiffnessController::CalcOutputActuation( + const Context& context, BasicVector* output) const { + const int num_q = plant_->num_positions(); + + const auto& plant_context = + this->get_cache_entry(plant_context_cache_index_) + .template Eval>(context); + + // These include gravity. + const auto& applied_forces = + this->get_cache_entry(applied_forces_cache_index_) + .template Eval>(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 tau = plant_->CalcInverseDynamics( + plant_context, VectorX::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 Cv(num_q); + plant_->CalcBiasTerm(plant_context, &Cv); + tau -= Cv; + + // Add in the stiffness terms. + const VectorX& x = get_input_port_estimated_state().Eval(context); + const VectorX& 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 diff --git a/systems/controllers/joint_stiffness_controller.h b/systems/controllers/joint_stiffness_controller.h index dc10cbf54928..bbab1719de03 100644 --- a/systems/controllers/joint_stiffness_controller.h +++ b/systems/controllers/joint_stiffness_controller.h @@ -18,16 +18,16 @@ namespace controllers { /** * Implements a joint-space stiffness controller of the form * 
- *   τ_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)]
  *
* 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 @@ -122,10 +122,21 @@ class JointStiffnessController final : public LeafSystem { /** * 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& 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& get_output_port_actuation() const { + return this->get_output_port(output_port_index_actuation_); + } + /** * Returns a constant pointer to the MultibodyPlant used for control. */ @@ -143,9 +154,12 @@ class JointStiffnessController final : public LeafSystem { template friend class JointStiffnessController; + // Calculator for the deprecated output port. + void CalcOutputForce(const Context& context, BasicVector* force) const; + // This is the calculator method for the output port. - void CalcOutputForce(const Context& context, - BasicVector* force) const; + void CalcOutputActuation(const Context& context, + BasicVector* force) const; // Methods for updating cache entries. void SetMultibodyContext(const Context&, Context*) const; @@ -157,9 +171,11 @@ class JointStiffnessController final : public LeafSystem { 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 Binv_; drake::systems::CacheIndex applied_forces_cache_index_; drake::systems::CacheIndex plant_context_cache_index_; diff --git a/systems/controllers/test/joint_stiffness_controller_test.cc b/systems/controllers/test/joint_stiffness_controller_test.cc index b166964d61e2..b4745e9b2386 100644 --- a/systems/controllers/test/joint_stiffness_controller_test.cc +++ b/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"""( + + + + + + + + + + + + + + + + + +)"""; + DiagramBuilder builder; auto plant = builder.AddSystem(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) {