[proximity] Adds hydroelastic support for Convex shapes (RobotLocomot…

…ion#14178)

Declaring a Convex shape to be rigid, will now produce a hydroelastic
representation. It is treated the same as a more general Mesh.

Updated `quad_cube.obj` as it wasn't really a cube; one of the vertex
positions was *slightly* out of position.

bindings/pydrake/test/geometry_test.py

@@ -606,7 +606,7 @@ def test_read_obj_to_surface_mesh(self):
             [-1.000000, -1.000000,  1.000000],
             [-1.000000, -1.000000, -1.000000],
             [1.000000,  1.000000, -1.000000],
-            [1.000000,  1.000000,  1.000001],
+            [1.000000,  1.000000,  1.000000],
             [-1.000000,  1.000000,  1.000000],
             [-1.000000,  1.000000, -1.000000],
         ]

geometry/proximity/hydroelastic_internal.cc

@@ -238,6 +238,15 @@ std::optional<RigidGeometry> MakeRigidRepresentation(
   return RigidGeometry(RigidMesh(move(mesh)));
 }
 
+std::optional<RigidGeometry> MakeRigidRepresentation(
+    const Convex& convex_spec, const ProximityProperties&) {
+  // Convex does not use any properties.
+  auto mesh = make_unique<SurfaceMesh<double>>(
+      ReadObjToSurfaceMesh(convex_spec.filename(), convex_spec.scale()));
+
+  return RigidGeometry(RigidMesh(move(mesh)));
+}
+
 std::optional<SoftGeometry> MakeSoftRepresentation(
     const Sphere& sphere, const ProximityProperties& props) {
   PositiveDouble validator("Sphere", "soft");

geometry/proximity/hydroelastic_internal.h

@@ -402,6 +402,14 @@ std::optional<RigidGeometry> MakeRigidRepresentation(
 std::optional<RigidGeometry> MakeRigidRepresentation(
     const Mesh& mesh, const ProximityProperties& props);
 
+/* Rigid convex support. It doesn't depend on any of the proximity properties.
+ Note: the convexity of the mesh is *not* tested (and does not need to be).
+ The representation of a Convex mesh is the same as a general Mesh, so its
+ representation and functionality is indistinguishable, whether convex or not.
+ */
+std::optional<RigidGeometry> MakeRigidRepresentation(
+    const Convex& convex, const ProximityProperties& props);
+
 /* Rigid half space support.  */
 std::optional<RigidGeometry> MakeRigidRepresentation(
     const HalfSpace& half_space, const ProximityProperties& props);

geometry/proximity/test/hydroelastic_internal_test.cc

@@ -430,10 +430,6 @@ TEST_F(HydroelasticRigidGeometryTest, UnsupportedRigidShapes) {
   ProximityProperties props = rigid_properties();
 
   EXPECT_EQ(MakeRigidRepresentation(Capsule(1, 1), props), std::nullopt);
-
-  // 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);
 }
 
 // Confirm support for a rigid half space. Tests that a hydroelastic
@@ -494,9 +490,9 @@ TEST_F(HydroelasticRigidGeometryTest, Box) {
   EXPECT_EQ(mesh.num_vertices(), 8);
   // Because it is a cube centered at the origin, the distance from the origin
   // to each vertex should be sqrt(3) * edge_len / 2.
-  const double expecte_dist = std::sqrt(3) * edge_len / 2;
+  const double expected_dist = std::sqrt(3) * edge_len / 2;
   for (SurfaceVertexIndex v(0); v < mesh.num_vertices(); ++v) {
-    ASSERT_NEAR(mesh.vertex(v).r_MV().norm(), expecte_dist, 1e-15);
+    ASSERT_NEAR(mesh.vertex(v).r_MV().norm(), expected_dist, 1e-15);
   }
 }
 
@@ -564,26 +560,55 @@ TEST_F(HydroelasticRigidGeometryTest, Ellipsoid) {
   }
 }
 
+// Confirm that a mesh type (convex/mesh) has a rigid representation. We rely
+// on the fact that we're loading a unit cube (vertices one unit away from the
+// origin along each axis) to confirm that the correct mesh got loaded. We also
+// confirm that the scale factor is included in the rigid representation.
+template <typename MeshType>
+void TestRigidMeshType() {
+  std::string file = FindResourceOrThrow("drake/geometry/test/quad_cube.obj");
+  // Empty props since its contents do not matter.
+  ProximityProperties props;
+
+  constexpr double kEps = 2 * std::numeric_limits<double>::epsilon();
+
+  for (const double scale : {1.0, 5.1, 0.4}) {
+    std::optional<RigidGeometry> geometry =
+        MakeRigidRepresentation(MeshType(file, scale), props);
+    ASSERT_NE(geometry, std::nullopt);
+    ASSERT_FALSE(geometry->is_half_space());
+
+    // 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 = geometry->mesh();
+    EXPECT_EQ(surface_mesh.num_vertices(), 8);
+    EXPECT_EQ(surface_mesh.num_faces(), 12);
+
+    // The scale factor multiplies the measure of every vertex position, so
+    // the expected distance of the vertex to the origin should be:
+    // scale * sqrt(3) (because the original mesh was the unit sphere).
+    const double expected_dist = std::sqrt(3) * scale;
+    for (SurfaceVertexIndex v(0); v < surface_mesh.num_vertices(); ++v) {
+      const double dist = surface_mesh.vertex(v).r_MV().norm();
+      ASSERT_NEAR(dist, expected_dist, scale * kEps)
+          << "for scale: " << scale << " at vertex " << v;
+    }
+  }
+}
+
 // 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;
+  SCOPED_TRACE("Rigid Mesh");
+  TestRigidMeshType<Mesh>();
+}
 
