From 7b0aba4d57e63afc67e74baab49146016f5d1528 Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Fri, 3 Jan 2025 15:04:30 -0500 Subject: [PATCH 01/11] Add functionality to create Convex shapes from vertices --- geometry/optimization/test/vpolytope_test.cc | 16 ++++++++++++ geometry/optimization/vpolytope.cc | 5 ++++ geometry/optimization/vpolytope.h | 4 +++ geometry/shape_specification.cc | 22 ++++++++++++++++ geometry/shape_specification.h | 18 +++++++++++++ geometry/test/shape_specification_test.cc | 27 ++++++++++++++++++++ 6 files changed, 92 insertions(+) diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc index 384b0382892b..759cdd5eb8c9 100644 --- a/geometry/optimization/test/vpolytope_test.cc +++ b/geometry/optimization/test/vpolytope_test.cc @@ -294,6 +294,22 @@ GTEST_TEST(VPolytopeTest, NonconvexMesh) { EXPECT_TRUE(V.PointInSet(V.MaybeGetFeasiblePoint().value())); } +GTEST_TEST(VPolytopeTest, ToShapeConvex) { + const int num_vertices = 4; + Eigen::Matrix 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(); + + auto vertices = convex.GetConvexHull().num_vertices(); + + EXPECT_EQ(vertices.cols(), num_vertices); +} + GTEST_TEST(VPolytopeTest, UnitBox6DTest) { VPolytope V = VPolytope::MakeUnitBox(6); EXPECT_EQ(V.ambient_dimension(), 6); diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc index 497156d74905..64860529e5e9 100644 --- a/geometry/optimization/vpolytope.cc +++ b/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 VPolytope::DoClone() const { return std::make_unique(*this); } diff --git a/geometry/optimization/vpolytope.h b/geometry/optimization/vpolytope.h index c416a0ad0fae..8c854225c029 100644 --- a/geometry/optimization/vpolytope.h +++ b/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; diff --git a/geometry/shape_specification.cc b/geometry/shape_specification.cc index f690a486effa..801d84a73c1a 100644 --- a/geometry/shape_specification.cc +++ b/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& 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& 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(); diff --git a/geometry/shape_specification.h b/geometry/shape_specification.h index ead489b74ad4..a12738bfceba 100644 --- a/geometry/shape_specification.h +++ b/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& vertices, + const std::string& filename_hint = {}, double scale = 1.0); + ~Convex() final; /** Returns the source for this specification's mesh data. When working with diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index db279d4ced47..e62fede4d27e 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -662,6 +662,33 @@ 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); + + EXPECT_THROW(convex.filename(), std::exception); + + const PolygonSurfaceMesh& 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"; From ed4c4143d4ccd9669f29649c3377a864f2d946b8 Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Fri, 3 Jan 2025 15:25:38 -0500 Subject: [PATCH 02/11] Update vpolytope_test.cc Fix the vertex test --- geometry/optimization/test/vpolytope_test.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc index 759cdd5eb8c9..ae33338af0f6 100644 --- a/geometry/optimization/test/vpolytope_test.cc +++ b/geometry/optimization/test/vpolytope_test.cc @@ -305,7 +305,7 @@ GTEST_TEST(VPolytopeTest, ToShapeConvex) { VPolytope V(vertices); const Convex convex = V.ToShapeConvex(); - auto vertices = convex.GetConvexHull().num_vertices(); + auto num_vertices = convex.GetConvexHull().num_vertices(); EXPECT_EQ(vertices.cols(), num_vertices); } From 4aad53c61a516a9eaa5f469db7b4012f96d04b0a Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Fri, 3 Jan 2025 16:25:00 -0500 Subject: [PATCH 03/11] Fix the VPolytope::ToShapeConvex test --- geometry/optimization/test/vpolytope_test.cc | 5 ++--- geometry/test/shape_specification_test.cc | 2 -- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc index ae33338af0f6..9a7e895dd30e 100644 --- a/geometry/optimization/test/vpolytope_test.cc +++ b/geometry/optimization/test/vpolytope_test.cc @@ -295,7 +295,6 @@ GTEST_TEST(VPolytopeTest, NonconvexMesh) { } GTEST_TEST(VPolytopeTest, ToShapeConvex) { - const int num_vertices = 4; Eigen::Matrix vertices; vertices.col(0) << 0, 0, 0; vertices.col(1) << 1, 0, 0; @@ -305,9 +304,9 @@ GTEST_TEST(VPolytopeTest, ToShapeConvex) { VPolytope V(vertices); const Convex convex = V.ToShapeConvex(); - auto num_vertices = convex.GetConvexHull().num_vertices(); + int num_vertices_of_convex = convex.GetConvexHull().num_vertices(); - EXPECT_EQ(vertices.cols(), num_vertices); + EXPECT_EQ(vertices.cols(), num_vertices_of_convex); } GTEST_TEST(VPolytopeTest, UnitBox6DTest) { diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index e62fede4d27e..64ac9389e10f 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -682,8 +682,6 @@ GTEST_TEST(ShapeTest, ConvexFromVertices) { ASSERT_TRUE(source.is_in_memory()); EXPECT_EQ(source.in_memory().mesh_file.filename_hint(), mesh_name); - EXPECT_THROW(convex.filename(), std::exception); - const PolygonSurfaceMesh& hull = convex.GetConvexHull(); EXPECT_EQ(hull.num_vertices(), 8); EXPECT_EQ(hull.num_elements(), 6); From 690a4bcc672e1929a240b78feb8b673fcca93cc7 Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Mon, 6 Jan 2025 14:13:04 -0500 Subject: [PATCH 04/11] Remove statements that the points used are vertices --- geometry/shape_specification.cc | 23 +++++++------ geometry/shape_specification.h | 19 +++++----- geometry/test/shape_specification_test.cc | 42 ++++++++++++++--------- 3 files changed, 47 insertions(+), 37 deletions(-) diff --git a/geometry/shape_specification.cc b/geometry/shape_specification.cc index 801d84a73c1a..579f8cc30128 100644 --- a/geometry/shape_specification.cc +++ b/geometry/shape_specification.cc @@ -19,13 +19,12 @@ #include "drake/geometry/proximity/triangle_surface_mesh.h" namespace { -std::string VerticesToString(const Eigen::Matrix3X& vertices) { +std::string PointsToObjString(const Eigen::Matrix3X& points) { 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)); + for (int i = 0; i < points.cols(); i++) { + result += + fmt::format("v {} {} {}\n", points(0, i), points(1, i), points(2, i)); } result += "\n"; @@ -147,12 +146,14 @@ Convex::Convex(MeshSource source, double scale) ThrowForBadScale(scale, "Convex"); } -Convex::Convex(const Eigen::Matrix3X& vertices, - const std::string& filename_hint, double scale) - : Convex(InMemoryMesh( - MemoryFile(VerticesToString(vertices), ".OBJ", filename_hint), - {}), - scale) {} +Convex::Convex(const Eigen::Matrix3X& points, + const std::string& convex_label, double scale) + : Convex( + InMemoryMesh{ + .mesh_file = + MemoryFile(PointsToObjString(points), ".obj", convex_label), + }, + scale) {} std::string Convex::filename() const { if (source_.is_path()) { diff --git a/geometry/shape_specification.h b/geometry/shape_specification.h index a12738bfceba..f0242ddf5fae 100644 --- a/geometry/shape_specification.h +++ b/geometry/shape_specification.h @@ -286,23 +286,22 @@ 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`. + /** Constructs an in-memory convex shape specification from the given points. - The convex hull is computed from the vertices provided. The vertices are + The convex hull is computed from the points provided. The points 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 points The points whose convex hull define the shape. + @param convex_label A label for the object. The label is used for warning + and error messages. Otherwise, the label has no other + functional purpose. It must consist of a single line. @param scale An optional scale to coordinates. - @throws std::exception if filename_hint contains newlines. + @throws std::exception if convex_label contains newlines. @throws std::exception if |scale| < 1e-8. */ - explicit Convex(const Eigen::Matrix3X& vertices, - const std::string& filename_hint = {}, double scale = 1.0); + explicit Convex(const Eigen::Matrix3X& points, + const std::string& convex_label = {}, double scale = 1.0); ~Convex() final; diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index 64ac9389e10f..c8b67031fd6a 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -664,27 +664,39 @@ GTEST_TEST(ShapeTest, ConvexFromMemory) { GTEST_TEST(ShapeTest, ConvexFromVertices) { // This will get normalized to ".obj". - Eigen::MatrixXd vertices(3, 8); + Eigen::Matrix vertices; 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; + vertices.col(2) << 0, 1, 0; + vertices.col(3) << 0, 0, 1; + const std::string mesh_name = "a_convex.obj"; - const Convex convex(vertices, mesh_name, 2.0); + const Convex convex_from_verts(vertices, mesh_name, 2.0); - EXPECT_EQ(convex.scale(), 2.0); - EXPECT_EQ(convex.extension(), ".obj"); - const MeshSource& source = convex.source(); + EXPECT_EQ(convex_from_verts.scale(), 2.0); + EXPECT_EQ(convex_from_verts.extension(), ".obj"); + const MeshSource& source = convex_from_verts.source(); ASSERT_TRUE(source.is_in_memory()); EXPECT_EQ(source.in_memory().mesh_file.filename_hint(), mesh_name); - const PolygonSurfaceMesh& hull = convex.GetConvexHull(); - EXPECT_EQ(hull.num_vertices(), 8); - EXPECT_EQ(hull.num_elements(), 6); + const PolygonSurfaceMesh& hull = convex_from_verts.GetConvexHull(); + EXPECT_EQ(hull.num_vertices(), 4); + EXPECT_EQ(hull.num_elements(), 4); + + // make sure the convex hull is automatically taken. + Eigen::Matrix points; + points.col(0) << 0, 0, 0; + points.col(1) << 1, 0, 0; + points.col(2) << 0, 1, 0; + points.col(3) << 0, 0, 1; + points.col(4) << 0.25, 0.25, 0.25; + + const std::string mesh_name2 = "a_convex2.obj"; + const Convex convex_from_points(points, mesh_name2, 2.0); + + const PolygonSurfaceMesh& hull2 = convex_from_points.GetConvexHull(); + EXPECT_EQ(hull2.num_vertices(), 4); + EXPECT_EQ(hull2.num_elements(), 4); } GTEST_TEST(ShapeTest, MeshFromMemory) { @@ -785,7 +797,6 @@ TEST_F(OverrideDefaultGeometryTest, UnsupportedGeometry) { EXPECT_NO_THROW(this->ImplementGeometry(Sphere(0.5), nullptr)); } - GTEST_TEST(ShapeTest, TypeNameAndToString) { // In-memory mesh we'll use on Convex and Mesh. const InMemoryMesh in_memory{ @@ -853,7 +864,6 @@ GTEST_TEST(ShapeTest, TypeNameAndToString) { EXPECT_EQ(fmt::to_string(base), "Box(width=1.5, depth=2.5, height=3.5)"); } - GTEST_TEST(ShapeTest, Volume) { EXPECT_NEAR(CalcVolume(Box(1, 2, 3)), 6.0, 1e-14); EXPECT_NEAR(CalcVolume(Capsule(1.23, 2.4)), From d7431fe3da11c4023689e4cea611878834b32fef Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Mon, 6 Jan 2025 15:12:06 -0500 Subject: [PATCH 05/11] Merge tests for ConvexFromVertices --- geometry/shape_specification.h | 2 +- geometry/test/shape_specification_test.cc | 44 ++++++++--------------- 2 files changed, 16 insertions(+), 30 deletions(-) diff --git a/geometry/shape_specification.h b/geometry/shape_specification.h index f0242ddf5fae..0942b90addac 100644 --- a/geometry/shape_specification.h +++ b/geometry/shape_specification.h @@ -292,7 +292,7 @@ class Convex final : public Shape { expected to be in the canonical frame of the shape. Optionally uniformly scaled by the given scale factor. - @param points The points whose convex hull define the shape. + @param points The points whose convex hull define the shape. @param convex_label A label for the object. The label is used for warning and error messages. Otherwise, the label has no other functional purpose. It must consist of a single line. diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index c8b67031fd6a..294bdccf9c00 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -663,40 +663,26 @@ GTEST_TEST(ShapeTest, ConvexFromMemory) { } GTEST_TEST(ShapeTest, ConvexFromVertices) { - // This will get normalized to ".obj". - Eigen::Matrix 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; - - const std::string mesh_name = "a_convex.obj"; - const Convex convex_from_verts(vertices, mesh_name, 2.0); - - EXPECT_EQ(convex_from_verts.scale(), 2.0); - EXPECT_EQ(convex_from_verts.extension(), ".obj"); - const MeshSource& source = convex_from_verts.source(); - ASSERT_TRUE(source.is_in_memory()); - EXPECT_EQ(source.in_memory().mesh_file.filename_hint(), mesh_name); - - const PolygonSurfaceMesh& hull = convex_from_verts.GetConvexHull(); - EXPECT_EQ(hull.num_vertices(), 4); - EXPECT_EQ(hull.num_elements(), 4); - - // make sure the convex hull is automatically taken. + // Make sure the convex hull is automatically taken. Eigen::Matrix points; points.col(0) << 0, 0, 0; points.col(1) << 1, 0, 0; - points.col(2) << 0, 1, 0; - points.col(3) << 0, 0, 1; - points.col(4) << 0.25, 0.25, 0.25; + points.col(2) << 0.25, 0.25, 0.25; + points.col(3) << 0, 1, 0; + points.col(4) << 0, 0, 1; + + const std::string mesh_name = "a_convex"; + const Convex convex(points, mesh_name, 2.0); - const std::string mesh_name2 = "a_convex2.obj"; - const Convex convex_from_points(points, mesh_name2, 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& hull2 = convex_from_points.GetConvexHull(); - EXPECT_EQ(hull2.num_vertices(), 4); - EXPECT_EQ(hull2.num_elements(), 4); + const PolygonSurfaceMesh& hull = convex.GetConvexHull(); + EXPECT_EQ(hull.num_vertices(), 4); + EXPECT_EQ(hull.num_elements(), 4); } GTEST_TEST(ShapeTest, MeshFromMemory) { From 08e182907e67e6aae03aaaf80bc2c22dfae4fc99 Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Mon, 6 Jan 2025 17:41:35 -0500 Subject: [PATCH 06/11] Update the test to ensure that the convex hull of points is taken when creating Convex from points --- geometry/test/shape_specification_test.cc | 29 +++++++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index 294bdccf9c00..643a3cd8d276 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -671,10 +671,12 @@ GTEST_TEST(ShapeTest, ConvexFromVertices) { points.col(3) << 0, 1, 0; points.col(4) << 0, 0, 1; + const double scale = 2.0; + const std::string mesh_name = "a_convex"; - const Convex convex(points, mesh_name, 2.0); + const Convex convex(points, mesh_name, scale); - EXPECT_EQ(convex.scale(), 2.0); + EXPECT_EQ(convex.scale(), scale); EXPECT_EQ(convex.extension(), ".obj"); const MeshSource& source = convex.source(); ASSERT_TRUE(source.is_in_memory()); @@ -683,6 +685,29 @@ GTEST_TEST(ShapeTest, ConvexFromVertices) { const PolygonSurfaceMesh& hull = convex.GetConvexHull(); EXPECT_EQ(hull.num_vertices(), 4); EXPECT_EQ(hull.num_elements(), 4); + + // Confirm that the vertices of the convex hull are the expected ones. + Eigen::Matrix expected_hull_points; + expected_hull_points.col(0) << 0, 0, 0; + expected_hull_points.col(1) << 1, 0, 0; + expected_hull_points.col(2) << 0, 1, 0; + expected_hull_points.col(3) << 0, 0, 1; + + expected_hull_points *= scale; + + std::vector hull_points; + for (int i = 0; i < hull.num_vertices(); ++i) { + hull_points.push_back(hull.vertex(i)); + } + + for (int i = 0; i < hull.num_vertices(); ++i) { + EXPECT_TRUE( + std::find_if(hull_points.begin(), hull_points.end(), + [&expected_hull_points, &i](const Eigen::Vector3d& v) { + return CompareMatrices(v, expected_hull_points.col(i), + 1e-14); + }) != hull_points.end()); + } } GTEST_TEST(ShapeTest, MeshFromMemory) { From 45c7e37ad9fe0abc54907702d5c6dfb0f96cef67 Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Mon, 6 Jan 2025 19:56:03 -0500 Subject: [PATCH 07/11] Test the contents of the in-memory file --- geometry/shape_specification.cc | 1 - geometry/test/shape_specification_test.cc | 27 +++++------------------ 2 files changed, 5 insertions(+), 23 deletions(-) diff --git a/geometry/shape_specification.cc b/geometry/shape_specification.cc index 579f8cc30128..863ead01b87b 100644 --- a/geometry/shape_specification.cc +++ b/geometry/shape_specification.cc @@ -26,7 +26,6 @@ std::string PointsToObjString(const Eigen::Matrix3X& points) { result += fmt::format("v {} {} {}\n", points(0, i), points(1, i), points(2, i)); } - result += "\n"; return result; } diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index 643a3cd8d276..3d60bf3574df 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -686,28 +686,11 @@ GTEST_TEST(ShapeTest, ConvexFromVertices) { EXPECT_EQ(hull.num_vertices(), 4); EXPECT_EQ(hull.num_elements(), 4); - // Confirm that the vertices of the convex hull are the expected ones. - Eigen::Matrix expected_hull_points; - expected_hull_points.col(0) << 0, 0, 0; - expected_hull_points.col(1) << 1, 0, 0; - expected_hull_points.col(2) << 0, 1, 0; - expected_hull_points.col(3) << 0, 0, 1; - - expected_hull_points *= scale; - - std::vector hull_points; - for (int i = 0; i < hull.num_vertices(); ++i) { - hull_points.push_back(hull.vertex(i)); - } - - for (int i = 0; i < hull.num_vertices(); ++i) { - EXPECT_TRUE( - std::find_if(hull_points.begin(), hull_points.end(), - [&expected_hull_points, &i](const Eigen::Vector3d& v) { - return CompareMatrices(v, expected_hull_points.col(i), - 1e-14); - }) != hull_points.end()); - } + // Confirm that the cntents of the in-memory mesh file are as expected. + EXPECT_EQ(source.in_memory().mesh_file.contents(), + std::string("v 0 0 0\n") + std::string("v 1 0 0\n") + + std::string("v 0.25 0.25 0.25\n") + std::string("v 0 1 0\n") + + std::string("v 0 0 1\n")); } GTEST_TEST(ShapeTest, MeshFromMemory) { From a5936bbe2b9d541d8d764a03a47baf13694b5eeb Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Tue, 7 Jan 2025 17:37:13 -0500 Subject: [PATCH 08/11] Update ConvexFromVertices shape specification test --- geometry/test/shape_specification_test.cc | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index 3d60bf3574df..420b1bc256da 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -686,11 +686,16 @@ GTEST_TEST(ShapeTest, ConvexFromVertices) { EXPECT_EQ(hull.num_vertices(), 4); EXPECT_EQ(hull.num_elements(), 4); - // Confirm that the cntents of the in-memory mesh file are as expected. - EXPECT_EQ(source.in_memory().mesh_file.contents(), - std::string("v 0 0 0\n") + std::string("v 1 0 0\n") + - std::string("v 0.25 0.25 0.25\n") + std::string("v 0 1 0\n") + - std::string("v 0 0 1\n")); + // Confirm that the contents of the in-memory mesh file are as expected. Note: + // We prefix a "\n" to match leading "\n" in the nicely readable raw string. + EXPECT_EQ("\n" + source.in_memory().mesh_file.contents(), + R"""( +v 0 0 0 +v 1 0 0 +v 0.25 0.25 0.25 +v 0 1 0 +v 0 0 1 +)"""); } GTEST_TEST(ShapeTest, MeshFromMemory) { From 17b789e32d126328a3752de2ed4a158c3b05f5ea Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Wed, 8 Jan 2025 19:49:18 -0500 Subject: [PATCH 09/11] Address comments --- geometry/optimization/test/vpolytope_test.cc | 2 +- geometry/shape_specification.cc | 16 +++++++--------- geometry/shape_specification.h | 14 +++++++------- geometry/test/shape_specification_test.cc | 6 ------ 4 files changed, 15 insertions(+), 23 deletions(-) diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc index 9a7e895dd30e..288857a4190b 100644 --- a/geometry/optimization/test/vpolytope_test.cc +++ b/geometry/optimization/test/vpolytope_test.cc @@ -301,7 +301,7 @@ GTEST_TEST(VPolytopeTest, ToShapeConvex) { vertices.col(2) << 0, 1, 0; vertices.col(3) << 0, 0, 1; - VPolytope V(vertices); + const VPolytope V(vertices); const Convex convex = V.ToShapeConvex(); int num_vertices_of_convex = convex.GetConvexHull().num_vertices(); diff --git a/geometry/shape_specification.cc b/geometry/shape_specification.cc index 863ead01b87b..ca77af9b58a4 100644 --- a/geometry/shape_specification.cc +++ b/geometry/shape_specification.cc @@ -22,7 +22,7 @@ namespace { std::string PointsToObjString(const Eigen::Matrix3X& points) { std::string result = ""; - for (int i = 0; i < points.cols(); i++) { + for (int i = 0; i < points.cols(); ++i) { result += fmt::format("v {} {} {}\n", points(0, i), points(1, i), points(2, i)); } @@ -145,14 +145,12 @@ Convex::Convex(MeshSource source, double scale) ThrowForBadScale(scale, "Convex"); } -Convex::Convex(const Eigen::Matrix3X& points, - const std::string& convex_label, double scale) - : Convex( - InMemoryMesh{ - .mesh_file = - MemoryFile(PointsToObjString(points), ".obj", convex_label), - }, - scale) {} +Convex::Convex(const Eigen::Matrix3X& points, const std::string& label, + double scale) + : Convex(InMemoryMesh{.mesh_file = MemoryFile( + PointsToObjString(points), ".obj", + label.empty() ? "convex-from-points" : label)}, + scale) {} std::string Convex::filename() const { if (source_.is_path()) { diff --git a/geometry/shape_specification.h b/geometry/shape_specification.h index 0942b90addac..eaf70be35813 100644 --- a/geometry/shape_specification.h +++ b/geometry/shape_specification.h @@ -292,16 +292,16 @@ class Convex final : public Shape { expected to be in the canonical frame of the shape. Optionally uniformly scaled by the given scale factor. - @param points The points whose convex hull define the shape. - @param convex_label A label for the object. The label is used for warning - and error messages. Otherwise, the label has no other - functional purpose. It must consist of a single line. - @param scale An optional scale to coordinates. + @param points The points whose convex hull define the shape. + @param label A label for the object. The label is used for warning and + error messages. Otherwise, the label has no other functional + purpose. It must consist of a single line. + @param scale An optional scale to coordinates. - @throws std::exception if convex_label contains newlines. + @throws std::exception if label contains newlines. @throws std::exception if |scale| < 1e-8. */ explicit Convex(const Eigen::Matrix3X& points, - const std::string& convex_label = {}, double scale = 1.0); + const std::string& label = {}, double scale = 1.0); ~Convex() final; diff --git a/geometry/test/shape_specification_test.cc b/geometry/test/shape_specification_test.cc index 420b1bc256da..525ef6d0f9d7 100644 --- a/geometry/test/shape_specification_test.cc +++ b/geometry/test/shape_specification_test.cc @@ -677,15 +677,9 @@ GTEST_TEST(ShapeTest, ConvexFromVertices) { const Convex convex(points, mesh_name, scale); EXPECT_EQ(convex.scale(), scale); - 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& hull = convex.GetConvexHull(); - EXPECT_EQ(hull.num_vertices(), 4); - EXPECT_EQ(hull.num_elements(), 4); - // Confirm that the contents of the in-memory mesh file are as expected. Note: // We prefix a "\n" to match leading "\n" in the nicely readable raw string. EXPECT_EQ("\n" + source.in_memory().mesh_file.contents(), From 482c2a4c00a00da11d29093ba975aaa48fa18862 Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Thu, 9 Jan 2025 10:05:35 -0500 Subject: [PATCH 10/11] Removing default option for label specification --- geometry/shape_specification.cc | 5 ++--- geometry/shape_specification.h | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/geometry/shape_specification.cc b/geometry/shape_specification.cc index ca77af9b58a4..b64cc5a476c7 100644 --- a/geometry/shape_specification.cc +++ b/geometry/shape_specification.cc @@ -147,9 +147,8 @@ Convex::Convex(MeshSource source, double scale) Convex::Convex(const Eigen::Matrix3X& points, const std::string& label, double scale) - : Convex(InMemoryMesh{.mesh_file = MemoryFile( - PointsToObjString(points), ".obj", - label.empty() ? "convex-from-points" : label)}, + : Convex(InMemoryMesh{.mesh_file = MemoryFile(PointsToObjString(points), + ".obj", label)}, scale) {} std::string Convex::filename() const { diff --git a/geometry/shape_specification.h b/geometry/shape_specification.h index eaf70be35813..3f73e03ba887 100644 --- a/geometry/shape_specification.h +++ b/geometry/shape_specification.h @@ -301,7 +301,7 @@ class Convex final : public Shape { @throws std::exception if label contains newlines. @throws std::exception if |scale| < 1e-8. */ explicit Convex(const Eigen::Matrix3X& points, - const std::string& label = {}, double scale = 1.0); + const std::string& label, double scale = 1.0); ~Convex() final; From 6a482273f1f93e6613c7fe2535cce56a9fae7f31 Mon Sep 17 00:00:00 2001 From: Akshay5312 <40531389+Akshay5312@users.noreply.github.com> Date: Thu, 9 Jan 2025 12:21:33 -0500 Subject: [PATCH 11/11] Add a convex_label when generating a Convex from a VPolytope --- geometry/optimization/test/vpolytope_test.cc | 7 ++++++- geometry/optimization/vpolytope.cc | 4 ++-- geometry/optimization/vpolytope.h | 7 +++++-- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc index 288857a4190b..85de5cd9145f 100644 --- a/geometry/optimization/test/vpolytope_test.cc +++ b/geometry/optimization/test/vpolytope_test.cc @@ -1,6 +1,7 @@ #include "drake/geometry/optimization/vpolytope.h" #include +#include #include @@ -301,12 +302,16 @@ GTEST_TEST(VPolytopeTest, ToShapeConvex) { vertices.col(2) << 0, 1, 0; vertices.col(3) << 0, 0, 1; + const std::string convex_label = "a_convex"; + const VPolytope V(vertices); - const Convex convex = V.ToShapeConvex(); + const Convex convex = V.ToShapeConvex(convex_label); int num_vertices_of_convex = convex.GetConvexHull().num_vertices(); EXPECT_EQ(vertices.cols(), num_vertices_of_convex); + EXPECT_EQ(convex.source().in_memory().mesh_file.filename_hint(), + convex_label); } GTEST_TEST(VPolytopeTest, UnitBox6DTest) { diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc index 64860529e5e9..af2a1d546684 100644 --- a/geometry/optimization/vpolytope.cc +++ b/geometry/optimization/vpolytope.cc @@ -492,9 +492,9 @@ void VPolytope::WriteObj(const std::filesystem::path& filename) const { file.close(); } -Convex VPolytope::ToShapeConvex() const { +Convex VPolytope::ToShapeConvex(const std::string& convex_label) const { DRAKE_THROW_UNLESS(ambient_dimension() == 3); - return Convex(vertices_); + return Convex(vertices_, convex_label); } std::unique_ptr VPolytope::DoClone() const { diff --git a/geometry/optimization/vpolytope.h b/geometry/optimization/vpolytope.h index 8c854225c029..287f4f6f7146 100644 --- a/geometry/optimization/vpolytope.h +++ b/geometry/optimization/vpolytope.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -98,9 +99,11 @@ 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. + /** Creates a geometry::Convex shape using the vertices of this VPolytope. The + convex_label is passed as the 'label' of the Convex object. @pre ambient_dimension() == 3. */ - Convex ToShapeConvex() const; + Convex ToShapeConvex( + const std::string& convex_label = "convex_from_vpolytope") const; /** Computes the volume of this V-Polytope. @note this function calls qhull to compute the volume. */