VPolytope and IrisInConfigurationSpace support Mesh geometry. (RobotL…

…ocomotion#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 RobotLocomotion#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_;
 };