DiscreteUpdateManager computes contact kinematics (RobotLocomotion#20273

)

The computation of DiscreteContactPair data and ContactKinematics data
is moved from CompliantContactManager to the base class
DiscreteUpdateManager. This is in line with computation of
ContactResults and ContactSolverResults. This allows objects with only
a pointer to the base class to access these information.

multibody/plant/compliant_contact_manager.h

@@ -8,10 +8,8 @@
 #include "drake/common/drake_copyable.h"
 #include "drake/common/eigen_types.h"
 #include "drake/geometry/scene_graph_inspector.h"
-#include "drake/multibody/plant/contact_pair_kinematics.h"
 #include "drake/multibody/plant/contact_results.h"
 #include "drake/multibody/plant/deformable_driver.h"
-#include "drake/multibody/plant/discrete_contact_data.h"
 #include "drake/multibody/plant/discrete_update_manager.h"
 #include "drake/systems/framework/context.h"
 
@@ -70,9 +68,6 @@ struct AccelerationsDueNonConstraintForcesCache {
 // where p₀ is the object-centric virtual pressure field introduced by the
 // hydroelastic model.
 //
-// TODO(amcastro-tri): Retire code from MultibodyPlant as this contact manager
-// replaces all the contact related capabilities, per #16106.
-//
 // @warning Scalar support on T = symbolic::Expression is only limited,
 // conditional to the solver in use:
 //   - For TAMSI. Discrete updates are only supported when there is no contact
@@ -121,8 +116,6 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
   // Struct used to conglomerate the indexes of cache entries declared by the
   // manager.
   struct CacheIndexes {
-    systems::CacheIndex contact_kinematics;
-    systems::CacheIndex discrete_contact_pairs;
     systems::CacheIndex hydroelastic_contact_info;
     systems::CacheIndex non_constraint_forces_accelerations;
   };
@@ -135,33 +128,13 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
   // Provide private access for unit testing only.
   friend class CompliantContactManagerTester;
 
-  const MultibodyTreeTopology& tree_topology() const {
-    return internal::GetInternalTree(this->plant()).get_topology();
-  }
-
   std::unique_ptr<DiscreteUpdateManager<double>> CloneToDouble() const final;
   std::unique_ptr<DiscreteUpdateManager<AutoDiffXd>> CloneToAutoDiffXd()
       const final;
   std::unique_ptr<DiscreteUpdateManager<symbolic::Expression>> CloneToSymbolic()
       const final;
 
-  // Extracts non state dependent model information from MultibodyPlant. See
-  // DiscreteUpdateManager for details.
-  void ExtractModelInfo() final;
-
-  // Associates the given `DeformableModel` with `this` manager. The discrete
-  // states of the deformable bodies registered in the given `model` will be
-  // advanced by this manager. This manager holds onto the given pointer and
-  // therefore the model must outlive the manager.
-  // @throws std::exception if a deformable model has already been registered.
-  // @pre model != nullptr.
-  void ExtractConcreteModel(const DeformableModel<T>* model);
-
-  // For testing purposes only, we provide a default no-op implementation on
-  // arbitrary models of unknown concrete model type. Otherwise, for the closed
-  // list of models forward declared in physical_model.h, we must provide a
-  // function that extracts the particular variant of the physical model.
-  void ExtractConcreteModel(std::monostate) {}
+  void DoExtractModelInfo() final;
 
   void DoDeclareCacheEntries() final;
 
@@ -181,8 +154,6 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
   void DoCalcContactSolverResults(
       const systems::Context<T>&,
       contact_solvers::internal::ContactSolverResults<T>*) const final;
-  void DoCalcDiscreteValues(const systems::Context<T>&,
-                            systems::DiscreteValues<T>*) const final;
   void DoCalcAccelerationKinematicsCache(
       const systems::Context<T>&,
       multibody::internal::AccelerationKinematicsCache<T>*) const final;
@@ -192,51 +163,6 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
       const systems::Context<T>& context,
       MultibodyForces<T>* forces) const final;
 
-  // This method computes sparse kinematics information for each contact pair at
-  // the given configuration stored in `context`.
-  DiscreteContactData<ContactPairKinematics<T>> CalcContactKinematics(
-      const systems::Context<T>& context) const;
-
-  // Eval version of CalcContactKinematics().
-  const DiscreteContactData<ContactPairKinematics<T>>& EvalContactKinematics(
-      const systems::Context<T>& context) const;
-
-  // Helper function for CalcContactKinematics() that computes the contact pair
-  // kinematics for point contact and hydroelastic contact respectively,
-  // depending on the value of `type`.
-  void AppendContactKinematics(
-      const systems::Context<T>& context,
-      const std::vector<DiscreteContactPair<T>>& contact_pairs,
-      DiscreteContactType type,
-      DiscreteContactData<ContactPairKinematics<T>>* contact_kinematics) const;
-
-  // Given the configuration stored in `context`, this method appends discrete
-  // pairs corresponding to point contact into `pairs`.
-  // @pre pairs != nullptr.
-  void AppendDiscreteContactPairsForPointContact(
-      const systems::Context<T>& context,
-      DiscreteContactData<DiscreteContactPair<T>>* pairs) const;
-
-  // Given the configuration stored in `context`, this method appends discrete
-  // pairs corresponding to hydroelastic contact into `pairs`.
-  // @pre pairs != nullptr.
-  void AppendDiscreteContactPairsForHydroelasticContact(
-      const systems::Context<T>& context,
-      DiscreteContactData<DiscreteContactPair<T>>* pairs) const;
-
-  // Given the configuration stored in `context`, this method computes all
-  // discrete contact pairs, including point, hydroelastic, and deformable
-  // contact, into `pairs`. Contact pairs including deformable bodies are
-  // guaranteed to come after point and hydroelastic contact pairs. Throws an
-  // exception if `pairs` is nullptr.
-  void CalcDiscreteContactPairs(
-      const systems::Context<T>& context,
-      DiscreteContactData<DiscreteContactPair<T>>* pairs) const;
-
-  // Eval version of CalcDiscreteContactPairs().
-  const DiscreteContactData<DiscreteContactPair<T>>& EvalDiscreteContactPairs(
-      const systems::Context<T>& context) const;
-
   // Computes per-face contact information for the hydroelastic model (slip
   // velocity, traction, etc). On return contact_info->size() will equal the
   // number of faces discretizing the contact surface.
