[multibody] Add ContactResults calculation to DiscreteUpdateManager (R…

…obotLocomotion#18402)

Add ContactResults calculation to DiscreteUpdateManager.

multibody/plant/BUILD.bazel

@@ -166,6 +166,7 @@ drake_cc_library(
     deps = [
         ":multibody_plant_core",
         "//common:essential",
+        "//geometry:geometry_ids",
     ],
 )
 
@@ -445,6 +446,7 @@ drake_cc_googletest(
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_throws_message",
         "//multibody/parsing",
+        "//multibody/plant/test_utilities:rigid_body_on_compliant_ground",
         "//systems/primitives:pass_through",
         "//systems/primitives:zero_order_hold",
     ],
@@ -496,11 +498,8 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "tamsi_driver_test",
-    data = ["test/square_surface.obj"],
     deps = [
-        ":compliant_contact_manager_tester",
-        ":plant",
-        "//common:find_resource",
+        "//multibody/plant/test_utilities:rigid_body_on_compliant_ground",
     ],
 )
 

multibody/plant/compliant_contact_manager.cc

@@ -20,8 +20,10 @@
 #include "drake/multibody/triangle_quadrature/gaussian_triangle_quadrature_rule.h"
 #include "drake/systems/framework/context.h"
 
+using drake::geometry::ContactSurface;
 using drake::geometry::GeometryId;
 using drake::geometry::PenetrationAsPointPair;
+using drake::math::RotationMatrix;
 using drake::multibody::contact_solvers::internal::ContactSolverResults;
 using drake::multibody::internal::DiscreteContactPair;
 using drake::multibody::internal::MultibodyTreeTopology;
@@ -92,6 +94,28 @@ void CompliantContactManager<T>::DeclareCacheEntries() {
   cache_indexes_.discrete_contact_pairs =
       discrete_contact_pairs_cache_entry.cache_index();
 
+  // Cache hydroelastic contact info.
+  const auto& hydroelastic_contact_info_cache_entry = this->DeclareCacheEntry(
+      "Hydroelastic contact info.",
+      systems::ValueProducer(
+          this, &CompliantContactManager<T>::CalcHydroelasticContactInfo),
+      // Compliant contact forces due to hydroelastics with Hunt &
+      // Crosseley are function of the kinematic variables q & v only.
+      {systems::System<T>::xd_ticket(),
+       systems::System<T>::all_parameters_ticket()});
+  cache_indexes_.hydroelastic_contact_info =
+      hydroelastic_contact_info_cache_entry.cache_index();
+
+  // Cache contact kinematics.
+  const auto& contact_kinematics_cache_entry = this->DeclareCacheEntry(
+      "Contact kinematics.",
+      systems::ValueProducer(
+          this, &CompliantContactManager::CalcContactKinematics),
+      {systems::System<T>::xd_ticket(),
+       systems::System<T>::all_parameters_ticket()});
+  cache_indexes_.contact_kinematics =
+      contact_kinematics_cache_entry.cache_index();
+
   // Accelerations due to non-contact forces.
   // We cache non-contact forces, ABA forces and accelerations into a
   // AccelerationsDueToExternalForcesCache.
@@ -232,6 +256,15 @@ CompliantContactManager<T>::CalcContactKinematics(
   return contact_kinematics;
 }
 
+template <typename T>
+const std::vector<ContactPairKinematics<T>>&
+CompliantContactManager<T>::EvalContactKinematics(
+    const systems::Context<T>& context) const {
+  return plant()
+      .get_cache_entry(cache_indexes_.contact_kinematics)
+      .template Eval<std::vector<ContactPairKinematics<T>>>(context);
+}
+
 template <>
 void CompliantContactManager<symbolic::Expression>::CalcDiscreteContactPairs(
     const drake::systems::Context<symbolic::Expression>&,
@@ -354,8 +387,9 @@ void CompliantContactManager<T>::AppendDiscreteContactPairsForPointContact(
     const T phi0 = -pair.depth;
     const T fn0 = k * pair.depth;  // Used by TAMSI, ignored by SAP.
 
-    contact_pairs.push_back(
-        {pair.id_A, pair.id_B, p_WC, pair.nhat_BA_W, phi0, fn0, k, d, tau, mu});
+    contact_pairs.push_back({pair.id_A, pair.id_B, p_WC, pair.nhat_BA_W, phi0,
+                             fn0, k, d, tau, mu, {} /* no surface index */,
+                             {} /* no face index */});
   }
 }
 
@@ -389,7 +423,11 @@ void CompliantContactManager<T>::
   const geometry::SceneGraphInspector<T>& inspector = query_object.inspector();
   const std::vector<geometry::ContactSurface<T>>& surfaces =
       this->EvalContactSurfaces(context);
-  for (const auto& s : surfaces) {
+
+  const int num_surfaces = surfaces.size();
+  for (int surface_index = 0; surface_index < num_surfaces; ++surface_index) {
+    const auto& s = surfaces[surface_index];
+
     const bool M_is_compliant = s.HasGradE_M();
     const bool N_is_compliant = s.HasGradE_N();
     DRAKE_DEMAND(M_is_compliant || N_is_compliant);
@@ -500,8 +538,8 @@ void CompliantContactManager<T>::
         const T phi0 = -p0 / g;
 
         if (k > 0) {
-          contact_pairs.push_back(
-              {s.id_M(), s.id_N(), p_WQ, nhat_W, phi0, fn0, k, d, tau, mu});
+          contact_pairs.push_back({s.id_M(), s.id_N(), p_WQ, nhat_W, phi0, fn0,
+                                   k, d, tau, mu, surface_index, face});
         }
       }
     }
@@ -649,6 +687,217 @@ void CompliantContactManager<T>::DoCalcDiscreteValues(
   }
 }
 
