geometry: support rigid Mesh in ComputeContactSurfaces for hydroelast…

…ic. (RobotLocomotion#12455)

It is for hydroelastic contact model. Mesh is not supported in other proximity queries, mostly because we cannot use FCL's mesh representation, which is a triangle soup without the concept of enclosing volume.

However, we still need broadphase culling for Mesh geometries colliding with other geometries. Therefore, we have colliding trees of bounding boxes of Mesh separated from the trees of other geometries.

geometry/BUILD.bazel

@@ -291,6 +291,7 @@ filegroup(
     testonly = 1,
     srcs = [
         "test/forbidden_two_cubes.obj",
+        "test/non_convex_mesh.obj",
         "test/quad_cube.mtl",
         "test/quad_cube.obj",
     ],

geometry/geometry_state.cc

@@ -656,47 +656,10 @@ bool GeometryState<T>::IsValidGeometryName(
   return NameIsUnique(frame_id, role, name);
 }
 
-namespace {
-// Small class for identifying mesh geometries.
-class MeshIdentifier final : public ShapeReifier {
- public:
-  bool is_mesh() const { return is_mesh_; }
-
-  // Implementation of ShapeReifier interface.
-  using ShapeReifier::ImplementGeometry;
-  void ImplementGeometry(const Sphere&, void*) final {}
-  void ImplementGeometry(const Cylinder&, void*) final {}
-  void ImplementGeometry(const Ellipsoid&, void*) final {}
-  void ImplementGeometry(const HalfSpace&, void*) final {}
-  void ImplementGeometry(const Box&, void*) final {}
-  void ImplementGeometry(const Capsule&, void*) final {}
-  void ImplementGeometry(const Mesh& mesh, void*) final {
-    is_mesh_ = true;
-    drake::log()->warn("Meshes are _not_ supported for proximity: ({})",
-                       mesh.filename());
-  }
-  void ImplementGeometry(const Convex&, void*) final {}
-
- private:
-  bool is_mesh_{false};
-};
-}  // namespace
-
 template <typename T>
 void GeometryState<T>::AssignRole(SourceId source_id, GeometryId geometry_id,
                                   ProximityProperties properties,
                                   RoleAssign assign) {
-  // TODO(SeanCurtis-TRI): When meshes are supported for proximity roles, remove
-  // this test and the MeshIdentifier class.
-  {
-    const InternalGeometry* g = GetGeometry(geometry_id);
-    if (g) {
-      MeshIdentifier identifier;
-      g->shape().Reify(&identifier);
-      if (identifier.is_mesh()) return;
-    }
-  }
-
   InternalGeometry& geometry =
       ValidateRoleAssign(source_id, geometry_id, Role::kProximity, assign);
 

geometry/proximity/BUILD.bazel

@@ -167,6 +167,7 @@ drake_cc_library(
         "//geometry/proximity:make_ellipsoid_mesh",
         "//geometry/proximity:make_sphere_field",
         "//geometry/proximity:make_sphere_mesh",
+        "//geometry/proximity:obj_to_surface_mesh",
         "//geometry/proximity:proximity_utilities",
         "@fmt",
     ],
@@ -478,9 +479,13 @@ drake_cc_googletest(
 
 drake_cc_googletest(
     name = "hydroelastic_internal_test",
+    data = [
+        "//geometry:test_obj_files",
+    ],
     deps = [
         ":hydroelastic_internal",
         ":proximity_utilities",
+        "//common:find_resource",
         "//common/test_utilities:eigen_matrix_compare",
         "//common/test_utilities:expect_no_throw",
         "//common/test_utilities:expect_throws_message",

geometry/proximity/hydroelastic_internal.cc

@@ -13,6 +13,7 @@
 #include "drake/geometry/proximity/make_ellipsoid_mesh.h"
 #include "drake/geometry/proximity/make_sphere_field.h"
 #include "drake/geometry/proximity/make_sphere_mesh.h"
+#include "drake/geometry/proximity/obj_to_surface_mesh.h"
 #include "drake/geometry/proximity/volume_to_surface_mesh.h"
 
 namespace drake {
@@ -276,6 +277,15 @@ std::optional<RigidGeometry> MakeRigidRepresentation(
   return RigidGeometry(move(mesh));
 }
 
+std::optional<RigidGeometry> MakeRigidRepresentation(
+    const Mesh& mesh_spec, const ProximityProperties&) {
+  // Mesh does not use any properties.
+  auto mesh = make_unique<SurfaceMesh<double>>(
+      ReadObjToSurfaceMesh(mesh_spec.filename(), mesh_spec.scale()));
+
+  return RigidGeometry(move(mesh));
+}
+
 std::optional<SoftGeometry> MakeSoftRepresentation(
     const Sphere& sphere, const ProximityProperties& props) {
   PositiveDouble validator("Sphere", "soft");

geometry/proximity/hydroelastic_internal.h

@@ -294,6 +294,10 @@ std::optional<RigidGeometry> MakeRigidRepresentation(
 std::optional<RigidGeometry> MakeRigidRepresentation(
     const Ellipsoid& ellipsoid, const ProximityProperties& props);
 
+/** Rigid mesh support. It doesn't depend on any of the proximity properties. */
+std::optional<RigidGeometry> MakeRigidRepresentation(
+     const Mesh& mesh, const ProximityProperties& props);
+
 /** Generic interface for handling unsupported soft Shapes. Unsupported
  geometries will return a std::nullopt.  */
 template <typename Shape>

geometry/proximity/surface_mesh.cc

@@ -1,8 +1,13 @@
 #include "drake/geometry/proximity/surface_mesh.h"
 
+#include "drake/common/default_scalars.h"
+
 namespace drake {
 namespace geometry {
 
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class SurfaceMesh)
+
 }  // namespace geometry
 }  // namespace drake
 

geometry/proximity/surface_mesh.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <array>
+#include <limits>
 #include <set>
 #include <utility>
 #include <vector>
@@ -355,6 +356,29 @@ class SurfaceMesh {
   // Optimization: save n, and the inverse of matrix |uᵢ.dot(uⱼ)| for later.
   //
 
+  // TODO(DamrongGuoy): Consider sharing this code with
+  //  bounding_volume_hierarchy.h. Currently we have a problem that
+  //  SurfaceMesh and its vertices are templated on T, but Aabb in
+  //  bounding_volume_hierarchy.h is for double only.
+  /**
+   Calculates the axis-aligned bounding box of this surface mesh M.
+   @returns the center and the size vector of the box expressed in M's frame.
+   */
+  std::pair<Vector3<T>, Vector3<T>> CalcBoundingBox() const {
+    Vector3<T> min_extent =
+        Vector3<T>::Constant(std::numeric_limits<double>::max());
+    Vector3<T> max_extent =
+        Vector3<T>::Constant(std::numeric_limits<double>::lowest());
+    for (SurfaceVertexIndex i(0); i < num_vertices(); ++i) {
+      Vector3<T> vertex = this->vertex(i).r_MV();
+      min_extent = min_extent.cwiseMin(vertex);
+      max_extent = max_extent.cwiseMax(vertex);
+    }
+    Vector3<T> center = (max_extent + min_extent) / 2.0;
+    Vector3<T> size = max_extent - min_extent;
+    return std::make_pair(center, size);
+  }
+
   // TODO(#12173): Consider NaN==NaN to be true in equality tests.
   /** Checks to see whether the given SurfaceMesh object is equal via deep
    exact comparison. NaNs are treated as not equal as per the IEEE standard.
@@ -442,6 +466,9 @@ void SurfaceMesh<T>::CalcAreasNormalsAndCentroid() {
     p_MSc_ /= (3. * total_area_);
 }
 
+DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class SurfaceMesh)
+
 }  // namespace geometry
 }  // namespace drake
 

geometry/proximity/test/hydroelastic_internal_test.cc

@@ -6,6 +6,7 @@
 #include <fmt/format.h>
 #include <gtest/gtest.h>
 
+#include "drake/common/find_resource.h"
 #include "drake/common/test_utilities/expect_no_throw.h"
 #include "drake/common/test_utilities/expect_throws_message.h"
 #include "drake/geometry/proximity_properties.h"
@@ -198,8 +199,6 @@ TEST_F(HydroelasticRigidGeometryTest, UnsupportedRigidShapes) {
   // Note: the file name doesn't have to be valid for this (and the Mesh) test.
   const std::string obj = "drake/geometry/proximity/test/no_such_files.obj";
   EXPECT_EQ(MakeRigidRepresentation(Convex(obj, 1.0), props), std::nullopt);
-
-  EXPECT_EQ(MakeRigidRepresentation(Mesh(obj, 1.0), props), std::nullopt);
 }
 
 // Confirm support for a rigid Sphere. Tests that a hydroelastic representation
@@ -307,6 +306,27 @@ TEST_F(HydroelasticRigidGeometryTest, Ellipsoid) {
   }
 }
 
+// Confirm support for a rigid Mesh. Tests that a hydroelastic representation
+// is made.
+TEST_F(HydroelasticRigidGeometryTest, Mesh) {
+  std::string file =
+    FindResourceOrThrow("drake/geometry/test/non_convex_mesh.obj");
+  const double scale = 1.1;
+  // Empty props since its contents do not matter.
+  ProximityProperties props;
+
+  std::optional<RigidGeometry> mesh_rigid_geometry =
+      MakeRigidRepresentation(Mesh(file, scale), props);
+  EXPECT_NE(mesh_rigid_geometry, std::nullopt);
+
+  // We only check that the obj file was read by verifying the number of
+  // vertices and triangles, which depend on the specific content of
+  // the obj file.
+  const SurfaceMesh<double>& surface_mesh = mesh_rigid_geometry->mesh();
+  EXPECT_EQ(surface_mesh.num_vertices(), 5);
+  EXPECT_EQ(surface_mesh.num_faces(), 6);
+}
+
 // Template magic to instantiate a particular kind of shape at compile time.
 template <typename ShapeType>
 ShapeType make_default_shape() {
@@ -325,6 +345,16 @@ Box make_default_shape<Box>() {
   return Box(0.5, 1.25, 3.5);
 }
 
+template <>
+Cylinder make_default_shape<Cylinder>() {
+  return Cylinder(0.5, 1.25);
+}
+
+template <>
+Ellipsoid make_default_shape<Ellipsoid>() {
+  return Ellipsoid(0.5, 0.8, 0.3);
+}
+
 // Boilerplate for testing error conditions relating to properties. Its purpose
 // is to test that the `Make*Representation` (either "Rigid" or "Soft")
 // family of functions correctly validate all required properties. A property
@@ -407,7 +437,7 @@ void TestPropertyErrors(
 // Test suite for testing the common failure conditions for generating rigid
 // geometry. Specifically, they just need to be tessellated into a triangle mesh
 // and, therefore, only depend on the (hydroelastic, characteristic_length)
-// value. This actively excludes half spaces because they don't get
+// value. This actively excludes HalfSpace and Mesh because they don't get
 // tessellated (see the `RigidErrorShapeTypes` declaration below.) It should
 // include every *other* supported rigid shape type.
 template <typename ShapeType>
@@ -429,7 +459,7 @@ TYPED_TEST_P(HydroelasticRigidGeometryErrorTests, BadCharacteristicLength) {
 
 REGISTER_TYPED_TEST_SUITE_P(HydroelasticRigidGeometryErrorTests,
                            BadCharacteristicLength);
-typedef ::testing::Types<Sphere, Box> RigidErrorShapeTypes;
+typedef ::testing::Types<Sphere, Cylinder, Box, Ellipsoid> RigidErrorShapeTypes;
 INSTANTIATE_TYPED_TEST_SUITE_P(My, HydroelasticRigidGeometryErrorTests,
                               RigidErrorShapeTypes);
 
@@ -594,10 +624,10 @@ TEST_F(HydroelasticSoftGeometryTest, Ellipsoid) {
 
 // Test suite for testing the common failure conditions for generating soft
 // geometry. Specifically, they need to be tessellated into a tet mesh
-// and define a pressure field. This actively excludes half spaces because they
-// are treated specially (they don't get tessellated).
-// (See the `RigidErrorShapeTypes` declaration below.) It should include every
-// *other* supported rigid shape type.
+// and define a pressure field. This actively excludes HalfSpace and Mesh
+// because they are treated specially (they don't get tessellated).
+// (See the `SoftErrorShapeTypes` declaration below.) It should include every
+// *other* supported soft shape type.
 template <typename ShapeType>
 class HydroelasticSoftGeometryErrorTests : public ::testing::Test {};
 
@@ -632,7 +662,7 @@ TYPED_TEST_P(HydroelasticSoftGeometryErrorTests, BadElasticModulus) {
 
 REGISTER_TYPED_TEST_SUITE_P(HydroelasticSoftGeometryErrorTests,
                            BadCharacteristicLength, BadElasticModulus);
-typedef ::testing::Types<Sphere> SoftErrorShapeTypes;
+typedef ::testing::Types<Sphere, Cylinder, Box, Ellipsoid> SoftErrorShapeTypes;
 INSTANTIATE_TYPED_TEST_SUITE_P(My, HydroelasticSoftGeometryErrorTests,
                               SoftErrorShapeTypes);
 

geometry/proximity/test/surface_mesh_test.cc

@@ -365,6 +365,13 @@ GTEST_TEST(SurfaceMeshTest, TestEqual) {
   EXPECT_FALSE(triangle_mesh->Equal(*empty_mesh));
 }
 
+GTEST_TEST(SurfaceMeshTest, CalcBoundingBox) {
+  auto mesh = TestSurfaceMesh<double>();
+  const auto[center, size] = mesh->CalcBoundingBox();
+  EXPECT_EQ(center, Vector3d(7.5, 7.5, 0));
+  EXPECT_EQ(size, Vector3d(15, 15, 0));
+}
+
 }  // namespace
 }  // namespace geometry
 }  // namespace drake