VPolytope and IrisInConfigurationSpace support Mesh geometry. (#19672)

Previously they supported Convex geometry; now we extend that support
to Mesh.

For VPolytope: The semantics of this class are already clear that the
set is defined by the convex hull of the points. We even had a test
case of passing a non-convex mesh as Convex to prove it. This PR
simply adds the natural support.

For IrisInConfigurationSpace: Since IRIS is attempting to certify
non-collision, taking the convex hull of the mesh is on the correct
side of conservatism. Having this support will improve usability for
folks bringing their own geometry to IRIS.

Related to #19514.

geometry/optimization/iris.cc

@@ -191,6 +191,12 @@ class IrisConvexSetMaker final : public ShapeReifier {
     set = std::make_unique<VPolytope>(query_, geom_id_, reference_frame_);
   }
 
+  void ImplementGeometry(const Mesh&, void* data) {
+    DRAKE_DEMAND(geom_id_.is_valid());
+    auto& set = *static_cast<copyable_unique_ptr<ConvexSet>*>(data);
+    set = std::make_unique<VPolytope>(query_, geom_id_, reference_frame_);
+  }
+
  private:
   const QueryObject<double>& query_{};
   std::optional<FrameId> reference_frame_{};

geometry/optimization/test/iris_in_configuration_space_test.cc

@@ -132,8 +132,12 @@ const char boxes_with_mesh_urdf[] = R"""(
       <geometry><box size="1 1 1"/></geometry>
     </collision>
     <collision name="left">
-      <origin rpy="0 0 0" xyz="-2.5 0 0"/>
-      <geometry><box size="1 1 1"/></geometry>
+      <origin rpy="0 0 0" xyz="-3 0 0"/>
+      <geometry>
+        <!-- This mesh is equivalent to <box size="2 2 2"/> -->
+        <!-- Note: not declared convex -->
+        <mesh filename="package://box_model/meshes/box.obj"/>
+      </geometry>
     </collision>
   </link>
   <joint name="fixed_link_weld" type="fixed">
@@ -142,8 +146,8 @@ const char boxes_with_mesh_urdf[] = R"""(
   </joint>
   <link name="movable">
     <collision name="center">
-      <!-- box size="2 2 2" -->
       <geometry>
+        <!-- This mesh is equivalent to <box size="2 2 2"/> -->
         <mesh filename="package://box_model/meshes/box.obj">
           <drake:declare_convex/>
         </mesh>
@@ -161,6 +165,7 @@ const char boxes_with_mesh_urdf[] = R"""(
 
 // Three boxes.  Two on the outside are fixed.  One in the middle on a prismatic
 // joint.  The configuration space is a (convex) line segment q ∈ (−1,1).
+// This also tests mesh geometry (both Convex and Mesh).
 GTEST_TEST(IrisInConfigurationSpaceTest, BoxesWithMeshPrismatic) {
   const Vector1d sample = Vector1d::Zero();
   IrisOptions options;

geometry/optimization/test/vpolytope_test.cc

@@ -247,7 +247,7 @@ GTEST_TEST(VPolytopeTest, OctahedronTest) {
 
 GTEST_TEST(VPolytopeTest, NonconvexMesh) {
   auto [scene_graph, geom_id] = MakeSceneGraphWithShape(
-      Convex(FindResourceOrThrow("drake/geometry/test/non_convex_mesh.obj")),
+      Mesh(FindResourceOrThrow("drake/geometry/test/non_convex_mesh.obj")),
       RigidTransformd{});
   auto context = scene_graph->CreateDefaultContext();
   auto query =

geometry/optimization/vpolytope.cc

@@ -79,6 +79,35 @@ MatrixXd OrderCounterClockwise(const MatrixXd& vertices) {
   return sorted_vertices;
 }
 
+MatrixXd GetConvexHullFromObjFile(const std::string& filename, double scale) {
+  const auto [tinyobj_vertices, faces, num_faces] =
+      internal::ReadObjFile(filename, scale, /* triangulate = */ false);
+  unused(faces);
+  unused(num_faces);
+  orgQhull::Qhull qhull;
+  const int dim = 3;
+  std::vector<double> tinyobj_vertices_flat(tinyobj_vertices->size() * dim);
+  for (int i = 0; i < ssize(*tinyobj_vertices); ++i) {
+    for (int j = 0; j < dim; ++j) {
+      tinyobj_vertices_flat[dim * i + j] = (*tinyobj_vertices)[i](j);
+    }
+  }
+  qhull.runQhull("", dim, tinyobj_vertices->size(),
+                 tinyobj_vertices_flat.data(), "");
+  if (qhull.qhullStatus() != 0) {
+    throw std::runtime_error(
+        fmt::format("Qhull terminated with status {} and  message:\n{}",
+                    qhull.qhullStatus(), qhull.qhullMessage()));
+  }
+  Matrix3Xd vertices(3, qhull.vertexCount());
+  int vertex_count = 0;
+  for (const auto& qhull_vertex : qhull.vertexList()) {
+    vertices.col(vertex_count++) =
+        Eigen::Map<Vector3d>(qhull_vertex.point().toStdVector().data());
+  }
+  return vertices;
+}
+
 }  // namespace
 
 VPolytope::VPolytope() : VPolytope(MatrixXd(0, 0)) {}
@@ -442,36 +471,17 @@ void VPolytope::ImplementGeometry(const Box& box, void* data) {
 void VPolytope::ImplementGeometry(const Convex& convex, void* data) {
   DRAKE_ASSERT(data != nullptr);
   Matrix3Xd* vertex_data = static_cast<Matrix3Xd*>(data);
-  *vertex_data = GetVertices(convex);
+  *vertex_data = GetConvexHullFromObjFile(convex.filename(), convex.scale());
+}
+
+void VPolytope::ImplementGeometry(const Mesh& mesh, void* data) {
+  DRAKE_ASSERT(data != nullptr);
+  Matrix3Xd* vertex_data = static_cast<Matrix3Xd*>(data);
+  *vertex_data = GetConvexHullFromObjFile(mesh.filename(), mesh.scale());
 }
 
 MatrixXd GetVertices(const Convex& convex) {
-  const auto [tinyobj_vertices, faces, num_faces] = internal::ReadObjFile(
-      convex.filename(), convex.scale(), false /* triangulate */);
-  unused(faces);
-  unused(num_faces);
-  orgQhull::Qhull qhull;
-  const int dim = 3;
-  std::vector<double> tinyobj_vertices_flat(tinyobj_vertices->size() * dim);
-  for (int i = 0; i < ssize(*tinyobj_vertices); ++i) {
-    for (int j = 0; j < dim; ++j) {
-      tinyobj_vertices_flat[dim * i + j] = (*tinyobj_vertices)[i](j);
-    }
-  }
-  qhull.runQhull("", dim, tinyobj_vertices->size(),
-                 tinyobj_vertices_flat.data(), "");
-  if (qhull.qhullStatus() != 0) {
-    throw std::runtime_error(
-        fmt::format("Qhull terminated with status {} and  message:\n{}",
-                    qhull.qhullStatus(), qhull.qhullMessage()));
-  }
-  Matrix3Xd vertices(3, qhull.vertexCount());
-  int vertex_count = 0;
-  for (const auto& qhull_vertex : qhull.vertexList()) {
-    vertices.col(vertex_count++) =
-        Eigen::Map<Vector3d>(qhull_vertex.point().toStdVector().data());
-  }
-  return vertices;
+  return GetConvexHullFromObjFile(convex.filename(), convex.scale());
 }
 
 }  // namespace optimization

geometry/optimization/vpolytope.h

@@ -122,6 +122,7 @@ class VPolytope final : public ConvexSet {
   using ShapeReifier::ImplementGeometry;
   void ImplementGeometry(const Box& box, void* data) final;
   void ImplementGeometry(const Convex& convex, void* data) final;
+  void ImplementGeometry(const Mesh& mesh, void* data) final;
 
   Eigen::MatrixXd vertices_;
 };