diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 83ff61a64ac8..067f3c70cb99 100644 --- 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 index ab23055cc067..d16715ca8552 100644 --- 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 clone = V.Clone(); diff --git a/geometry/optimization/vpolytope.cc b/geometry/optimization/vpolytope.cc index f2b0f214a086..4b6c23ed52b9 100644 --- a/geometry/optimization/vpolytope.cc +++ b/geometry/optimization/vpolytope.cc @@ -3,6 +3,8 @@ #include #include +#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& query_object, vertices_ = X_EG * vertices; } +VPolytope::VPolytope(const HPolyhedron& hpoly) + : ConvexSet(&ConvexSetCloner, hpoly.ambient_dimension()) { + orgQhull::Qhull qhull; + unused(qhull); +} + VPolytope::~VPolytope() = default; VPolytope VPolytope::MakeBox(const Eigen::Ref& lb, diff --git a/geometry/optimization/vpolytope.h b/geometry/optimization/vpolytope.h index 5e21ab13f077..9eeb5c8ef8f9 100644 --- a/geometry/optimization/vpolytope.h +++ b/geometry/optimization/vpolytope.h @@ -6,6 +6,7 @@ #include #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& query_object, GeometryId geometry_id, std::optional 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 index 0c9066e38968..be9e383c4055 100644 --- 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 new file mode 100644 index 000000000000..7198e3bb9b53 --- /dev/null +++ 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 new file mode 100644 index 000000000000..62a63dd4e795 --- /dev/null +++ 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 new file mode 100644 index 000000000000..876be5edae65 --- /dev/null +++ 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, + )