From 00713d1974d3998659de208c7bbd4508d407992f Mon Sep 17 00:00:00 2001 From: Tobia Marcucci Date: Sun, 24 Oct 2021 11:36:24 -0400 Subject: [PATCH 1/2] Checks that hpolyhedron is bounded to construct vpolytope and adds tests. --- geometry/optimization/test/vpolytope_test.cc | 107 ++++++++++++++++++- geometry/optimization/vpolytope.cc | 3 + 2 files changed, 109 insertions(+), 1 deletion(-) diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc index bd7688b9a210..3cedb2565ffa 100644 --- a/geometry/optimization/test/vpolytope_test.cc +++ b/geometry/optimization/test/vpolytope_test.cc @@ -18,7 +18,10 @@ namespace drake { namespace geometry { namespace optimization { +using Eigen::Matrix; +using Eigen::Vector2d; using Eigen::Vector3d; +using Eigen::Vector4d; using internal::CheckAddPointInSetConstraints; using internal::MakeSceneGraphWithShape; using math::RigidTransformd; @@ -155,7 +158,7 @@ GTEST_TEST(VPolytopeTest, UnitBox6DTest) { EXPECT_FALSE(V.PointInSet(out2_W, kTol)); } -GTEST_TEST(VPolytopeTest, FromHPolyhedronTest) { +GTEST_TEST(VPolytopeTest, FromHUnitBoxTest) { HPolyhedron H = HPolyhedron::MakeUnitBox(6); VPolytope V(H); EXPECT_EQ(V.ambient_dimension(), 6); @@ -172,6 +175,108 @@ GTEST_TEST(VPolytopeTest, FromHPolyhedronTest) { EXPECT_FALSE(V.PointInSet(out2_W, kTol)); } +GTEST_TEST(HPolyhedronTest, From2DHPolytopeTest) { + Matrix A; + Vector4d b; + A << 1, -1, // y ≥ x + 1, 0, // x ≤ 1 + 0, 1, // y ≤ 2 + -1, 0; // x ≥ 0 + b << 0, 1, 2, 0; + HPolyhedron H(A, b); + VPolytope V(H); + + // Vertices are (0,0), (1,1), (1,2), and (0,2). + const Vector2d in1_W{.01, .02}, in2_W{.99, 1.0}, in3_W{.99, 1.99}, + in4_W{.01, 1.99}, out1_W{-.01, -.01}, out2_W{1.01, .99}, + out3_W{1.01, 2.01}, out4_W{-.01, 2.01}; + + // drake::log()->info("{}", V.vertices()); + + // const double kTol = 1e-11; + // EXPECT_TRUE(V.PointInSet(in1_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in2_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in3_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in4_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out1_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out2_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out3_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out4_W, kTol)); +} + +GTEST_TEST(HPolyhedronTest, From3DHSimplexTest) { + Matrix A; + Vector4d b; + A << -1, 0, 0, + 0, -1, 0, + 0, 0, -1, + 1, 1, 1; + b << 0, 0, 0, 1; + HPolyhedron H(A, b); + VPolytope V(H); + + // Vertices are (0,0,0), (1,0,0), (0,1,0), and (0,0,1). + const Vector3d in1_W{.01, .01, .01}, in2_W{.9, .01, .01}, in3_W{.01, .9, .01}, + in4_W{.01, .01, .9}, in5_W{.33, .33, .33}, out1_W{-.01, -.01, -.01}, + out2_W{1., .01, .01}, out3_W{.01, 1., .01}, out4_W{.01, .01, 1.}, + out5_W{.34, .34, .34}; + + // const double kTol = 1e-11; + // EXPECT_TRUE(V.PointInSet(in1_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in2_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in3_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in4_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in5_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out1_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out2_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out3_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out4_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out5_W, kTol)); +} + +// Same as FromHPolytopeTest but with two redundant constraints. +GTEST_TEST(HPolyhedronTest, FromRedundantHPolytopeTest) { + Matrix A; + Vector6d b; + A << 1, -1, // y ≥ x + 1, 0, // x ≤ 1 + 0, 1, // y ≤ 2 + -1, 0, // x ≥ 0 + 1, 1, // x + y ≤ 3.1 (redundant) + -1, -1; // x + y ≥ - 0.1 (redundant) + b << 0, 1, 2, 0, 3.1, 0.1; + HPolyhedron H(A, b); + VPolytope V(H); + + const Vector2d in1_W{.01, .02}, in2_W{.99, 1.0}, in3_W{.99, 1.99}, + in4_W{.01, 1.99}, out1_W{-.01, -.01}, out2_W{1.01, .99}, + out3_W{1.01, 2.01}, out4_W{-.01, 2.01}; + + // const double kTol = 1e-11; + // EXPECT_TRUE(V.PointInSet(in1_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in2_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in3_W, kTol)); + // EXPECT_TRUE(V.PointInSet(in4_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out1_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out2_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out3_W, kTol)); + // EXPECT_FALSE(V.PointInSet(out4_W, kTol)); +} + +GTEST_TEST(HPolyhedronTest, FromUnboundedHPolytopeTest) { + Matrix A; + Vector4d b; + A << 1, -1, // y ≥ x + 1, 0, // x ≤ 1 + 0, 1, // y ≤ 2 + b << 0, 1, 2; + HPolyhedron H(A, b); + + // The following should trigger DRAKE_DEMAND(hpoly.IsBounded()) but I don't + // know how to test this in C++/Drake. + // VPolytope V(H); +} + GTEST_TEST(VPolytopeTest, CloneTest) { VPolytope V = VPolytope::MakeBox(Vector3d{-3, -4, -5}, Vector3d{6, 7, 8}); std::unique_ptr clone = V.Clone(); diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc index aa59ceadea50..bf0ee06bae39 100644 --- a/geometry/optimization/vpolytope.cc +++ b/geometry/optimization/vpolytope.cc @@ -44,6 +44,9 @@ VPolytope::VPolytope(const QueryObject& query_object, VPolytope::VPolytope(const HPolyhedron& hpoly) : ConvexSet(&ConvexSetCloner, hpoly.ambient_dimension()) { + + DRAKE_DEMAND(hpoly.IsBounded()); + Eigen::MatrixXd coeffs(hpoly.A().rows(), hpoly.A().cols() + 1); coeffs.leftCols(hpoly.A().cols()) = hpoly.A(); coeffs.col(hpoly.A().cols()) = -hpoly.b(); From 1952a1fe70a7bfe3574420476281a3a1ece0406f Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Wed, 8 Dec 2021 01:43:50 -0500 Subject: [PATCH 2/2] Fix hpolytope to vpolytope conversion exposed by unit tests --- geometry/optimization/BUILD.bazel | 1 + geometry/optimization/test/vpolytope_test.cc | 77 ++++++++++---------- geometry/optimization/vpolytope.cc | 22 ++++-- 3 files changed, 53 insertions(+), 47 deletions(-) diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 067f3c70cb99..d2b2e434478d 100644 --- a/geometry/optimization/BUILD.bazel +++ b/geometry/optimization/BUILD.bazel @@ -163,6 +163,7 @@ drake_cc_googletest( ":convex_set", ":test_utilities", "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", ], ) diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc index 3cedb2565ffa..9587fa10ff0d 100644 --- a/geometry/optimization/test/vpolytope_test.cc +++ b/geometry/optimization/test/vpolytope_test.cc @@ -6,6 +6,7 @@ #include "drake/common/eigen_types.h" #include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" #include "drake/geometry/geometry_frame.h" #include "drake/geometry/optimization/test_utilities.h" #include "drake/geometry/scene_graph.h" @@ -175,7 +176,7 @@ GTEST_TEST(VPolytopeTest, FromHUnitBoxTest) { EXPECT_FALSE(V.PointInSet(out2_W, kTol)); } -GTEST_TEST(HPolyhedronTest, From2DHPolytopeTest) { +GTEST_TEST(VPolytopeTest, From2DHPolytopeTest) { Matrix A; Vector4d b; A << 1, -1, // y ≥ x @@ -191,20 +192,18 @@ GTEST_TEST(HPolyhedronTest, From2DHPolytopeTest) { in4_W{.01, 1.99}, out1_W{-.01, -.01}, out2_W{1.01, .99}, out3_W{1.01, 2.01}, out4_W{-.01, 2.01}; - // drake::log()->info("{}", V.vertices()); - - // const double kTol = 1e-11; - // EXPECT_TRUE(V.PointInSet(in1_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in2_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in3_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in4_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out1_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out2_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out3_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out4_W, kTol)); + const double kTol = 1e-11; + EXPECT_TRUE(V.PointInSet(in1_W, kTol)); + EXPECT_TRUE(V.PointInSet(in2_W, kTol)); + EXPECT_TRUE(V.PointInSet(in3_W, kTol)); + EXPECT_TRUE(V.PointInSet(in4_W, kTol)); + EXPECT_FALSE(V.PointInSet(out1_W, kTol)); + EXPECT_FALSE(V.PointInSet(out2_W, kTol)); + EXPECT_FALSE(V.PointInSet(out3_W, kTol)); + EXPECT_FALSE(V.PointInSet(out4_W, kTol)); } -GTEST_TEST(HPolyhedronTest, From3DHSimplexTest) { +GTEST_TEST(VPolytopeTest, From3DHSimplexTest) { Matrix A; Vector4d b; A << -1, 0, 0, @@ -220,22 +219,22 @@ GTEST_TEST(HPolyhedronTest, From3DHSimplexTest) { in4_W{.01, .01, .9}, in5_W{.33, .33, .33}, out1_W{-.01, -.01, -.01}, out2_W{1., .01, .01}, out3_W{.01, 1., .01}, out4_W{.01, .01, 1.}, out5_W{.34, .34, .34}; - - // const double kTol = 1e-11; - // EXPECT_TRUE(V.PointInSet(in1_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in2_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in3_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in4_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in5_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out1_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out2_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out3_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out4_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out5_W, kTol)); + + const double kTol = 1e-11; + EXPECT_TRUE(V.PointInSet(in1_W, kTol)); + EXPECT_TRUE(V.PointInSet(in2_W, kTol)); + EXPECT_TRUE(V.PointInSet(in3_W, kTol)); + EXPECT_TRUE(V.PointInSet(in4_W, kTol)); + EXPECT_TRUE(V.PointInSet(in5_W, kTol)); + EXPECT_FALSE(V.PointInSet(out1_W, kTol)); + EXPECT_FALSE(V.PointInSet(out2_W, kTol)); + EXPECT_FALSE(V.PointInSet(out3_W, kTol)); + EXPECT_FALSE(V.PointInSet(out4_W, kTol)); + EXPECT_FALSE(V.PointInSet(out5_W, kTol)); } // Same as FromHPolytopeTest but with two redundant constraints. -GTEST_TEST(HPolyhedronTest, FromRedundantHPolytopeTest) { +GTEST_TEST(VPolytopeTest, FromRedundantHPolytopeTest) { Matrix A; Vector6d b; A << 1, -1, // y ≥ x @@ -251,19 +250,19 @@ GTEST_TEST(HPolyhedronTest, FromRedundantHPolytopeTest) { const Vector2d in1_W{.01, .02}, in2_W{.99, 1.0}, in3_W{.99, 1.99}, in4_W{.01, 1.99}, out1_W{-.01, -.01}, out2_W{1.01, .99}, out3_W{1.01, 2.01}, out4_W{-.01, 2.01}; - - // const double kTol = 1e-11; - // EXPECT_TRUE(V.PointInSet(in1_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in2_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in3_W, kTol)); - // EXPECT_TRUE(V.PointInSet(in4_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out1_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out2_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out3_W, kTol)); - // EXPECT_FALSE(V.PointInSet(out4_W, kTol)); + + const double kTol = 1e-11; + EXPECT_TRUE(V.PointInSet(in1_W, kTol)); + EXPECT_TRUE(V.PointInSet(in2_W, kTol)); + EXPECT_TRUE(V.PointInSet(in3_W, kTol)); + EXPECT_TRUE(V.PointInSet(in4_W, kTol)); + EXPECT_FALSE(V.PointInSet(out1_W, kTol)); + EXPECT_FALSE(V.PointInSet(out2_W, kTol)); + EXPECT_FALSE(V.PointInSet(out3_W, kTol)); + EXPECT_FALSE(V.PointInSet(out4_W, kTol)); } -GTEST_TEST(HPolyhedronTest, FromUnboundedHPolytopeTest) { +GTEST_TEST(VPolytopeTest, FromUnboundedHPolytopeTest) { Matrix A; Vector4d b; A << 1, -1, // y ≥ x @@ -272,9 +271,7 @@ GTEST_TEST(HPolyhedronTest, FromUnboundedHPolytopeTest) { b << 0, 1, 2; HPolyhedron H(A, b); - // The following should trigger DRAKE_DEMAND(hpoly.IsBounded()) but I don't - // know how to test this in C++/Drake. - // VPolytope V(H); + DRAKE_EXPECT_THROWS_MESSAGE(VPolytope{H}, ".*hpoly.IsBounded().*"); } GTEST_TEST(VPolytopeTest, CloneTest) { diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc index bf0ee06bae39..b2de0acebdde 100644 --- a/geometry/optimization/vpolytope.cc +++ b/geometry/optimization/vpolytope.cc @@ -4,6 +4,7 @@ #include #include "libqhullcpp/Qhull.h" +#include "libqhullcpp/QhullVertexSet.h" #include "drake/common/is_approx_equal_abstol.h" #include "drake/solvers/solve.h" @@ -44,8 +45,7 @@ VPolytope::VPolytope(const QueryObject& query_object, VPolytope::VPolytope(const HPolyhedron& hpoly) : ConvexSet(&ConvexSetCloner, hpoly.ambient_dimension()) { - - DRAKE_DEMAND(hpoly.IsBounded()); + DRAKE_THROW_UNLESS(hpoly.IsBounded()); Eigen::MatrixXd coeffs(hpoly.A().rows(), hpoly.A().cols() + 1); coeffs.leftCols(hpoly.A().cols()) = hpoly.A(); @@ -68,11 +68,19 @@ VPolytope::VPolytope(const HPolyhedron& hpoly) vertices_.resize(hpoly.ambient_dimension(), qhull.facetCount()); int ii = 0; - for (orgQhull::QhullFacet facet = qhull.beginFacet(); - facet != qhull.endFacet(); facet = facet.next()) { - std::vector vertex = facet.getCenter().toStdVector(); - vertices_.col(ii) = Eigen::Map( - vertex.data(), vertex.size()) * hpoly.ambient_dimension(); + for (const auto& facet : qhull.facetList()) { + auto incident_hyperplanes = facet.vertices(); + Eigen::MatrixXd vertex_A(incident_hyperplanes.count(), + hpoly.ambient_dimension()); + for (int jj = 0; jj < incident_hyperplanes.count(); jj++) { + std::vector hyperplane = + incident_hyperplanes.at(jj).point().toStdVector(); + vertex_A.row(jj) = Eigen::Map( + hyperplane.data(), hyperplane.size()); + } + vertices_.col(ii) = vertex_A.partialPivLu().solve(Eigen::VectorXd::Ones( + incident_hyperplanes.count())) + + eigen_center; ii++; } }