[geometry:optimization] Add VPolytope from HPolyhedron

brings in qhull as an external.
RussTedrake · Oct 1, 2021 · d52e84e · d52e84e
1 parent 55ce547
commit d52e84e
Show file tree

Hide file tree

Showing 8 changed files with 86 additions and 0 deletions.
diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel
@@ -44,6 +44,7 @@ drake_cc_library(
         "//geometry:scene_graph",
         "//solvers:mathematical_program",
         "//solvers:solve",
+        "@qhull",
     ],
 )
 

diff --git a/geometry/optimization/test/vpolytope_test.cc b/geometry/optimization/test/vpolytope_test.cc
@@ -155,6 +155,23 @@ GTEST_TEST(VPolytopeTest, UnitBox6DTest) {
   EXPECT_FALSE(V.PointInSet(out2_W, kTol));
 }
 
+GTEST_TEST(VPolytopeTest, FromHPolyhedronTest) {
+  HPolyhedron H = HPolyhedron::MakeUnitBox(6);
+  VPolytope V(H);
+  EXPECT_EQ(V.ambient_dimension(), 6);
+  EXPECT_EQ(V.vertices().rows(), 6);
+  EXPECT_EQ(V.vertices().cols(), 8);
+
+  Vector6d in1_W{Vector6d::Constant(-.99)}, in2_W{Vector6d::Constant(.99)},
+      out1_W{Vector6d::Constant(-1.01)}, out2_W{Vector6d::Constant(1.01)};
+
+  const double kTol = 1e-11;
+  EXPECT_TRUE(V.PointInSet(in1_W, kTol));
+  EXPECT_TRUE(V.PointInSet(in2_W, kTol));
+  EXPECT_FALSE(V.PointInSet(out1_W, kTol));
+  EXPECT_FALSE(V.PointInSet(out2_W, kTol));
+}
+
 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
@@ -3,6 +3,8 @@
 #include <limits>
 #include <memory>
 
+#include "libqhullcpp/Qhull.h"
+
 #include "drake/common/is_approx_equal_abstol.h"
 #include "drake/solvers/solve.h"
 
@@ -40,6 +42,12 @@ VPolytope::VPolytope(const QueryObject<double>& query_object,
   vertices_ = X_EG * vertices;
 }
 
+VPolytope::VPolytope(const HPolyhedron& hpoly)
+    : ConvexSet(&ConvexSetCloner<VPolytope>, hpoly.ambient_dimension()) {
+  orgQhull::Qhull qhull;
+  unused(qhull);
+}
+
 VPolytope::~VPolytope() = default;
 
 VPolytope VPolytope::MakeBox(const Eigen::Ref<const VectorXd>& lb,

diff --git a/geometry/optimization/vpolytope.h b/geometry/optimization/vpolytope.h
@@ -6,6 +6,7 @@
 #include <vector>
 
 #include "drake/geometry/optimization/convex_set.h"
+#include "drake/geometry/optimization/hpolyhedron.h"
 
 namespace drake {
 namespace geometry {
@@ -35,6 +36,9 @@ class VPolytope final : public ConvexSet {
   VPolytope(const QueryObject<double>& query_object, GeometryId geometry_id,
             std::optional<FrameId> reference_frame = std::nullopt);
 
+  /** Constructs the polytope from a bounded polyhedron (using qhull). */
+  explicit VPolytope(const HPolyhedron& hpoly);
+
   ~VPolytope() final;
 
   /** Returns true if the point is within @p tol of the set under the L∞-norm.

diff --git a/tools/workspace/default.bzl b/tools/workspace/default.bzl
@@ -67,6 +67,7 @@ load("@drake//tools/workspace/pycodestyle:repository.bzl", "pycodestyle_reposito
 load("@drake//tools/workspace/pygame_py:repository.bzl", "pygame_py_repository")  # noqa
 load("@drake//tools/workspace/python:repository.bzl", "python_repository")
 load("@drake//tools/workspace/qdldl:repository.bzl", "qdldl_repository")
+load("@drake//tools/workspace/qhull:repository.bzl", "qhull_repository")
 load("@drake//tools/workspace/ros_xacro:repository.bzl", "ros_xacro_repository")  # noqa
 load("@drake//tools/workspace/rules_pkg:repository.bzl", "rules_pkg_repository")  # noqa
 load("@drake//tools/workspace/rules_python:repository.bzl", "rules_python_repository")  # noqa
@@ -231,6 +232,8 @@ def add_default_repositories(excludes = [], mirrors = DEFAULT_MIRRORS):
         python_repository(name = "python")
     if "qdldl" not in excludes:
         qdldl_repository(name = "qdldl", mirrors = mirrors)
+    if "qhull" not in excludes:
+        qhull_repository(name = "qhull", mirrors = mirrors)
     if "ros_xacro" not in excludes:
         ros_xacro_repository(name = "ros_xacro", mirrors = mirrors)
     if "rules_pkg" not in excludes:

diff --git a/tools/workspace/qhull/BUILD.bazel b/tools/workspace/qhull/BUILD.bazel
@@ -0,0 +1,5 @@
+# -*- python -*-
+
+load("//tools/lint:lint.bzl", "add_lint_tests")
+
+add_lint_tests()
diff --git a/tools/workspace/qhull/package.BUILD.bazel b/tools/workspace/qhull/package.BUILD.bazel
@@ -0,0 +1,33 @@
+# -*- python -*-
+
+load(
+    "@drake//tools/install:install.bzl",
+    "install",
+)
+
+licenses(["notice"])  # Copyright
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+    name = "qhull",
+    hdrs = glob(["src/libqhullcpp/*.h", "src/libqhull_r/*.h"]),
+    includes = ["src"],
+    srcs = glob([
+        "src/libqhull_r/*.c",
+        "src/libqhullcpp/*.cpp",
+        "src/libqhullcpp/*.h",
+      ], 
+      exclude = [
+        "src/libqhullcpp/qt-qhull.cpp",
+        "src/libqhullcpp/usermem_r-cpp.cpp",
+      ],
+    ),
+    linkstatic = 1,
+)
+
+# Install the license file.
+install(
+    name = "install",
+    docs = ["LICENSE"],
+)
diff --git a/tools/workspace/qhull/repository.bzl b/tools/workspace/qhull/repository.bzl
@@ -0,0 +1,15 @@
+# -*- python -*-
+
+load("@drake//tools/workspace:github.bzl", "github_archive")
+
+def qhull_repository(
+        name,
+        mirrors = None):
+    github_archive(
+        name = name,
+        repository = "qhull/qhull",
+        commit = "2020.2",
+        sha256 = "59356b229b768e6e2b09a701448bfa222c37b797a84f87f864f97462d8dbc7c5",  # noqa
+        build_file = "@drake//tools/workspace/qhull:package.BUILD.bazel",
+        mirrors = mirrors,
+    )