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 bd7688b9a210..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" @@ -18,7 +19,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 +159,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 +176,104 @@ GTEST_TEST(VPolytopeTest, FromHPolyhedronTest) { EXPECT_FALSE(V.PointInSet(out2_W, kTol)); } +GTEST_TEST(VPolytopeTest, From2DHPolytopeTest) { + Matrix<double, 4, 2> 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}; + + 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(VPolytopeTest, From3DHSimplexTest) { + Matrix<double, 4, 3> 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(VPolytopeTest, FromRedundantHPolytopeTest) { + Matrix<double, 6, 2> 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(VPolytopeTest, FromUnboundedHPolytopeTest) { + Matrix<double, 4, 2> A; + Vector4d b; + A << 1, -1, // y ≥ x + 1, 0, // x ≤ 1 + 0, 1, // y ≤ 2 + b << 0, 1, 2; + HPolyhedron H(A, b); + + DRAKE_EXPECT_THROWS_MESSAGE(VPolytope{H}, ".*hpoly.IsBounded().*"); +} + GTEST_TEST(VPolytopeTest, CloneTest) { VPolytope V = VPolytope::MakeBox(Vector3d{-3, -4, -5}, Vector3d{6, 7, 8}); std::unique_ptr<ConvexSet> clone = V.Clone(); diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc index aa59ceadea50..b2de0acebdde 100644 --- a/geometry/optimization/vpolytope.cc +++ b/geometry/optimization/vpolytope.cc @@ -4,6 +4,7 @@ #include <memory> #include "libqhullcpp/Qhull.h" +#include "libqhullcpp/QhullVertexSet.h" #include "drake/common/is_approx_equal_abstol.h" #include "drake/solvers/solve.h" @@ -44,6 +45,8 @@ VPolytope::VPolytope(const QueryObject<double>& query_object, VPolytope::VPolytope(const HPolyhedron& hpoly) : ConvexSet(&ConvexSetCloner<VPolytope>, hpoly.ambient_dimension()) { + DRAKE_THROW_UNLESS(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(); @@ -65,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<double> vertex = facet.getCenter().toStdVector(); - vertices_.col(ii) = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>( - 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<double> hyperplane = + incident_hyperplanes.at(jj).point().toStdVector(); + vertex_A.row(jj) = Eigen::Map<Eigen::RowVectorXd, Eigen::Unaligned>( + hyperplane.data(), hyperplane.size()); + } + vertices_.col(ii) = vertex_A.partialPivLu().solve(Eigen::VectorXd::Ones( + incident_hyperplanes.count())) + + eigen_center; ii++; } }