Skip to content

Commit

Permalink
Merge pull request #45 from mpetersen94/qhull2
Browse files Browse the repository at this point in the history 
Fix Hpolytope to Vpolytope conversion and add unit tests
  • Loading branch information
@RussTedrake
RussTedrake authored Dec 21, 2021
2 parents ed8ff11 + 1952a1f commit 4d320b1
Show file tree
Hide file tree
Showing 3 changed files with 120 additions and 6 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
104 changes: 103 additions & 1 deletion 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 All @@ -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;
Expand Down Expand Up @@ -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);
Expand All @@ -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();
Expand Down
21 changes: 16 additions & 5 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,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();
Expand All @@ -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++;
}
}
Expand Down

0 comments on commit 4d320b1

Please sign in to comment.