@@ -278,29 +204,13 @@ class CompliantContactManager final : public DiscreteUpdateManager<T> {
   // This information is extracted during the call to ExtractModelInfo().
   VectorX<T> joint_damping_;
 
-  // deformable_driver_ computes the information on all deformable bodies needed
-  // to advance the discrete states.
-  std::unique_ptr<DeformableDriver<double>> deformable_driver_;
-
   // Specific contact solver drivers are created at ExtractModelInfo() time,
   // when the manager retrieves modeling information from MultibodyPlant.
   // Only one of these drivers will be non-nullptr.
   std::unique_ptr<SapDriver<T>> sap_driver_;
   std::unique_ptr<TamsiDriver<T>> tamsi_driver_;
 };
 
-// N.B. These geometry queries are not supported when T = symbolic::Expression
-// and therefore their implementation throws.
-template <>
-void CompliantContactManager<symbolic::Expression>::CalcDiscreteContactPairs(
-    const drake::systems::Context<symbolic::Expression>&,
-    DiscreteContactData<DiscreteContactPair<symbolic::Expression>>*) const;
-template <>
-void CompliantContactManager<symbolic::Expression>::
-    AppendDiscreteContactPairsForHydroelasticContact(
-        const drake::systems::Context<symbolic::Expression>&,
-        DiscreteContactData<DiscreteContactPair<symbolic::Expression>>*) const;
-
 }  // namespace internal
 }  // namespace multibody
 }  // namespace drake

multibody/plant/deformable_driver.cc

@@ -16,6 +16,7 @@
 #include "drake/multibody/fem/fem_model.h"
 #include "drake/multibody/fem/velocity_newmark_scheme.h"
 #include "drake/multibody/plant/contact_properties.h"
+#include "drake/multibody/plant/multibody_plant.h"
 #include "drake/systems/framework/context.h"
 
 using drake::geometry::GeometryId;

multibody/plant/deformable_driver.h

@@ -15,7 +15,7 @@
 #include "drake/multibody/plant/contact_pair_kinematics.h"
 #include "drake/multibody/plant/deformable_model.h"
 #include "drake/multibody/plant/discrete_contact_data.h"
-#include "drake/multibody/plant/discrete_update_manager.h"
+#include "drake/multibody/plant/discrete_contact_pair.h"
 #include "drake/systems/framework/context.h"
 
 namespace drake {
@@ -69,6 +69,9 @@ class Multiplexer {
   int num_entries_{0};
 };
 
+template <typename T>
+class DiscreteUpdateManager;
+
 /* DeformableDriver is responsible for computing dynamics information about
  all deformable bodies. It works in tandem with a DeformableModel and a
  DiscreteUpdateManager that are provided at construction time. The deformable

multibody/plant/deformable_model.cc

@@ -11,6 +11,7 @@
 #include "drake/multibody/fem/linear_simplex_element.h"
 #include "drake/multibody/fem/simplex_gaussian_quadrature.h"
 #include "drake/multibody/fem/volumetric_model.h"
+#include "drake/multibody/plant/multibody_plant.h"
 
 namespace drake {
 namespace multibody {
@@ -25,6 +26,12 @@ using geometry::SourceId;
 using fem::DeformableBodyConfig;
 using fem::MaterialModel;
 
+template <typename T>
+DeformableModel<T>::DeformableModel(MultibodyPlant<T>* plant) : plant_(plant) {
+  DRAKE_DEMAND(plant_ != nullptr);
+  DRAKE_DEMAND(!plant_->is_finalized());
+}
+
 template <typename T>
 DeformableBodyId DeformableModel<T>::RegisterDeformableBody(
     std::unique_ptr<geometry::GeometryInstance> geometry_instance,

multibody/plant/deformable_model.h

@@ -12,12 +12,15 @@
 #include "drake/multibody/fem/fem_model.h"
 #include "drake/multibody/plant/constraint_specs.h"
 #include "drake/multibody/plant/deformable_ids.h"
-#include "drake/multibody/plant/multibody_plant.h"
 #include "drake/multibody/plant/physical_model.h"
+#include "drake/multibody/tree/body.h"
 
 namespace drake {
 namespace multibody {
 
+template <typename T>
+class MultibodyPlant;
+
 /** DeformableModel implements the interface in PhysicalModel and provides the
  functionalities to specify deformable bodies. Unlike rigid bodies, the shape of
  deformable bodies can change in a simulation. Each deformable body is modeled
@@ -34,10 +37,7 @@ class DeformableModel final : public multibody::PhysicalModel<T> {
   /** Constructs a DeformableModel to be owned by the given MultibodyPlant.
    @pre plant != nullptr.
    @pre Finalize() has not been called on `plant`. */
-  explicit DeformableModel(MultibodyPlant<T>* plant) : plant_(plant) {
-    DRAKE_DEMAND(plant_ != nullptr);
-    DRAKE_DEMAND(!plant_->is_finalized());
-  }
+  explicit DeformableModel(MultibodyPlant<T>* plant);
 
   /** Returns the number of deformable bodies registered with this
    DeformableModel. */