[geometry] ContactSurface automatically swaps M and N when id_M > id_…

…N. (RobotLocomotion#11803)

geometry/proximity/mesh_intersection.h

@@ -636,19 +636,15 @@ ComputeContactSurfaceFromSoftVolumeRigidSurface(
     const GeometryId id_S, const VolumeMeshField<T, T>& field_S,
     const GeometryId id_R, const SurfaceMesh<T>& mesh_R,
     const math::RigidTransform<T>& X_SR) {
-  std::unique_ptr<SurfaceMesh<T>> surface_MN_M;
-  std::unique_ptr<SurfaceMeshFieldLinear<T, T>> e_MN;
-  std::unique_ptr<SurfaceMeshFieldLinear<Vector3<T>, T>> grad_h_MN_M;
-  SampleVolumeFieldOnSurface(field_S, mesh_R, X_SR, &surface_MN_M, &e_MN,
-                             &grad_h_MN_M);
-
-  // Blindly map S->M and R->N.
-  auto surface = std::make_unique<ContactSurface<T>>(
-      id_S, id_R, std::move(surface_MN_M), std::move(e_MN),
-      std::move(grad_h_MN_M));
-  // Correct mapping as necessary.
-  if (surface->id_N() < surface->id_M()) surface->SwapMAndN(X_SR.inverse());
-  return surface;
+  std::unique_ptr<SurfaceMesh<T>> surface_SR_S;
+  std::unique_ptr<SurfaceMeshFieldLinear<T, T>> e_SR;
+  std::unique_ptr<SurfaceMeshFieldLinear<Vector3<T>, T>> grad_h_SR_S;
+  SampleVolumeFieldOnSurface(field_S, mesh_R, X_SR, &surface_SR_S, &e_SR,
+                             &grad_h_SR_S);
+
+  return std::make_unique<ContactSurface<T>>(
+      id_S, id_R, std::move(surface_SR_S), std::move(e_SR),
+      std::move(grad_h_SR_S), X_SR);
 }
 
 #endif  // #ifndef DRAKE_DOXYGEN_CXX

geometry/proximity/test/contact_surface_test.cc

@@ -13,8 +13,32 @@
 #include "drake/geometry/proximity/surface_mesh.h"
 #include "drake/math/rigid_transform.h"
 
+// TODO(DamrongGuoy): Move to geometry/query_results/test/.
 namespace drake {
 namespace geometry {
+
+// TODO(DamrongGuoy): Remove this helper class when ContactSurface allows
+//  direct access to e_MN_ and grad_h_MN_M_.
+template <typename T>
+class ContactSurfaceTester {
+ public:
+  explicit ContactSurfaceTester(const geometry::ContactSurface<T>& surface)
+      : surface_(surface) {}
+
+  const SurfaceMeshFieldLinear<T, T>& e_MN() const {
+    DRAKE_DEMAND(surface_.e_MN_ != nullptr);
+    return *(surface_.e_MN_);
+  }
+
+  const SurfaceMeshFieldLinear<Vector3<T>, T>& grad_h_MN_M() const {
+    DRAKE_DEMAND(surface_.grad_h_MN_M_ != nullptr);
+    return *(surface_.grad_h_MN_M_);
+  }
+
+ private:
+  const geometry::ContactSurface<T>& surface_;
+};
+
 namespace {
 
 using Eigen::AngleAxisd;
@@ -104,7 +128,8 @@ ContactSurface<T> TestContactSurface() {
 
   ContactSurface<T> contact_surface(id_M, id_N, std::move(surface_mesh),
                                     std::move(e_field),
-                                    std::move(grad_h_MN_M_field));
+                                    std::move(grad_h_MN_M_field),
+                                    math::RigidTransform<T>::Identity());
 
   // Start testing the ContactSurface<> data structure.
   EXPECT_EQ(id_M, contact_surface.id_M());
@@ -169,23 +194,44 @@ GTEST_TEST(ContactSurfaceTest, TestCopy) {
   EXPECT_EQ(original.EvaluateGrad_h_MN_M(f, b), copy.EvaluateGrad_h_MN_M(f, b));
 }
 
-GTEST_TEST(ContactSurfaceTest, SwapMAndN) {
+// Tests the constructor of ContactSurface that when id_M is greater than
+// id_N, it will swap M and N.
+GTEST_TEST(ContactSurfaceTest, TestSwapMAndN) {
+  // Create the original contact surface for comparison later.
   const ContactSurface<double> original = TestContactSurface<double>();
-  ContactSurface<double> dut(original);
-
-  // We assume the copy works.
+  auto mesh = std::make_unique<SurfaceMesh<double>>(original.mesh());
+  SurfaceMesh<double>* mesh_pointer = mesh.get();
+  // TODO(DamrongGuoy): Remove `original_tester` when ContactSurface allows
+  //  direct access to e_MN and grad_h_MN_M.
+  const ContactSurfaceTester<double> original_tester(original);
+  std::vector<double> e_MN_values = original_tester.e_MN().values();
+  std::vector<Vector3<double>> grad_h_MN_M_values =
+      original_tester.grad_h_MN_M().values();
 
   const RigidTransformd X_NM{
       AngleAxisd{M_PI / 4, Vector3d{1, 2, 3}.normalized()}, Vector3d{1, 2, 3}};
-  dut.SwapMAndN(X_NM);
+  const RigidTransformd X_MN = X_NM.inverse();
+
+  // Create id_M after id_N, so id_M > id_N. This condition will trigger
+  // SwapMAndN in the constructor of ContactSurface.
+  auto id_N = GeometryId::get_new_id();
+  auto id_M = GeometryId::get_new_id();
+  ASSERT_LT(id_N, id_M);
+  ContactSurface<double> dut(
+      id_M, id_N, std::move(mesh),
+      std::make_unique<SurfaceMeshFieldLinear<double, double>>(
+          "e_MN", std::move(e_MN_values), mesh_pointer),
+      std::make_unique<SurfaceMeshFieldLinear<Vector3<double>, double>>(
+          "grad_h_MN_M", std::move(grad_h_MN_M_values), mesh_pointer),
+      X_MN);
 
   // We rely on the underlying meshes and mesh fields to *do* the right thing.
-  // These tests are just to confirm that those things changed that we expected
-  // to change (and not, where appropriate).
+  // These tests are just to confirm that those things changed where we
+  // expected to change (and not, where appropriate).
 
   // Ids have swapped.
-  EXPECT_EQ(dut.id_M(), original.id_N());
-  EXPECT_EQ(dut.id_N(), original.id_M());
+  EXPECT_EQ(dut.id_M(), id_N);
+  EXPECT_EQ(dut.id_N(), id_M);
 
   // Determines if two faces have the same indices in the same order.
   auto are_identical = [](const SurfaceFace& f1, const SurfaceFace& f2) {
@@ -200,8 +246,9 @@ GTEST_TEST(ContactSurfaceTest, SwapMAndN) {
 
   // Vertices have been transformed.
   for (SurfaceVertexIndex v(0); v < original.mesh().num_vertices(); ++v) {
-    const Vector3d r_NV = X_NM * original.mesh().vertex(v).r_MV();
-    EXPECT_TRUE(CompareMatrices(dut.mesh().vertex(v).r_MV(), r_NV));
+    const Vector3d expected_r_NV = X_NM * original.mesh().vertex(v).r_MV();
+    EXPECT_TRUE(CompareMatrices(dut.mesh().vertex(v).r_MV(), expected_r_NV,
+                                2.0 * std::numeric_limits<double>::epsilon()));
   }
 
   // Test the mesh fields by evaluating each field, once per face for an

geometry/query_results/contact_surface.h

@@ -172,16 +172,23 @@ class ContactSurface {
                        orthogonal to every triangle sharing the vertex.
                        Orthogonality generally does improve with finer
                        discretization.
+   @param X_MN         The pose of the frame of N in the frame of M.
+   @note If the id_M is greater than the id_N, we will swap M and N.
+         Therefore, the mesh_M and grad_h_MN_M will change their frames.
+         Furthermore, the vector field grad_h_MN_M will switch its direction.
    */
   ContactSurface(
       GeometryId id_M, GeometryId id_N, std::unique_ptr<SurfaceMesh<T>> mesh_M,
       std::unique_ptr<SurfaceMeshFieldLinear<T, T>> e_MN,
-      std::unique_ptr<SurfaceMeshFieldLinear<Vector3<T>, T>> grad_h_MN_M)
+      std::unique_ptr<SurfaceMeshFieldLinear<Vector3<T>, T>> grad_h_MN_M,
+      const math::RigidTransform<T>& X_MN)
       : id_M_(id_M),
         id_N_(id_N),
         mesh_M_(std::move(mesh_M)),
         e_MN_(std::move(e_MN)),
-        grad_h_MN_M_(std::move(grad_h_MN_M)) {}
+        grad_h_MN_M_(std::move(grad_h_MN_M)) {
+    if (id_N_ < id_M_) SwapMAndN(X_MN.inverse());
+  }
 
   /** Returns the geometry id of Geometry M. */
   GeometryId id_M() const { return id_M_; }
@@ -239,10 +246,10 @@ class ContactSurface {
     return *mesh_M_;
   }
 
-  /** Swaps M and N (modifying the data in place to reflect the change in
-   frames).
-   @param X_NM  The pose of frame M in N.
-   */
+ private:
+  // Swaps M and N (modifying the data in place to reflect the change in
+  // frames).
+  // @param X_NM  The pose of frame M in N.
   void SwapMAndN(const math::RigidTransform<T>& X_NM) {
     std::swap(id_M_, id_N_);
     mesh_M_->TransformVertices(X_NM);
@@ -268,22 +275,23 @@ class ContactSurface {
     // Note: the scalar field does not depend on frames.
   }
 
- private:
-  /** The id of the first geometry M */
+  // The id of the first geometry M.
   GeometryId id_M_;
-  /** The id of the second geometry N. */
+  // The id of the second geometry N.
   GeometryId id_N_;
-  /** The surface mesh of the contact surface 𝕊ₘₙ between M and N. */
+  // The surface mesh of the contact surface 𝕊ₘₙ between M and N.
   std::unique_ptr<SurfaceMesh<T>> mesh_M_;
   // TODO(SeanCurtis-TRI): We can only construct from a linear field, so store
   //  it as such for now. This can be promoted once there's a construction that
   //  uses a different derivation.
-  /** Represents the scalar field eₘₙ on the surface mesh. */
+  // Represents the scalar field eₘₙ on the surface mesh.
   std::unique_ptr<SurfaceMeshFieldLinear<T, T>> e_MN_;
-  /** Represents the vector field ∇hₘₙ on the surface mesh, expressed in M's
-    frame */
+  // Represents the vector field ∇hₘₙ on the surface mesh, expressed in M's
+  // frame.
   std::unique_ptr<SurfaceMeshFieldLinear<Vector3<T>, T>> grad_h_MN_M_;
-  friend class ContactSurfaceTester;
+  // TODO(DamrongGuoy): Remove this when we allow direct access to e_MN and
+  //  grad_h_MN.
+  template <typename U> friend class ContactSurfaceTester;
 };
 
 }  // namespace geometry

multibody/hydroelastics/hydroelastic_engine.cc

@@ -156,17 +156,8 @@ optional<ContactSurface<T>> HydroelasticEngine<T>::CalcContactSurface(
   // In consistency with ContactSurface's contract, the first id must belong
   // to the geometry associated with the frame in which quantities are
   // expressed, in this case id_R.
-  ContactSurface<T> contact_surface(id_R, id_S, std::move(surface_R),
-                                    std::move(e_s),
-                                    std::move(grad_level_set_R));
-  // ContactSurface's contract is that id_M < id_N. We make sure this is the
-  // case by swapping if needed.
-  // TODO(amcastro-tri): Update to pass X_SR to ContactSurface's constructor
-  // once ContactSurface takes care of the swapping at construction.
-  if (id_S < id_R) {
-    contact_surface.SwapMAndN(X_RS.inverse());
-  }
-  return contact_surface;
+  return ContactSurface<T>(id_R, id_S, std::move(surface_R), std::move(e_s),
+                           std::move(grad_level_set_R), X_RS);
 }
 
 template <typename T>

multibody/plant/test/hydroelastic_traction_test.cc

@@ -252,7 +252,8 @@ public ::testing::TestWithParam<RigidTransform<double>> {
       std::make_unique<MeshFieldLinear<double, SurfaceMesh<double>>>(
           "e_MN", std::move(e_MN), mesh_pointer),
       std::make_unique<MeshFieldLinear<Vector3<double>, SurfaceMesh<double>>>(
-          "h_MN_M", std::move(h_MN_M), mesh_pointer));
+          "h_MN_M", std::move(h_MN_M), mesh_pointer),
+      RigidTransform<double>::Identity());
   }
 
   const double tol_{10 * std::numeric_limits<double>::epsilon()};
@@ -569,6 +570,7 @@ public ::testing::TestWithParam<RigidTransform<double>> {
     const RigidTransform<double> X_YA = RigidTransform<double>::Identity();
     const RigidTransform<double> X_YB = RigidTransform<double>::Identity();
     const RigidTransform<double> X_YM = RigidTransform<double>::Identity();
+    const RigidTransform<double> X_MN = RigidTransform<double>::Identity();
 
     // Note: this test does not require valid GeometryIds.
     const GeometryId null_id;
@@ -593,7 +595,8 @@ public ::testing::TestWithParam<RigidTransform<double>> {
       std::make_unique<MeshFieldLinear<double, SurfaceMesh<double>>>(
           "e_MN", std::move(e_MN), mesh_pointer),
       std::make_unique<MeshFieldLinear<Vector3<double>, SurfaceMesh<double>>>(
-          "h_MN_M", std::move(h_MN_M), mesh_pointer));
+          "h_MN_M", std::move(h_MN_M), mesh_pointer),
+      X_MN);
 
     // Set the velocities to correspond to one body fixed and one body
     // free so that we can test the slip velocity. Additionally, we'll