Implements actuation output port for MultibodyPlant (RobotLocomotion#…

…20421)

Implements output port to report the total actuation values applied
through joint actuators, including PD controllers.

bindings/pydrake/multibody/plant_py.cc

@@ -916,6 +916,9 @@ void DoScalarDependentDefinitions(py::module m, T) {
                 ModelInstanceIndex>(&Class::get_actuation_input_port),
             py::arg("model_instance"), py_rvp::reference_internal,
             cls_doc.get_actuation_input_port.doc_1args)
+        .def("get_net_actuation_output_port",
+            &Class::get_net_actuation_output_port, py_rvp::reference_internal,
+            cls_doc.get_net_actuation_output_port.doc)
         .def("get_desired_state_input_port",
             overload_cast_explicit<const systems::InputPort<T>&,
                 multibody::ModelInstanceIndex>(

bindings/pydrake/multibody/test/plant_test.py

@@ -432,6 +432,8 @@ def check_repr(element, expected):
                 [shoulder.index()]), [link1.index(), link2.index()])
         self.assertIsInstance(
             plant.get_actuation_input_port(), InputPort)
+        self.assertIsInstance(
+            plant.get_net_actuation_output_port(), OutputPort)
         self.assertIsInstance(
             plant.get_state_output_port(), OutputPort)
         self.assertIsInstance(

multibody/contact_solvers/sap/sap_contact_problem.cc

@@ -244,6 +244,39 @@ int SapContactProblem<T>::AddConstraint(std::unique_ptr<SapConstraint<T>> c) {
   return constraint_index;
 }
 
+template <typename T>
+void SapContactProblem<T>::CalcConstraintGeneralizedForces(
+    const VectorX<T>& gamma, int constraint_start, int constraint_end,
+    VectorX<T>* generalized_forces) const {
+  DRAKE_THROW_UNLESS(0 <= constraint_start &&
+                     constraint_start < num_constraints());
+  DRAKE_THROW_UNLESS(0 <= constraint_end && constraint_end < num_constraints());
+  DRAKE_THROW_UNLESS(constraint_start <= constraint_end);
+  DRAKE_THROW_UNLESS(gamma.size() == num_constraint_equations());
+  DRAKE_THROW_UNLESS(generalized_forces != nullptr);
+  DRAKE_THROW_UNLESS(generalized_forces->size() == num_velocities());
+
+  generalized_forces->setZero();
+  for (int i = constraint_start; i <= constraint_end; ++i) {
+    const SapConstraint<T>& constraint = get_constraint(i);
+    const int equation_start = constraint_equations_start(i);
+    const int ne = constraint.num_constraint_equations();
+    const auto constraint_gamma = gamma.segment(equation_start, ne);
+
+    // Compute generalized forces per clique.
+    for (int c = 0; c < constraint.num_cliques(); ++c) {
+      const int clique = constraint.clique(c);
+      auto clique_forces = generalized_forces->segment(velocities_start(clique),
+                                                       num_velocities(clique));
+      constraint.AccumulateGeneralizedImpulses(c, constraint_gamma,
+                                               &clique_forces);
+    }
+  }
+
+  // Conversion of impulses into forces.
+  (*generalized_forces) /= time_step();
+}
+
 template <typename T>
 void SapContactProblem<T>::CalcConstraintMultibodyForces(
     const VectorX<T>& gamma, VectorX<T>* generalized_forces,

multibody/contact_solvers/sap/sap_contact_problem.h

@@ -271,6 +271,26 @@ class SapContactProblem {
       const VectorX<T>& gamma, VectorX<T>* generalized_forces,
       std::vector<SpatialForce<T>>* spatial_forces) const;
 
+  /* Computes the generalized forces given a known vector of impulses `gamma`,
+   for constraints with index i in the inclusive range constraint_start <= i &&
+   i <= constraint_end.
+
+   @param[in] gamma Constraint impulses for this full problem. Of size
+   num_constraint_equations().
+   @param[out] generalized_forces On output, the set of generalized forces
+   result of the combined action of all constraints in `this` problem given the
+   known impulses `gamma`.
+
+   @throws if gamma.size() != num_constraint_equations().
+   @throws if constraint_start is not in [0, num_constraints()).
+   @throws if constraint_end is not in [0, num_constraints()).
+   @throws if constraint_end < constraint_start.
+   @throws if generalized_forces is nullptr.
+   @throws if generalized_forces.size() != num_velocities(). */
+  void CalcConstraintGeneralizedForces(const VectorX<T>& gamma,
+                                       int constraint_start, int constraint_end,
+                                       VectorX<T>* generalized_forces) const;
+
  private:
   int nv_{0};           // Total number of generalized velocities.
   T time_step_{0.0};    // Discrete time step.

multibody/contact_solvers/sap/test/sap_contact_problem_test.cc

@@ -583,6 +583,11 @@ GTEST_TEST(ContactProblem, CalcConstraintMultibodyForces) {
   //  tau_c = time_step * E * gamma_c.sum()
   // where E is the vector of all ones.
   VectorXd tau_expected = VectorXd::Zero(problem.num_velocities());
+  // tau_partial_expected only accumulates generalized forces for constraints
+  // with indexes in start <= i <= end.
+  const int start = 1;
+  const int end = 2;
+  VectorXd tau_partial_expected = VectorXd::Zero(problem.num_velocities());
   int offset = 0;
   for (int i = 0; i < problem.num_constraints(); ++i) {
     const auto& constraint = problem.get_constraint(i);
@@ -594,15 +599,28 @@ GTEST_TEST(ContactProblem, CalcConstraintMultibodyForces) {
           .segment(problem.velocities_start(clique),
                    problem.num_velocities(clique))
           .array() += gamma_c.sum();
+      if (start <= i && i <= end) {
+        tau_partial_expected
+            .segment(problem.velocities_start(clique),
+                     problem.num_velocities(clique))
+            .array() += gamma_c.sum();
+      }
     }
     offset += ne;
   }
   tau_expected /= problem.time_step();
+  tau_partial_expected /= problem.time_step();
 
   EXPECT_TRUE(CompareMatrices(tau, tau_expected,
                               std::numeric_limits<double>::epsilon(),
                               MatrixCompareType::relative));
 
+  // Verify computation of generalized forces for only a set of the constraints.
+  problem.CalcConstraintGeneralizedForces(gamma, start, end, &tau);
+  EXPECT_TRUE(CompareMatrices(tau, tau_partial_expected,
+                              std::numeric_limits<double>::epsilon(),
+                              MatrixCompareType::relative));
+
   // TestConstraint applies spatial force:
   // F = (time_step + local_object) * gamma.sum() * Vector6d::Ones().
   std::vector<SpatialForce<double>> spatial_forces_expected(

multibody/plant/compliant_contact_manager.cc

@@ -334,6 +334,32 @@ void CompliantContactManager<T>::DoCalcDiscreteUpdateMultibodyForces(
   }
 }
 
+template <typename T>
+void CompliantContactManager<T>::DoCalcActuation(
+    const systems::Context<T>& context, VectorX<T>* actuation) const {
+  // Thus far only TAMSI and SAP are supported. Verify this is true.
+  DRAKE_DEMAND(
+      plant().get_discrete_contact_solver() == DiscreteContactSolver::kSap ||
+      plant().get_discrete_contact_solver() == DiscreteContactSolver::kTamsi);
+
+  if (plant().get_discrete_contact_solver() == DiscreteContactSolver::kSap) {
+    if constexpr (std::is_same_v<T, symbolic::Expression>) {
+      throw std::logic_error(
+          "Discrete updates with the SAP solver are not supported for T = "
+          "symbolic::Expression");
+    } else {
+      DRAKE_DEMAND(sap_driver_ != nullptr);
+      sap_driver_->CalcActuation(context, actuation);
+    }
+  }
+
+  if (plant().get_discrete_contact_solver() == DiscreteContactSolver::kTamsi) {
+    DRAKE_DEMAND(tamsi_driver_ != nullptr);
+    // TAMSI does not model additional actuation terms as SAP does.
+    *actuation = this->AssembleActuationInput(context);
+  }
+}
+
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake

multibody/plant/compliant_contact_manager.h

@@ -158,6 +158,8 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
   void DoCalcDiscreteUpdateMultibodyForces(
       const systems::Context<T>& context,
       MultibodyForces<T>* forces) const final;
+  void DoCalcActuation(const systems::Context<T>& context,
+                       VectorX<T>* forces) const final;
 
   // Computes non-constraint forces and the accelerations they induce.
   void CalcAccelerationsDueToNonConstraintForcesCache(

multibody/plant/discrete_update_manager.cc

@@ -108,6 +108,14 @@ void DiscreteUpdateManager<T>::DeclareCacheEntries() {
   cache_indexes_.discrete_update_multibody_forces =
       multibody_forces_cache_entry.cache_index();
 
+  const auto& actuation_cache_entry = DeclareCacheEntry(
+      "Discrete update actuation",
+      systems::ValueProducer(this, VectorX<T>(plant().num_actuated_dofs()),
+                             &DiscreteUpdateManager<T>::CalcActuation),
+      {systems::System<T>::xd_ticket(),
+       systems::System<T>::all_parameters_ticket()});
+  cache_indexes_.actuation = actuation_cache_entry.cache_index();
+
   const auto& contact_results_cache_entry = DeclareCacheEntry(
       "Contact results (discrete)",
       systems::ValueProducer(this,
@@ -740,6 +748,15 @@ void DiscreteUpdateManager<T>::CalcDiscreteUpdateMultibodyForces(
   DoCalcDiscreteUpdateMultibodyForces(context, forces);
 }
 
+template <typename T>
+void DiscreteUpdateManager<T>::CalcActuation(const systems::Context<T>& context,
+                                             VectorX<T>* actuation) const {
+  plant().ValidateContext(context);
+  DRAKE_DEMAND(actuation != nullptr);
+  DRAKE_DEMAND(actuation->size() == plant().num_actuated_dofs());
+  DoCalcActuation(context, actuation);
+}
+
 template <typename T>
 void DiscreteUpdateManager<T>::CalcContactKinematics(
     const systems::Context<T>& context,
@@ -1227,6 +1244,14 @@ DiscreteUpdateManager<T>::EvalDiscreteUpdateMultibodyForces(
       .template Eval<MultibodyForces<T>>(context);
 }
 
+template <typename T>
+const VectorX<T>& DiscreteUpdateManager<T>::EvalActuation(
+    const systems::Context<T>& context) const {
+  return plant()
+      .get_cache_entry(cache_indexes_.actuation)
+      .template Eval<VectorX<T>>(context);
+}
+
 template <typename T>
 const ContactResults<T>& DiscreteUpdateManager<T>::EvalContactResults(
     const systems::Context<T>& context) const {

multibody/plant/discrete_update_manager.h

@@ -194,6 +194,11 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent<T> {
   const MultibodyForces<T>& EvalDiscreteUpdateMultibodyForces(
       const systems::Context<T>& context) const;
 
+  /* Evaluate the actuation applied through actuators during the discrete
+   update. This will include actuation input as well as controller models.
+   The returned vector is indexed by JointActuatorIndex. */
+  const VectorX<T>& EvalActuation(const systems::Context<T>& context) const;
+
   /* Evaluates sparse kinematics information for each contact pair at
    the given configuration stored in `context`. */
   const DiscreteContactData<ContactPairKinematics<T>>& EvalContactKinematics(
@@ -354,6 +359,13 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent<T> {
   virtual void DoCalcDiscreteUpdateMultibodyForces(
       const systems::Context<T>& context, MultibodyForces<T>* forces) const = 0;
 
+  /* Concrete managers must implement this method to compute the total actuation
+   applied during a discrete update.
+   For instance, managers that implement implicit controllers must override this
+   method to include these terms. */
+  virtual void DoCalcActuation(const systems::Context<T>& context,
+                               VectorX<T>* actuation) const = 0;
+
   /* Performs discrete updates for rigid DoFs in the system. Defaults to
    symplectic Euler updates. Derived classes may choose to override the
    implemention to provide a different update scheme. */
@@ -399,6 +411,7 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent<T> {
     systems::CacheIndex contact_kinematics;
     systems::CacheIndex discrete_contact_pairs;
     systems::CacheIndex hydroelastic_contact_info;
+    systems::CacheIndex actuation;
   };
 
   /* Exposes indices for the cache entries declared by this class for derived
@@ -442,6 +455,10 @@ class DiscreteUpdateManager : public ScalarConvertibleComponent<T> {
   void CalcDiscreteUpdateMultibodyForces(const systems::Context<T>& context,
                                          MultibodyForces<T>* forces) const;
 
+  /* Calc version of EvalActuation, NVI to DoCalcActuation. */
+  void CalcActuation(const systems::Context<T>& context,
+                     VectorX<T>* forces) const;
+
   /* Calc version of EvalContactKinematics(). */
   void CalcContactKinematics(
       const systems::Context<T>& context,

multibody/plant/multibody_plant.cc

@@ -369,6 +369,7 @@ MultibodyPlant<T>::MultibodyPlant(const MultibodyPlant<U>& other)
     // in FinalizePlantOnly():
     //   -instance_actuation_ports_
     //   -actuation_port_
+    //   -net_actuation_port_
     //   -applied_generalized_force_input_port_
     //   -applied_spatial_force_input_port_
     //   -body_poses_port_
@@ -2348,6 +2349,18 @@ VectorX<T> MultibodyPlant<T>::AssembleActuationInput(
   return actuation_input;
 }
 
+template <typename T>
+void MultibodyPlant<T>::CalcActuationOutput(const systems::Context<T>& context,
+                                            BasicVector<T>* actuation) const {
+  DRAKE_DEMAND(actuation != nullptr);
+  DRAKE_DEMAND(actuation->size() == num_actuated_dofs());
+  if (is_discrete()) {
+    actuation->SetFromVector(discrete_update_manager_->EvalActuation(context));
+  } else {
+    actuation->SetFromVector(AssembleActuationInput(context));
+  }
+}
+
 template <typename T>
 VectorX<T> MultibodyPlant<T>::AssembleDesiredStateInput(
     const systems::Context<T>& context) const {
@@ -2738,6 +2751,10 @@ void MultibodyPlant<T>::DeclareStateCacheAndPorts() {
   DeclareCacheEntries();
   DeclareParameters();
 
+  // State ticket.
+  const systems::DependencyTicket state_ticket =
+      is_discrete() ? this->xd_ticket() : this->kinematics_ticket();
+
   // Declare per model instance actuation ports.
   instance_actuation_ports_.resize(num_model_instances());
   for (ModelInstanceIndex model_instance_index(0);
@@ -2753,6 +2770,17 @@ void MultibodyPlant<T>::DeclareStateCacheAndPorts() {
       this->DeclareVectorInputPort("actuation", num_actuated_dofs())
           .get_index();
 
+  // Total applied actuation. For discrete models it can contain constraint
+  // terms.
+  // N.B. We intentionally declare a dependency on kinematics in the continuous
+  // mode in anticipation for adding PD support in continuous mode.
+  net_actuation_port_ = this->DeclareVectorOutputPort(
+                                "net_actuation", num_actuated_dofs(),
+                                &MultibodyPlant::CalcActuationOutput,
+                                {state_ticket, this->all_input_ports_ticket(),
+                                 this->all_parameters_ticket()})
+                            .get_index();
+
   // Declare per model instance desired states input ports.
   instance_desired_state_ports_.resize(num_model_instances());
   for (ModelInstanceIndex model_instance_index(0);
@@ -2940,8 +2968,6 @@ void MultibodyPlant<T>::DeclareStateCacheAndPorts() {
           .get_index();
 
   // Contact results output port.
-  const systems::DependencyTicket state_ticket =
-      is_discrete() ? this->xd_ticket() : this->kinematics_ticket();
   std::set<systems::DependencyTicket> contact_results_prerequisites = {
       state_ticket};
   if (is_discrete()) {
@@ -3145,6 +3171,13 @@ const systems::InputPort<T>& MultibodyPlant<T>::get_actuation_input_port()
   return systems::System<T>::get_input_port(actuation_port_);
 }
 
+template <typename T>
+const systems::OutputPort<T>& MultibodyPlant<T>::get_net_actuation_output_port()
+    const {
+  DRAKE_MBP_THROW_IF_NOT_FINALIZED();
+  return systems::System<T>::get_output_port(net_actuation_port_);
+}
+
 template <typename T>
 const systems::InputPort<T>& MultibodyPlant<T>::get_desired_state_input_port(
     ModelInstanceIndex model_instance) const {