Skip to content

Commit

Permalink
Fix hpolytope to vpolytope conversion exposed by unit tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@mpetersen94
mpetersen94 committed Dec 8, 2021
1 parent 00713d1 commit 1952a1f
Show file tree
Hide file tree
Showing 3 changed files with 53 additions and 47 deletions.
1 change: 1 addition & 0 deletions geometry/optimization/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,6 +163,7 @@ drake_cc_googletest(
":convex_set",
":test_utilities",
"//common/test_utilities:eigen_matrix_compare",
"//common/test_utilities:expect_throws_message",
],
)

Expand Down
77 changes: 37 additions & 40 deletions geometry/optimization/test/vpolytope_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -175,7 +176,7 @@ GTEST_TEST(VPolytopeTest, FromHUnitBoxTest) {
EXPECT_FALSE(V.PointInSet(out2_W, kTol));
}

GTEST_TEST(HPolyhedronTest, From2DHPolytopeTest) {
GTEST_TEST(VPolytopeTest, From2DHPolytopeTest) {
Matrix<double, 4, 2> A;
Vector4d b;
A << 1, -1, // y ≥ x
Expand All @@ -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<double, 4, 3> A;
Vector4d b;
A << -1, 0, 0,
Expand All @@ -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<double, 6, 2> A;
Vector6d b;
A << 1, -1, // y ≥ x
Expand All @@ -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<double, 4, 2> A;
Vector4d b;
A << 1, -1, // y ≥ x
Expand All @@ -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) {
Expand Down
22 changes: 15 additions & 7 deletions geometry/optimization/vpolytope.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -44,8 +45,7 @@ VPolytope::VPolytope(const QueryObject<double>& query_object,

VPolytope::VPolytope(const HPolyhedron& hpoly)
: ConvexSet(&ConvexSetCloner<VPolytope>, 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();
Expand All @@ -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<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++;
}
}
Expand Down

0 comments on commit 1952a1f

Please sign in to comment.