-  std::optional<RigidGeometry> mesh_rigid_geometry =
-      MakeRigidRepresentation(Mesh(file, scale), props);
-  ASSERT_NE(mesh_rigid_geometry, std::nullopt);
-  ASSERT_FALSE(mesh_rigid_geometry->is_half_space());
-
-  // 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);
+// Confirm support for a rigid Convex. Tests that a hydroelastic representation
+// is made.
+TEST_F(HydroelasticRigidGeometryTest, Convex) {
+  SCOPED_TRACE("Rigid Convex");
+  TestRigidMeshType<Convex>();
 }
 
 // Template magic to instantiate a particular kind of shape at compile time.

geometry/proximity/test/obj_to_surface_mesh_test.cc

@@ -90,7 +90,7 @@ GTEST_TEST(ObjToSurfaceMeshTest, ReadObjToSurfaceMesh) {
       {-1.000000, -1.000000,  1.000000},
       {-1.000000, -1.000000, -1.000000},
       { 1.000000,  1.000000, -1.000000},
-      { 1.000000,  1.000000,  1.000001},
+      { 1.000000,  1.000000,  1.000000},
       {-1.000000,  1.000000,  1.000000},
       {-1.000000,  1.000000, -1.000000}
   };

geometry/test/proximity_engine_test.cc

@@ -175,7 +175,7 @@ GTEST_TEST(ProximityEngineTests, ProcessHydroelasticProperties) {
               HydroelasticType::kRigid);
   }
 
-  // Case: meshes.
+  // Case: rigid mesh.
   {
     Mesh mesh{
         drake::FindResourceOrThrow("drake/geometry/test/non_convex_mesh.obj"),
@@ -186,15 +186,15 @@ GTEST_TEST(ProximityEngineTests, ProcessHydroelasticProperties) {
               HydroelasticType::kRigid);
   }
 
-  // Case: rigid convex (unsupported)
+  // Case: rigid convex.
   {
     Convex convex{
         drake::FindResourceOrThrow("drake/geometry/test/quad_cube.obj"),
         edge_length};
     const GeometryId convex_id = GeometryId::get_new_id();
     engine.AddDynamicGeometry(convex, {}, convex_id, rigid_properties);
     EXPECT_EQ(ProximityEngineTester::hydroelastic_type(convex_id, engine),
-              HydroelasticType::kUndefined);
+              HydroelasticType::kRigid);
   }
 }
 

geometry/test/quad_cube.obj

@@ -5,7 +5,7 @@ v  1.000000 -1.000000  1.000000
 v -1.000000 -1.000000  1.000000
 v -1.000000 -1.000000 -1.000000
 v  1.000000  1.000000 -1.000000
-v  1.000000  1.000000  1.000001
+v  1.000000  1.000000  1.000000
 v -1.000000  1.000000  1.000000
 v -1.000000  1.000000 -1.000000
 vn 0.0000 -1.0000 0.0000