+template <typename T>
+void CompliantContactManager<T>::AppendContactResultsForPointContact(
+    const drake::systems::Context<T>& context,
+    ContactResults<T>* contact_results) const {
+  DRAKE_DEMAND(contact_results != nullptr);
+
+  const std::vector<PenetrationAsPointPair<T>>& point_pairs =
+      plant().EvalPointPairPenetrations(context);
+  const std::vector<internal::DiscreteContactPair<T>>& discrete_pairs =
+      this->EvalDiscreteContactPairs(context);
+  const std::vector<ContactPairKinematics<T>>& contact_kinematics =
+      this->EvalContactKinematics(context);
+  const contact_solvers::internal::ContactSolverResults<T>& solver_results =
+      this->EvalContactSolverResults(context);
+
+  const VectorX<T>& fn = solver_results.fn;
+  const VectorX<T>& ft = solver_results.ft;
+  const VectorX<T>& vt = solver_results.vt;
+  const VectorX<T>& vn = solver_results.vn;
+
+
+  const int num_contacts = point_pairs.size();
+  DRAKE_DEMAND(fn.size() >= num_contacts);
+  DRAKE_DEMAND(ft.size() >= 2 * num_contacts);
+  DRAKE_DEMAND(vn.size() >= num_contacts);
+  DRAKE_DEMAND(vt.size() >= 2 * num_contacts);
+
+  // The correspondence between `discrete_pairs` and `point_pairs` depends on
+  // strict ordering of CompliantContactManager::CalcDiscreteContactPairs.
+  // All point contacts must come first and their order in discrete_pairs
+  // corresponds to their order in point_pairs.
+  for (int icontact = 0; icontact < num_contacts; ++icontact) {
+    const auto& discrete_pair = discrete_pairs[icontact];
+    const auto& point_pair = point_pairs[icontact];
+
+    const GeometryId geometryA_id = discrete_pair.id_A;
+    const GeometryId geometryB_id = discrete_pair.id_B;
+
+    const BodyIndex bodyA_index = this->FindBodyByGeometryId(geometryA_id);
+    const BodyIndex bodyB_index = this->FindBodyByGeometryId(geometryB_id);
+
+    const RotationMatrix<T>& R_WC = contact_kinematics[icontact].R_WC;
+
+    // Contact forces applied on B at contact point C.
+    const Vector3<T> f_Bc_C(ft(2 * icontact), ft(2 * icontact + 1),
+                            fn(icontact));
+    const Vector3<T> f_Bc_W = R_WC * f_Bc_C;
+
+    // Slip velocity.
+    const T slip = vt.template segment<2>(2 * icontact).norm();
+
+    // Separation velocity in the normal direction.
+    const T separation_velocity = vn(icontact);
+
+    // Add pair info to the contact results.
+    contact_results->AddContactInfo({bodyA_index, bodyB_index, f_Bc_W,
+                                     discrete_pair.p_WC, separation_velocity,
+                                     slip, point_pair});
+  }
+}
+
+template <typename T>
+void CompliantContactManager<T>::AppendContactResultsForHydroelasticContact(
+    const drake::systems::Context<T>& context,
+    ContactResults<T>* contact_results) const {
+  const std::vector<HydroelasticContactInfo<T>>& contact_info =
+      this->EvalHydroelasticContactInfo(context);
+
+  for (const HydroelasticContactInfo<T>& info : contact_info) {
+    // Note: caching dependencies guarantee that the lifetime of `info` is
+    // valid for the lifetime of the contact results.
+    contact_results->AddContactInfo(&info);
+  }
+}
+
+template <typename T>
+void CompliantContactManager<T>::CalcHydroelasticContactInfo(
+    const systems::Context<T>& context,
+    std::vector<HydroelasticContactInfo<T>>* contact_info) const {
+  DRAKE_DEMAND(contact_info != nullptr);
+
+  const std::vector<ContactSurface<T>>& all_surfaces =
+      this->EvalContactSurfaces(context);
+
+  // Reserve memory here to keep from repeatedly allocating heap storage in
+  // the loop below.
+  // TODO(joemasterjohn): Consider caching this vector and the quadrature
+  // point data vectors to avoid this dynamic allocation.
+  contact_info->clear();
+  contact_info->reserve(all_surfaces.size());
+
+  const std::vector<internal::DiscreteContactPair<T>>& discrete_pairs =
+      this->EvalDiscreteContactPairs(context);
+  const std::vector<ContactPairKinematics<T>>& contact_kinematics =
+      this->EvalContactKinematics(context);
+
+  const contact_solvers::internal::ContactSolverResults<T>& solver_results =
+      this->EvalContactSolverResults(context);
+
+  const VectorX<T>& fn = solver_results.fn;
+  const VectorX<T>& ft = solver_results.ft;
+  const VectorX<T>& vt = solver_results.vt;
+  const VectorX<T>& vn = solver_results.vn;
+
+  // Discrete pairs contain both point and hydro contact force results.
+  const int num_contacts = discrete_pairs.size();
+  DRAKE_DEMAND(fn.size() == num_contacts);
+  DRAKE_DEMAND(ft.size() == 2 * num_contacts);
+  DRAKE_DEMAND(vn.size() == num_contacts);
+  DRAKE_DEMAND(vt.size() == 2 * num_contacts);
+
+  int num_point_contacts = plant().EvalPointPairPenetrations(context).size();
+  const int num_surfaces = all_surfaces.size();
+
+  std::vector<SpatialForce<T>> F_Ao_W_per_surface(num_surfaces,
+                                                      SpatialForce<T>::Zero());
+
+  std::vector<std::vector<HydroelasticQuadraturePointData<T>>> quadrature_data(
+      num_surfaces);
+  for (int isurface = 0; isurface < num_surfaces; ++isurface) {
+    quadrature_data[isurface].reserve(all_surfaces[isurface].num_faces());
+  }
+
+  // We only scan discrete pairs corresponding to hydroelastic quadrature
+  // points. These are appended by CalcDiscreteContactPairs() at the end of the
+  // point contact forces.
+  for (int icontact = num_point_contacts; icontact < num_contacts; ++icontact) {
+    const auto& pair = discrete_pairs[icontact];
+    // Quadrature point Q.
+    const Vector3<T>& p_WQ = pair.p_WC;
+    const RotationMatrix<T>& R_WC = contact_kinematics[icontact].R_WC;
+
+    // Contact forces applied on B at quadrature point Q expressed in the
+    // contact frame.
+    const Vector3<T> f_Bq_C(ft(2 * icontact), ft(2 * icontact + 1),
+                            fn(icontact));
+    // Contact force applied on A at quadrature point Q expressed in the world
+    // frame.
+    const Vector3<T> f_Aq_W = -(R_WC * f_Bq_C);
+
+    const int surface_index = pair.surface_index.value();
+    const auto& s = all_surfaces[surface_index];
+    // Surface's centroid point O.
+    const Vector3<T>& p_WO = s.is_triangle() ? s.tri_mesh_W().centroid()
+                                             : s.poly_mesh_W().centroid();
+
+    // Spatial force
+    const Vector3<T> p_QO_W = p_WO - p_WQ;
+    const SpatialForce<T> Fq_Ao_W =
+        SpatialForce<T>(Vector3<T>::Zero(), f_Aq_W).Shift(p_QO_W);
+    // Accumulate force for the corresponding contact surface.
+    F_Ao_W_per_surface[surface_index] += Fq_Ao_W;
+
+    // Velocity of Aq relative to Bq in the tangent direction.
+    // N.B. CompliantContactManager<T>::CalcContactKinematics() uses the
+    // convention of computing J_AcBc_C and thus J_AcBc_C * v = v_AcBc_W (i.e.
+    // relative velocity of Bc with respect to Ac). Thus we flip the sign here
+    // for the convention used by HydroelasticQuadratureData.
+    const Vector3<T> vt_BqAq_C(-vt(2 * icontact), -vt(2 * icontact + 1), 0);
+    const Vector3<T> vt_BqAq_W = R_WC * vt_BqAq_C;
+
+    // Traction vector applied to body A at point Aq (Aq and Bq are coincident)
+    // expressed in the world frame.
+    const int face_index = pair.face_index.value();
+    const Vector3<T> traction_Aq_W = f_Aq_W / s.area(face_index);
+
+    quadrature_data[surface_index].emplace_back(p_WQ, face_index, vt_BqAq_W,
+                                                traction_Aq_W);
+  }
+
+  // Update contact info to include the correct contact forces.
+  for (int surface_index = 0; surface_index < num_surfaces; ++surface_index) {
+    contact_info->emplace_back(
+        &all_surfaces[surface_index], F_Ao_W_per_surface[surface_index],
+        std::move(quadrature_data[surface_index]));
+  }
+}
+
+template <typename T>
+const std::vector<HydroelasticContactInfo<T>>&
+CompliantContactManager<T>::EvalHydroelasticContactInfo(
+    const systems::Context<T>& context) const {
+  return plant()
+      .get_cache_entry(cache_indexes_.hydroelastic_contact_info)
+      .template Eval<std::vector<HydroelasticContactInfo<T>>>(context);
+}
+
+template <typename T>
+void CompliantContactManager<T>::DoCalcContactResults(
+    const drake::systems::Context<T>& context,
+    ContactResults<T>* contact_results) const {
+  DRAKE_DEMAND(contact_results != nullptr);
+  contact_results->Clear();
+  contact_results->set_plant(&plant());
+
+  if (plant().num_collision_geometries() == 0) return;
+
+  switch (plant().get_contact_model()) {
+    case ContactModel::kPoint:
+      AppendContactResultsForPointContact(context, contact_results);
+      break;
+    case ContactModel::kHydroelastic:
+      AppendContactResultsForHydroelasticContact(context, contact_results);
+      break;
+    case ContactModel::kHydroelasticWithFallback:
+      AppendContactResultsForPointContact(context, contact_results);
+      AppendContactResultsForHydroelasticContact(context, contact_results);
+      break;
+  }
+}
+
 // TODO(xuchenhan-tri): Consider a scalar converting constructor to cut down
 // repeated code in CloneToDouble() and CloneToAutoDiffXd().
 template <typename T>