Computes multibody-related fields for hydroelastic model visualization (

RobotLocomotion#11795)

geometry/query_results/contact_surface.h

@@ -189,6 +189,9 @@ class ContactSurface {
   /** Returns the geometry id of Geometry N. */
   GeometryId id_N() const { return id_N_; }
 
+  // TODO(damrongguoy) Consider removing these evaluation methods and instead
+  // make the fields accessible, and then evaluate the fields directly.
+
   /** Evaluates the scalar field eₘₙ at Point Q in a triangle.
     Point Q is specified by its barycentric coordinates.
     @param face         The face index of the triangle.
@@ -200,6 +203,14 @@ class ContactSurface {
     return e_MN_->Evaluate(face, barycentric);
   }
 
+  /** Evaluates the scalar field eₘₙ at the given vertex on the contact surface
+    mesh.
+    @param vertex       The index of the vertex in the mesh.
+   */
+  T EvaluateE_MN(SurfaceVertexIndex vertex) const {
+    return e_MN_->EvaluateAtVertex(vertex);
+  }
+
   /** Evaluates the vector field ∇hₘₙ at Point Q on a triangle.
     Point Q is specified by its barycentric coordinates.
     @param face         The face index of the triangle.
@@ -212,6 +223,15 @@ class ContactSurface {
     return grad_h_MN_M_->Evaluate(face, barycentric);
   }
 
+  /** Evaluates the vector field ∇hₘₙ at the given vertex on the contact surface
+    mesh.
+    @param vertex       The index of the vertex in the mesh.
+    @retval  grad_h_MN_M is the vector expressed in M's frame.
+   */
+  Vector3<T> EvaluateGrad_h_MN_M(SurfaceVertexIndex vertex) const {
+    return grad_h_MN_M_->EvaluateAtVertex(vertex);
+  }
+
   /** Returns a reference to the surface mesh.
    */
   const SurfaceMesh<T>& mesh() const {

multibody/plant/hydroelastic_traction_calculator.cc

@@ -1,7 +1,9 @@
 #include "drake/multibody/plant/hydroelastic_traction_calculator.h"
 
 #include <algorithm>
+#include <memory>
 #include <utility>
+#include <vector>
 
 #include "drake/math/orthonormal_basis.h"
 #include "drake/math/rotation_matrix.h"
@@ -11,8 +13,11 @@
 namespace drake {
 
 using geometry::ContactSurface;
+using geometry::SurfaceFace;
 using geometry::SurfaceFaceIndex;
 using geometry::SurfaceMesh;
+using geometry::SurfaceMeshFieldLinear;
+using geometry::SurfaceVertexIndex;
 using math::RigidTransform;
 
 namespace multibody {
@@ -42,19 +47,17 @@ ComputeSpatialForcesAtBodyOriginsFromHydroelasticModel(
   F_Ac_W.SetZero();
 
   // Integrate the tractions over all triangles in the contact surface.
-  for (geometry::SurfaceFaceIndex i(0); i < data.surface.mesh().num_faces();
-      ++i) {
+  for (SurfaceFaceIndex i(0); i < data.surface.mesh().num_faces(); ++i) {
     // Construct the function to be integrated over triangle i.
     // TODO(sherm1) Pull functor creation out of the loop (not a good idea to
     //              create a new functor for every i).
     std::function<SpatialForce<T>(const Vector3<T>&)> traction_Ac_W =
         [this, &data, i, dissipation,
          mu_coulomb](const Vector3<T>& Q_barycentric) {
-          Vector3<T> p_WQ;
-          const Vector3<T> traction_Aq_W = CalcTractionAtPoint(
-              data, i, Q_barycentric, dissipation, mu_coulomb, &p_WQ);
-          return ComputeSpatialTractionAtAcFromTractionAtAq(data, p_WQ,
-                                                            traction_Aq_W);
+          TractionAtPointData traction_output = CalcTractionAtPoint(
+              data, i, Q_barycentric, dissipation, mu_coulomb);
+          return ComputeSpatialTractionAtAcFromTractionAtAq(
+              data, traction_output.p_WQ, traction_output.traction_Aq_W);
         };
 
     // Compute the integral over the triangle to get a force from the
@@ -108,44 +111,90 @@ SpatialForce<T> HydroelasticTractionCalculator<T>::
   return Ft_Ac_W;  // Still a traction (force/area).
 }
 
+// Method for computing traction at a vertex of the contact surface.
 template <typename T>
-Vector3<T> HydroelasticTractionCalculator<T>::CalcTractionAtPoint(
+typename HydroelasticTractionCalculator<T>::TractionAtPointData
+HydroelasticTractionCalculator<T>::CalcTractionAtVertex(
+    const Data& data,
+    SurfaceVertexIndex vertex_index,
+    double dissipation, double mu_coulomb) const {
+  // Compute the point of contact in the world frame.
+  const Vector3<T> p_WQ = data.X_WM *
+      data.surface.mesh().vertex(vertex_index).r_MV();
+
+  const T e = data.surface.EvaluateE_MN(vertex_index);
+
+  const Vector3<T> h_M = data.surface.EvaluateGrad_h_MN_M(vertex_index);
+  const Vector3<T> nhat_M = h_M.normalized();
+  const Vector3<T> nhat_W = data.X_WM.rotation() * nhat_M;
+
+  return CalcTractionAtQHelper(data, e, nhat_W, dissipation, mu_coulomb, p_WQ);
+}
+
+// Method for computing traction at a point on a face of the contact surface.
+// If the point is coincident with a vertex, CalcTractionAtVertex()
+// is more efficient.
+template <typename T>
+typename HydroelasticTractionCalculator<T>::TractionAtPointData
+HydroelasticTractionCalculator<T>::CalcTractionAtPoint(
     const Data& data,
     SurfaceFaceIndex face_index,
     const typename SurfaceMesh<T>::Barycentric& Q_barycentric,
-    double dissipation, double mu_coulomb, Vector3<T>* p_WQ) const {
-  DRAKE_DEMAND(p_WQ != nullptr);
+    double dissipation, double mu_coulomb) const {
   // Compute the point of contact in the world frame.
   const Vector3<T> p_MQ = data.surface.mesh().CalcCartesianFromBarycentric(
       face_index, Q_barycentric);
-  *p_WQ = data.X_WM * p_MQ;
+  const Vector3<T> p_WQ = data.X_WM * p_MQ;
 
-  // Get the "potential pressure" (in N/m²) at the point as defined in
-  // [Elandt 2019]. Note that we drop the _MN suffix here and below, as this
-  // suffix can get confused with the identical suffix (used for a different
-  // purpose) employed by monogram notation.
-  const T E = data.surface.EvaluateE_MN(face_index, Q_barycentric);
+  const T e = data.surface.EvaluateE_MN(face_index, Q_barycentric);
 
-  // Get the normal from Geometry M to Geometry N, expressed in the world frame,
-  // to the contact surface at Point Q. By extension, this means that the normal
-  // points from Body A to Body B.
   const Vector3<T> h_M = data.surface.EvaluateGrad_h_MN_M(
       face_index, Q_barycentric);
   const Vector3<T> nhat_M = h_M.normalized();
   const Vector3<T> nhat_W = data.X_WM.rotation() * nhat_M;
 
+  return CalcTractionAtQHelper(data, e, nhat_W, dissipation, mu_coulomb, p_WQ);
+}
+
+/*
+ Helper function for computing the traction at a point, irrespective of whether
+ that point is coincident with a vertex or is located at an arbitrary
+ point on the contact surface.
+ @param e the "potential pressure" (in N/m²) at the point as defined in
+        [Elandt 2019]. Note that we drop the _MN suffix here, as this
+        suffix can get confused with the identical suffix (used for a different
+        purpose) employed by monogram notation.
+ @param nhat_W the normal from Geometry M to Geometry N, expressed in the world
+        frame, to the contact surface at p_WQ. By extension, this
+        means that the normal points from Body A to Body B.
+ @param dissipation the dissipation acting between the two bodies.
+ @param mu_coulomb the coefficient of friction acting between the two bodies.
+ @param p_WQ the point localized to the contact surface, as an offset vector
+        expressed in the world frame.
+ */
+template <typename T>
+typename HydroelasticTractionCalculator<T>::TractionAtPointData
+HydroelasticTractionCalculator<T>::CalcTractionAtQHelper(
+    const Data& data,
+    const T& e, const Vector3<T>& nhat_W,
+    double dissipation, double mu_coulomb, const Vector3<T>& p_WQ) const {
+  TractionAtPointData traction_data;
+
+  // Set p_WQ first.
+  traction_data.p_WQ = p_WQ;
+
   // Get the relative spatial velocity at the point Q between the
   // two bodies A and B (to which M and N are affixed, respectively) by
   // subtracting the spatial velocity of a point (Bq) coincident with p_WQ on
   // Body B from the spatial velocity of a point (Aq) coincident with p_WQ on
   // Body A.
 
   // First compute the spatial velocity of Body A at Aq.
-  const Vector3<T> p_AoAq_W = *p_WQ - data.X_WA.translation();
+  const Vector3<T> p_AoAq_W = traction_data.p_WQ - data.X_WA.translation();
   const SpatialVelocity<T> V_WAq = data.V_WA.Shift(p_AoAq_W);
 
   // Next compute the spatial velocity of Body B at Bq.
-  const Vector3<T> p_BoBq_W = *p_WQ - data.X_WB.translation();
+  const Vector3<T> p_BoBq_W = traction_data.p_WQ - data.X_WB.translation();
   const SpatialVelocity<T> V_WBq = data.V_WB.Shift(p_BoBq_W);
 
   // Finally compute the relative velocity of Frame Aq relative to Frame Bq,
@@ -162,30 +211,67 @@ Vector3<T> HydroelasticTractionCalculator<T>::CalcTractionAtPoint(
   // Get the damping value (c) from the compliant model dissipation (α).
   // Equation (16) from [Hunt 1975], but neglecting the 3/2 term used for
   // Hertzian contact, yields c = α * e_mn with units of N⋅s/m³.
-  const T c = dissipation * E;
+  const T c = dissipation * e;
 
   // Determine the normal traction at the point.
   using std::max;
-  const T normal_traction = max(E - vn_BqAq_W * c, T(0));
+  const T normal_traction = max(e - vn_BqAq_W * c, T(0));
 
   // Get the slip velocity at the point.
-  const Vector3<T> vt_BqAq_W = v_BqAq_W - nhat_W * vn_BqAq_W;
+  traction_data.vt_BqAq_W = v_BqAq_W - nhat_W * vn_BqAq_W;
 
   // Determine the traction using a soft-norm.
   using std::atan;
   using std::sqrt;
-  const T squared_vt = vt_BqAq_W.squaredNorm();
+  const T squared_vt = traction_data.vt_BqAq_W.squaredNorm();
   const T norm_vt = sqrt(squared_vt);
   const T soft_norm_vt = sqrt(squared_vt +
       vslip_regularizer_ * vslip_regularizer_);
 
   // Get the regularized direction of slip.
-  const Vector3<T> vt_hat_BqAq_W = vt_BqAq_W / soft_norm_vt;
+  const Vector3<T> vt_hat_BqAq_W = traction_data.vt_BqAq_W / soft_norm_vt;
 
   // Compute the traction.
   const T frictional_scalar = mu_coulomb * normal_traction *
       2.0 / M_PI * atan(norm_vt / T(vslip_regularizer_));
-  return nhat_W * normal_traction - vt_hat_BqAq_W * frictional_scalar;
+  traction_data.traction_Aq_W = nhat_W * normal_traction -
+      vt_hat_BqAq_W * frictional_scalar;
+
+  return traction_data;
+}
+
+// Creates linearly interpolated fields over the contact surface for use in
+// contact reporting.
+// @warning The newly created mesh fields retain a pointer to the surface mesh
+//         (i.e., `data.surface.mesh()`), so that pointer must remain valid
+//        while this object is alive.
+template <typename T>
+typename HydroelasticTractionCalculator<T>::ContactReportingFields
+HydroelasticTractionCalculator<T>::CreateReportingFields(
+    const HydroelasticTractionCalculator<T>::Data& data,
+    double dissipation, double mu_coulomb) const {
+  // Alias the contact surface.
+  const ContactSurface<T>& surface = data.surface;
+
+  // Compute a value for each vertex.
+  std::vector<Vector3<T>> vt_BqAq_W(surface.mesh().num_vertices());
+  std::vector<Vector3<T>> traction_Aq_W(surface.mesh().num_vertices());
+
+  for (SurfaceVertexIndex i(0); i < surface.mesh().num_vertices(); ++i) {
+    // Compute the traction and the slip velocity at the vertex.
+    HydroelasticTractionCalculator<T>::TractionAtPointData output =
+        CalcTractionAtVertex(data, i, dissipation, mu_coulomb);
+    traction_Aq_W[i] = output.traction_Aq_W;
+    vt_BqAq_W[i] = output.vt_BqAq_W;
+  }
+
+  // Create the field structure.
+  ContactReportingFields fields;
+  fields.traction_A_W = std::make_unique<SurfaceMeshFieldLinear<Vector3<T>, T>>(
+      "traction", std::move(traction_Aq_W), &surface.mesh());
+  fields.vslip_AB_W = std::make_unique<SurfaceMeshFieldLinear<Vector3<T>, T>>(
+      "slip_velocity", std::move(vt_BqAq_W), &surface.mesh());
+  return fields;
 }
 
 }  // namespace internal

multibody/plant/hydroelastic_traction_calculator.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <memory>
+
 #include "drake/geometry/proximity/surface_mesh.h"
 #include "drake/geometry/query_results/contact_surface.h"
 #include "drake/math/rigid_transform.h"
@@ -72,7 +74,6 @@ class HydroelasticTractionCalculator {
     const Vector3<T> p_WC;
   };
 
- public:
   DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(HydroelasticTractionCalculator)
 
   HydroelasticTractionCalculator() {}
@@ -95,20 +96,75 @@ class HydroelasticTractionCalculator {
    @param mu_coulomb the nonnegative coefficient for Coulomb friction.
    @param[output] F_Ao_W the spatial force on Body A, on return.
    @param[output] F_Bo_W the spatial force on Body B, on return.
-   */ 
+   */
   void ComputeSpatialForcesAtBodyOriginsFromHydroelasticModel(
        const Data& data, double dissipation, double mu_coulomb,
        multibody::SpatialForce<T>* F_Ao_W,
        multibody::SpatialForce<T>* F_Bo_W) const;
 
  private:
+  // TODO(edrumwri): Consider methods that expose inner structures of
+  // HydroelasticTractionCalculator in HydroelasticReportingTests if we have
+  // to "friend" too many testing functions.
   // To allow GTEST to test private functions.
   friend class MultibodyPlantHydroelasticTractionTests;
+  friend class HydroelasticReportingTests;
+  friend class HydroelasticReportingTests_LinearTraction_Test;
+  friend class HydroelasticReportingTests_LinearSlipVelocity_Test;
+
+  struct TractionAtPointData {
+    // Q, the point that the traction is computed, as an offset vector expressed
+    // in the world frame.
+    Vector3<T> p_WQ;
+
+    // The slip velocity between Bodies A and B at Point Q, expressed in the
+    // world frame. Note that Point Q is coincident to frames Aq and Bq attached
+    // to Bodies A and B, respectively, and shifted to common point Q.
+    Vector3<T> vt_BqAq_W;
+
+    // The traction vector applied to Frame Aq rigidly attached to Body A at
+    // Point Q (i.e., Frame A is shifted to Aq), expressed in the world frame.
+    Vector3<T> traction_Aq_W;
+  };
 
-  Vector3<T> CalcTractionAtPoint(
-      const Data& data, geometry::SurfaceFaceIndex face_index,
+  // Various fields used for querying kinematic and dynamic quantities over a
+  // contact surface.
+  struct ContactReportingFields {
+    // The traction acting on Body A (i.e., the body that Geometry M is affixed
+    // to), expressed in the 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.
+    std::unique_ptr<geometry::SurfaceMeshField<Vector3<T>, T>> traction_A_W;
+
+    // The slip velocity of Body B (i.e., the body that Geometry N is affixed
+    // to) relative to Body A, expressed in the world frame. At each point Q on
+    // the contact surface, `vslip_AB_W` gives the slip velocity `vslip_AqBq_W`,
+    // which is the "tangential" velocity of Frame Bq (located at Q and attached
+    // Frame B) relative to the tangential velocity of Frame Aq (also located
+    // at Q and 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.
+    std::unique_ptr<geometry::SurfaceMeshField<Vector3<T>, T>> vslip_AB_W;
+  };
+
+  ContactReportingFields CreateReportingFields(
+      const Data& data, double dissipation, double mu_coulomb) const;
+
+  TractionAtPointData CalcTractionAtVertex(
+      const Data& data,
+      geometry::SurfaceVertexIndex vertex_index,
+      double dissipation, double mu_coulomb) const;
+
+  TractionAtPointData CalcTractionAtPoint(
+      const Data& data,
+      geometry::SurfaceFaceIndex face_index,
       const typename geometry::SurfaceMesh<T>::Barycentric& Q_barycentric,
-      double dissipation, double mu_coulomb, Vector3<T>* p_WQ) const;
+      double dissipation, double mu_coulomb) const;
+
+  TractionAtPointData CalcTractionAtQHelper(
+      const Data& data,
+      const T& e, const Vector3<T>& nhat_W,
+      double dissipation, double mu_coulomb, const Vector3<T>& p_WQ) const;
 
   multibody::SpatialForce<T> ComputeSpatialTractionAtAcFromTractionAtAq(
       const Data& data, const Vector3<T>& p_WQ,