Add functionality to create Convex shapes from vertices

geometry/optimization/test/vpolytope_test.cc

@@ -294,6 +294,21 @@ GTEST_TEST(VPolytopeTest, NonconvexMesh) {
   EXPECT_TRUE(V.PointInSet(V.MaybeGetFeasiblePoint().value()));
 }
 
+GTEST_TEST(VPolytopeTest, ToShapeConvex) {
+  Eigen::Matrix<double, 3, 4> vertices;
+  vertices.col(0) << 0, 0, 0;
+  vertices.col(1) << 1, 0, 0;
+  vertices.col(2) << 0, 1, 0;
+  vertices.col(3) << 0, 0, 1;
+
+  VPolytope V(vertices);
+  const Convex convex = V.ToShapeConvex();
+
+  int num_vertices_of_convex = convex.GetConvexHull().num_vertices();
+
+  EXPECT_EQ(vertices.cols(), num_vertices_of_convex);
+}
+
 GTEST_TEST(VPolytopeTest, UnitBox6DTest) {
   VPolytope V = VPolytope::MakeUnitBox(6);
   EXPECT_EQ(V.ambient_dimension(), 6);

geometry/optimization/vpolytope.cc

@@ -492,6 +492,11 @@ void VPolytope::WriteObj(const std::filesystem::path& filename) const {
   file.close();
 }
 
+Convex VPolytope::ToShapeConvex() const {
+  DRAKE_THROW_UNLESS(ambient_dimension() == 3);
+  return Convex(vertices_);
+}
+
 std::unique_ptr<ConvexSet> VPolytope::DoClone() const {
   return std::make_unique<VPolytope>(*this);
 }

geometry/optimization/vpolytope.h

@@ -98,6 +98,10 @@ class VPolytope final : public ConvexSet {
   @pre ambient_dimension() == 3. */
   void WriteObj(const std::filesystem::path& filename) const;
 
+  /** Creates a geometry::Convex shape using the vertices of this VPolytope.
+  @pre ambient_dimension() == 3. */
+  Convex ToShapeConvex() const;
+
   /** Computes the volume of this V-Polytope.
   @note this function calls qhull to compute the volume. */
   using ConvexSet::CalcVolume;

geometry/shape_specification.cc

@@ -18,6 +18,21 @@
 #include "drake/geometry/proximity/polygon_to_triangle_mesh.h"
 #include "drake/geometry/proximity/triangle_surface_mesh.h"
 
+namespace {
+std::string VerticesToString(const Eigen::Matrix3X<double>& vertices) {
+  std::string result = "";
+
+  result += fmt::format("# Vertices: {}\n", vertices.cols());
+  for (int i = 0; i < vertices.cols(); i++) {
+    result += fmt::format("v {} {} {}\n", vertices(0, i), vertices(1, i),
+                          vertices(2, i));
+  }
+  result += "\n";
+
+  return result;
+}
+}  // namespace
+
 namespace drake {
 namespace geometry {
 namespace {
@@ -132,6 +147,13 @@ Convex::Convex(MeshSource source, double scale)
   ThrowForBadScale(scale, "Convex");
 }
 
+Convex::Convex(const Eigen::Matrix3X<double>& vertices,
+               const std::string& filename_hint, double scale)
+    : Convex(InMemoryMesh(
+                 MemoryFile(VerticesToString(vertices), ".OBJ", filename_hint),
+                 {}),
+             scale) {}
+
 std::string Convex::filename() const {
   if (source_.is_path()) {
     return source_.path().string();

geometry/shape_specification.h

@@ -286,6 +286,24 @@ class Convex final : public Shape {
    @param scale    An optional scale to coordinates. */
   explicit Convex(MeshSource source, double scale = 1.0);
 
+  /** Constructs a convex shape specification from the given `vertices`.
+
+   The convex hull is computed from the vertices provided. The vertices are
+   expected to be in the canonical frame of the shape. Optionally uniformly
+   scaled by the given scale factor.
+
+   @param vertices      The vertices of the convex hull.
+   @param filename_hint A label for the file. The label is used for warning and
+                        error messages. Otherwise, the label has no other
+                        functional purpose. It need not be a valid file name,
+                        but must consist of a single line (no newlines).
+   @param scale         An optional scale to coordinates.
+
+   @throws std::exception       if filename_hint contains newlines.
+   @throws std::exception       if |scale| < 1e-8. */
+  explicit Convex(const Eigen::Matrix3X<double>& vertices,
+                  const std::string& filename_hint = {}, double scale = 1.0);
+
   ~Convex() final;
 
   /** Returns the source for this specification's mesh data. When working with

geometry/test/shape_specification_test.cc

@@ -662,6 +662,31 @@ GTEST_TEST(ShapeTest, ConvexFromMemory) {
   EXPECT_EQ(hull.num_elements(), 6);
 }
 
+GTEST_TEST(ShapeTest, ConvexFromVertices) {
+  // This will get normalized to ".obj".
+  Eigen::MatrixXd vertices(3, 8);
+  vertices.col(0) << 0, 0, 0;
+  vertices.col(1) << 1, 0, 0;
+  vertices.col(2) << 1, 1, 0;
+  vertices.col(3) << 0, 1, 0;
+  vertices.col(4) << 0, 0, 1;
+  vertices.col(5) << 1, 0, 1;
+  vertices.col(6) << 1, 1, 1;
+  vertices.col(7) << 0, 1, 1;
+  const std::string mesh_name = "a_convex.obj";
+  const Convex convex(vertices, mesh_name, 2.0);
+
+  EXPECT_EQ(convex.scale(), 2.0);
+  EXPECT_EQ(convex.extension(), ".obj");
+  const MeshSource& source = convex.source();
+  ASSERT_TRUE(source.is_in_memory());
+  EXPECT_EQ(source.in_memory().mesh_file.filename_hint(), mesh_name);
+
+  const PolygonSurfaceMesh<double>& hull = convex.GetConvexHull();
+  EXPECT_EQ(hull.num_vertices(), 8);
+  EXPECT_EQ(hull.num_elements(), 6);
+}
+
 GTEST_TEST(ShapeTest, MeshFromMemory) {
   // This will get normalized to ".obj".
   const std::string mesh_name = "a_mesh.OBJ";