Hooked up hydroelastic contact results to MBP. (#12109)

examples/multibody/rolling_sphere/BUILD.bazel

@@ -1,6 +1,8 @@
 # -*- python -*-
 # This file contains rules for Bazel; see drake/doc/bazel.rst.
 
+package(default_visibility = ["//visibility:public"])
+
 load(
     "@drake//tools/skylark:drake_cc.bzl",
     "drake_cc_binary",
@@ -38,6 +40,7 @@ drake_cc_binary(
         ":make_rolling_sphere_plant",
         "//common:text_logging_gflags",
         "//geometry:geometry_visualization",
+        "//multibody/plant:contact_results_to_lcm",
         "//systems/analysis:implicit_euler_integrator",
         "//systems/analysis:runge_kutta2_integrator",
         "//systems/analysis:runge_kutta3_integrator",

examples/multibody/rolling_sphere/rolling_sphere_run_dynamics.cc

@@ -10,6 +10,7 @@
 #include "drake/lcm/drake_lcm.h"
 #include "drake/math/random_rotation.h"
 #include "drake/multibody/math/spatial_velocity.h"
+#include "drake/multibody/plant/contact_results_to_lcm.h"
 #include "drake/systems/analysis/implicit_euler_integrator.h"
 #include "drake/systems/analysis/runge_kutta2_integrator.h"
 #include "drake/systems/analysis/runge_kutta3_integrator.h"
@@ -134,12 +135,12 @@ int do_main() {
       scene_graph.get_source_pose_port(plant.get_source_id().value()));
 
   geometry::ConnectDrakeVisualizer(&builder, scene_graph);
+  ConnectContactResultsToDrakeVisualizer(&builder, plant);
   auto diagram = builder.Build();
 
   // Create a context for this system:
   std::unique_ptr<systems::Context<double>> diagram_context =
       diagram->CreateDefaultContext();
-  diagram->SetDefaultContext(diagram_context.get());
   systems::Context<double>& plant_context =
       diagram->GetMutableSubsystemContext(plant, diagram_context.get());
 

geometry/query_results/contact_surface.h

@@ -228,6 +228,14 @@ class ContactSurface {
     return *mesh_W_;
   }
 
+  /** Returns a reference to the scalar field eₘₙ. */
+  const MeshField<T, SurfaceMesh<T>>& e_MN() const { return *e_MN_; }
+
+  /** Returns a reference to the vector field ∇hₘₙ. */
+  const MeshField<Vector3<T>, SurfaceMesh<T>>& grad_h_MN_W() const {
+    return *grad_h_MN_W_;
+  }
+
  private:
   // Swaps M and N (modifying the data in place to reflect the change).
   void SwapMAndN() {

multibody/plant/BUILD.bazel

@@ -512,4 +512,12 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "multibody_plant_hydroelastic_contact_results_output_test",
+    deps = [
+        ":plant",
+        "//examples/multibody/rolling_sphere:make_rolling_sphere_plant",
+    ],
+)
+
 add_lint_tests()

multibody/plant/contact_model_doxygen.h

@@ -204,7 +204,7 @@ Next topic: @ref contact_engineering
 
  @note When modeling the multibody system as discrete (refer to
  the @ref time_advancement_strategy "Choice of Time Advancement Strategy"
- section), only the static coefficient of friction is used while the kinetic
+ section), only the dynamic coefficient of friction is used while the static
  coefficient of friction is ignored.
 
  @anchor time_advancement_strategy
@@ -392,7 +392,7 @@ Next topic: @ref contact_engineering
  Rather than modeling _perfect_ stiction, it makes use of an _allowable_ amount
  of relative motion to approximate stiction.  When we refer to
  "relative motion", we refer specifically to the relative translational speed of
- two points `Ac` and `Bc` defined to instantly be located at contact point 
+ two points `Ac` and `Bc` defined to instantly be located at contact point
  `Co` and moving with bodies A and B, respectively.
 
  The function, as illustrated in Figure 3, is a function of the unitless

multibody/plant/hydroelastic_contact_info.h

@@ -118,7 +118,8 @@ class HydroelasticContactInfo {
   /// world frame. At each point Q on the contact surface, `traction_A_W` gives
   /// the traction `traction_Aq_W`, where `Aq` is a frame attached to Body A and
   /// shifted to Q.
-  const geometry::SurfaceMeshField<Vector3<T>, T>& traction_A_W() const {
+  const geometry::MeshField<Vector3<T>, geometry::SurfaceMesh<T>>&
+  traction_A_W() const {
     return *traction_A_W_;
   }
 
@@ -130,16 +131,19 @@ class HydroelasticContactInfo {
   /// attached to Frame B). The tangential velocity at Q corresponds to the
   /// components of velocity orthogonal to the normal to the contact surface at
   /// Q.
-  const geometry::SurfaceMeshField<Vector3<T>, T>& vslip_AB_W() const {
+  const geometry::MeshField<Vector3<T>, geometry::SurfaceMesh<T>>& vslip_AB_W()
+      const {
     return *vslip_AB_W_;
   }
 
  private:
   // Note that the mesh of the contact surface is defined in the world frame.
   drake::variant<const geometry::ContactSurface<T>*,
                  std::unique_ptr<geometry::ContactSurface<T>>> contact_surface_;
-  std::unique_ptr<geometry::SurfaceMeshField<Vector3<T>, T>> traction_A_W_;
-  std::unique_ptr<geometry::SurfaceMeshField<Vector3<T>, T>> vslip_AB_W_;
+  std::unique_ptr<geometry::MeshField<Vector3<T>, geometry::SurfaceMesh<T>>>
+      traction_A_W_;
+  std::unique_ptr<geometry::MeshField<Vector3<T>, geometry::SurfaceMesh<T>>>
+      vslip_AB_W_;
 };
 
 // Workaround for https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57728 which

multibody/plant/multibody_plant.cc

@@ -1048,19 +1048,90 @@ void MultibodyPlant<T>::CalcContactResultsContinuous(
     const systems::Context<T>& context,
     ContactResults<T>* contact_results) const {
   DRAKE_DEMAND(contact_results != nullptr);
+  contact_results->Clear();
   if (num_collision_geometries() == 0) return;
 
-  // Thus far we do not allow mixing hydroelastics with point contact and
-  // therefore we throw an exception.
-  // TODO(amcastro-tri): Update the computation of contact results to report
-  // both point and hydroelastic results.
-  if (contact_model_  != ContactModel::kPointContactOnly) {
-    throw std::runtime_error(
-        "Currently we do not support mixing point contact with hydroelastics. "
-        "You requested the hydroelastic model with a call to "
-        "set_contact_model(). "
-        "Currently contact results are only supported for point contact.");
+  switch (contact_model_) {
+    case ContactModel::kPointContactOnly:
+      CalcContactResultsContinuousPointPair(context, contact_results);
+      break;
+
+    case ContactModel::kHydroelasticsOnly:
+      CalcContactResultsContinuousHydroelastic(context, contact_results);
+      break;
+  }
+}
+
+template <>
+void MultibodyPlant<symbolic::Expression>::
+    CalcContactResultsContinuousHydroelastic(
+        const Context<symbolic::Expression>&,
+        ContactResults<symbolic::Expression>*) const {
+  throw std::logic_error(
+      "This method doesn't support T = symbolic::Expression.");
+}
+
+// TODO(edrumwri) Convert this function to use a cached HydroelasticTractionInfo
+// once that structure can serve to both compute contact wrenches and report
+// contact details.
+template <typename T>
+void MultibodyPlant<T>::CalcContactResultsContinuousHydroelastic(
+    const systems::Context<T>& context,
+    ContactResults<T>* contact_results) const {
+  const std::vector<ContactSurface<T>>& all_surfaces =
+      EvalContactSurfaces(context);
+
+  internal::HydroelasticTractionCalculator<T> traction_calculator(
+      friction_model_.stiction_tolerance());
+
+  for (const ContactSurface<T>& surface : all_surfaces) {
+    const GeometryId geometryM_id = surface.id_M();
+    const GeometryId geometryN_id = surface.id_N();
+    const int collision_indexM =
+        geometry_id_to_collision_index_.at(geometryM_id);
+    const int collision_indexN =
+        geometry_id_to_collision_index_.at(geometryN_id);
+    const CoulombFriction<double>& geometryM_friction =
+        default_coulomb_friction_[collision_indexM];
+    const CoulombFriction<double>& geometryN_friction =
+        default_coulomb_friction_[collision_indexN];
+
+    // Compute combined friction coefficient.
+    const CoulombFriction<double> combined_friction =
+        CalcContactFrictionFromSurfaceProperties(geometryM_friction,
+                                                 geometryN_friction);
+    const double dynamic_friction = combined_friction.dynamic_friction();
+
+    // Get the bodies that the two geometries are affixed to. We'll call these
+    // A and B.
+    const BodyIndex bodyA_index = geometry_id_to_body_index_.at(geometryM_id);
+    const BodyIndex bodyB_index = geometry_id_to_body_index_.at(geometryN_id);
+    const Body<T>& bodyA = internal_tree().get_body(bodyA_index);
+    const Body<T>& bodyB = internal_tree().get_body(bodyB_index);
+
+    // The poses and spatial velocities of bodies A and B.
+    const RigidTransform<T>& X_WA = bodyA.EvalPoseInWorld(context);
+    const RigidTransform<T>& X_WB = bodyB.EvalPoseInWorld(context);
+    const SpatialVelocity<T>& V_WA = bodyA.EvalSpatialVelocityInWorld(context);
+    const SpatialVelocity<T>& V_WB = bodyB.EvalSpatialVelocityInWorld(context);
+
+    // Pack everything the calculator needs.
+    typename internal::HydroelasticTractionCalculator<T>::Data data(
+        X_WA, X_WB, V_WA, V_WB, &surface);
+
+    // Combined Hunt & Crossley dissipation.
+    const double dissipation = hydroelastics_engine_.CalcCombinedDissipation(
+        geometryM_id, geometryN_id);
+
+    contact_results->AddContactInfo(traction_calculator.ComputeContactInfo(
+       data, dissipation, dynamic_friction));
   }
+}
+
+template <typename T>
+void MultibodyPlant<T>::CalcContactResultsContinuousPointPair(
+    const systems::Context<T>& context,
+    ContactResults<T>* contact_results) const {
 
   const std::vector<PenetrationAsPointPair<T>>& point_pairs =
       EvalPointPairPenetrations(context);
@@ -1303,12 +1374,8 @@ void MultibodyPlant<T>::CalcHydroelasticContactForces(
   F_BBo_W_array->assign(num_bodies(), SpatialForce<T>::Zero());
   if (num_collision_geometries() == 0) return;
 
-  const auto& query_object =
-      this->get_geometry_query_input_port()
-          .template Eval<geometry::QueryObject<T>>(context);
-
-  const std::vector<ContactSurface<T>> all_surfaces =
-      hydroelastics_engine_.ComputeContactSurfaces(query_object);
+  const std::vector<ContactSurface<T>>& all_surfaces =
+      EvalContactSurfaces(context);
 
   internal::HydroelasticTractionCalculator<T> traction_calculator(
       friction_model_.stiction_tolerance());
@@ -1329,7 +1396,7 @@ void MultibodyPlant<T>::CalcHydroelasticContactForces(
     const CoulombFriction<double> combined_friction =
         CalcContactFrictionFromSurfaceProperties(geometryM_friction,
                                                  geometryN_friction);
-    const double static_friction = combined_friction.static_friction();
+    const double dynamic_friction = combined_friction.dynamic_friction();
 
     // Get the bodies that the two geometries are affixed to. We'll call these
     // A and B.
@@ -1355,7 +1422,7 @@ void MultibodyPlant<T>::CalcHydroelasticContactForces(
     // Integrate the hydroelastic traction field over the contact surface.
     SpatialForce<T> F_Ao_W, F_Bo_W;
     traction_calculator.ComputeSpatialForcesAtBodyOriginsFromHydroelasticModel(
-        data, dissipation, static_friction, &F_Ao_W, &F_Bo_W);
+        data, dissipation, dynamic_friction, &F_Ao_W, &F_Bo_W);
 
     if (bodyA_index != world_index()) {
       F_BBo_W_array->at(bodyA.node_index()) += F_Ao_W;
@@ -1568,6 +1635,20 @@ TamsiSolverResult MultibodyPlant<T>::SolveUsingSubStepping(
   return info;
 }
 
+template <typename T>
+void MultibodyPlant<T>::CalcContactSurfaces(
+    const drake::systems::Context<T>& context,
+    std::vector<ContactSurface<T>>* contact_surfaces) const {
+  DRAKE_DEMAND(contact_surfaces);
+
+  const auto& query_object =
+      this->get_geometry_query_input_port()
+          .template Eval<geometry::QueryObject<T>>(context);
+
+  *contact_surfaces =
+      hydroelastics_engine_.ComputeContactSurfaces(query_object);
+}
+
 template <typename T>
 void MultibodyPlant<T>::CalcAppliedForces(
     const drake::systems::Context<T>& context,
@@ -1654,15 +1735,15 @@ void MultibodyPlant<T>::CalcTamsiResults(
   const internal::ContactJacobians<T>& contact_jacobians =
       EvalContactJacobians(context0);
 
-  // Get friction coefficient into a single vector. Dynamic friction is ignored
+  // Get friction coefficient into a single vector. Static friction is ignored
   // by the time stepping scheme.
   std::vector<CoulombFriction<double>> combined_friction_pairs =
       CalcCombinedFrictionCoefficients(point_pairs0);
   VectorX<T> mu(num_contacts);
   std::transform(combined_friction_pairs.begin(), combined_friction_pairs.end(),
                  mu.data(),
                  [](const CoulombFriction<double>& coulomb_friction) {
-                   return coulomb_friction.static_friction();
+                   return coulomb_friction.dynamic_friction();
                  });
 
   // Place all the penetration depths within a single vector as required by
@@ -2022,6 +2103,23 @@ void MultibodyPlant<T>::DeclareCacheEntries() {
       {this->configuration_ticket()});
   cache_indexes_.point_pairs = point_pairs_cache_entry.cache_index();
 
+  // Cache entry for hydroelastic contact surfaces.
+  auto& contact_surfaces_cache_entry = this->DeclareCacheEntry(
+      std::string("Hydroelastic contact surfaces."),
+      []() {
+        return AbstractValue::Make(
+            std::vector<ContactSurface<T>>());
+      },
+      [this](const systems::ContextBase& context_base,
+             AbstractValue* cache_value) {
+        auto& context = dynamic_cast<const Context<T>&>(context_base);
+        auto& contact_surfaces_cache = cache_value->get_mutable_value<
+            std::vector<ContactSurface<T>>>();
+        this->CalcContactSurfaces(context, &contact_surfaces_cache);
+      },
+      {this->configuration_ticket()});
+  cache_indexes_.contact_surfaces = contact_surfaces_cache_entry.cache_index();
+
   // Cache contact Jacobians.
   auto& contact_jacobians_cache_entry = this->DeclareCacheEntry(
       std::string("Contact Jacobians Jn(q) and Jt(q)."),

multibody/plant/multibody_plant.h

@@ -3184,6 +3184,7 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
   struct CacheIndexes {
     systems::CacheIndex contact_jacobians;
     systems::CacheIndex contact_results;
+    systems::CacheIndex contact_surfaces;
     systems::CacheIndex generalized_accelerations;
     systems::CacheIndex hydro_contact_forces;
     systems::CacheIndex tamsi_solver_results;
@@ -3335,11 +3336,38 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
         .template Eval<internal::TamsiSolverResults<T>>(context);
   }
 
+  // Computes the vector of ContactSurfaces for hydroelastic contact.
+  void CalcContactSurfaces(
+      const drake::systems::Context<T>& context,
+      std::vector<geometry::ContactSurface<T>>* contact_surfaces) const;
+
+  // Eval version of the method CalcContactSurfaces().
+  const std::vector<geometry::ContactSurface<T>>& EvalContactSurfaces(
+      const systems::Context<T>& context) const {
+    return this
+        ->get_cache_entry(cache_indexes_.contact_surfaces)
+        .template Eval<std::vector<geometry::ContactSurface<T>>>(context);
+  }
+
   // Helper method to fill in the ContactResults given the current context when
   // the model is continuous.
   void CalcContactResultsContinuous(const systems::Context<T>& context,
                                     ContactResults<T>* contact_results) const;
 
+  // Helper method for the continuous mode plant, to fill in the ContactResults
+  // for the point pair model, given the current context. Called by
+  // CalcContactResultsContinuous.
+  void CalcContactResultsContinuousPointPair(
+      const systems::Context<T>& context,
+      ContactResults<T>* contact_results) const;
+
+  // Helper method for the continuous mode plant, to fill in the ContactResults
+  // for the hydroelastic model, given the current context. Called by
+  // CalcContactResultsContinuous.
+  void CalcContactResultsContinuousHydroelastic(
+      const systems::Context<T>& context,
+      ContactResults<T>* contact_results) const;
+
   // Helper method to fill in the ContactResults given the current context when
   // the model is discrete. If cached contact solver results are not up-to-date
   // with `context`, they'll be  recomputed, see EvalTamsiResults(). The solver
@@ -3930,6 +3958,11 @@ template <>
 void MultibodyPlant<symbolic::Expression>::CalcHydroelasticContactForces(
     const systems::Context<symbolic::Expression>&,
     std::vector<SpatialForce<symbolic::Expression>>*) const;
+template <>
+void MultibodyPlant<symbolic::Expression>::
+    CalcContactResultsContinuousHydroelastic(
+        const systems::Context<symbolic::Expression>&,
+        ContactResults<symbolic::Expression>*) const;
 #endif
 
 }  // namespace multibody