diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 4a6e091ca6b5..f3aa50567506 100644 --- a/geometry/optimization/BUILD.bazel +++ b/geometry/optimization/BUILD.bazel @@ -51,6 +51,7 @@ drake_cc_library( "affine_ball.cc", "affine_subspace.cc", "cartesian_product.cc", + "convex_hull.cc", "convex_set.cc", "hpolyhedron.cc", "hyperellipsoid.cc", @@ -65,6 +66,7 @@ drake_cc_library( "affine_ball.h", "affine_subspace.h", "cartesian_product.h", + "convex_hull.h", "convex_set.h", "hpolyhedron.h", "hyperellipsoid.h", @@ -275,6 +277,16 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "convex_hull_test", + deps = [ + ":convex_set", + ":test_utilities", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", + ], +) + drake_cc_googletest( name = "convex_set_test", deps = [ diff --git a/geometry/optimization/convex_hull.cc b/geometry/optimization/convex_hull.cc new file mode 100644 index 000000000000..2490114140b1 --- /dev/null +++ b/geometry/optimization/convex_hull.cc @@ -0,0 +1,272 @@ +#include "drake/geometry/optimization/convex_hull.h" + +#include +#include + +#include + +#include "drake/solvers/solve.h" + +namespace drake { +namespace geometry { +namespace optimization { + +using Eigen::Map; +using Eigen::MatrixXd; +using Eigen::VectorXd; +using solvers::Binding; +using solvers::Constraint; +using solvers::LinearConstraint; +using solvers::MathematicalProgram; +using solvers::ProgramAttribute; +using solvers::ProgramAttributes; +using solvers::VectorXDecisionVariable; +using symbolic::Expression; +using symbolic::Variable; + +namespace { + +int GetAmbientDimension(const ConvexSets& sets) { + if (sets.empty()) { + return 0; + } + const int ambient_dimension = sets[0]->ambient_dimension(); + for (const copyable_unique_ptr& set : sets) { + DRAKE_THROW_UNLESS(set != nullptr); + DRAKE_THROW_UNLESS(set->ambient_dimension() == ambient_dimension); + } + return ambient_dimension; +} + +// Add the new variables to the existing_variables from the bindings. +void AddNewVariables( + const std::vector>& bindings, + symbolic::Variables* existing_variables) { + for (const auto& binding : bindings) { + const auto& vars = binding.variables(); + existing_variables->insert(symbolic::Variables(vars)); + } +} + +// Given a vector of convex sets, return a vector of non-empty convex sets if +// remove_empty_sets is true, otherwise return the original vector. +ConvexSets MakeParticipartingSets(const ConvexSets& sets, + const bool remove_empty_sets) { + if (!remove_empty_sets) { + return sets; + } + ConvexSets non_empty_sets; + for (const auto& set : sets) { + if (!set->IsEmpty()) { + non_empty_sets.push_back(set); + } + } + return non_empty_sets; +} +} // namespace + +ConvexHull::ConvexHull(const ConvexSets& sets, const bool remove_empty_sets) + : ConvexSet(GetAmbientDimension(sets), false), + sets_(sets), + participating_sets_{MakeParticipartingSets(sets_, remove_empty_sets)}, + empty_sets_removed_(remove_empty_sets) {} + +ConvexHull::~ConvexHull() = default; + +const ConvexSet& ConvexHull::element(int index) const { + DRAKE_THROW_UNLESS(0 <= index && index < std::ssize(sets_)); + return *sets_[index]; +} + +std::unique_ptr ConvexHull::DoClone() const { + return std::make_unique(*this); +} + +std::optional ConvexHull::DoIsBoundedShortcut() const { + return std::nullopt; +} + +bool ConvexHull::DoIsEmpty() const { + if (empty_sets_removed_) { + return participating_sets_.empty(); + } + // If empty_sets_removed_ is false, then we reconstruct the + // participating_sets_ and check if it is empty. + return ConvexHull(sets_, true).IsEmpty(); +} + +std::optional ConvexHull::DoMaybeGetPoint() const { + return std::nullopt; +} + +bool ConvexHull::DoPointInSet(const Eigen::Ref& x, + double tol) const { + // Check the feasibility of |x - ∑ᵢ xᵢ| <= tol * 1, xᵢ ∈ 𝛼ᵢXᵢ, 𝛼ᵢ ≥ 0, ∑ᵢ 𝛼ᵢ = + // 1, where 1 is a vector of ones and |.| is interpreted element-wise. + MathematicalProgram prog; + const int n = std::ssize(participating_sets_); + const int d = ambient_dimension(); + // x_sets is d * n, each column is a variable for a convex set + auto x_sets = prog.NewContinuousVariables(d, n, "x_sets"); + auto alpha = prog.NewContinuousVariables(n, "alpha"); + // constraint: x - tol * 1 ≤ ∑ᵢ xᵢ ≤ x + tol * 1 + Eigen::VectorXd ones = Eigen::VectorXd::Ones(n); + for (int i = 0; i < d; ++i) { + prog.AddLinearConstraint(Eigen::VectorXd::Ones(n), x(i) - tol, x(i) + tol, + x_sets.row(i)); + } + // constraint: ∑ᵢ αᵢ = 1 + prog.AddLinearEqualityConstraint(Eigen::MatrixXd::Ones(1, n), + VectorXd::Ones(1), alpha); + // constraint: 1 ≥ αᵢ ≥ 0 + prog.AddBoundingBoxConstraint(0, 1, alpha); + // Add the constraints for each convex set. + for (int i = 0; i < n; ++i) { + participating_sets_[i]->AddPointInNonnegativeScalingConstraints( + &prog, x_sets.col(i), alpha(i)); + } + // Check feasibility. + const auto result = solvers::Solve(prog); + return result.is_success(); +} + +std::pair, + std::vector>> +ConvexHull::DoAddPointInSetConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& vars) const { + // Add the constraint that vars = ∑ᵢ xᵢ, xᵢ ∈ 𝛼ᵢXᵢ, 𝛼ᵢ ≥ 0, ∑ᵢ 𝛼ᵢ = 1. + const int n = std::ssize(participating_sets_); + const int d = ambient_dimension(); + // The variable is d * n, each column is a variable for a convex set + auto x_sets = prog->NewContinuousVariables(d, n, "x_sets"); + auto alpha = prog->NewContinuousVariables(n, "alpha"); + std::vector> new_bindings; + new_bindings.push_back(prog->AddBoundingBoxConstraint(0, 1, alpha)); + // constraint: vars - ∑ᵢ xᵢ == 0 -> (1 ... 1 -1)(x_sets[i, :], vars) = 0 + Eigen::VectorXd a(n + 1); + a.head(n) = Eigen::VectorXd::Ones(n); + a(n) = -1; + for (int i = 0; i < d; ++i) { + Eigen::Ref vars_i = + vars.segment(i, 1); + solvers::VectorXDecisionVariable x_i_vars(n + 1); + x_i_vars.head(n) = x_sets.row(i); + x_i_vars(n) = vars(i); + new_bindings.push_back(prog->AddLinearEqualityConstraint(a, 0, x_i_vars)); + } + // constraint: ∑ᵢ αᵢ = 1 + new_bindings.push_back(prog->AddLinearEqualityConstraint( + Eigen::MatrixXd::Ones(1, n), VectorXd::Ones(1), alpha)); + auto new_vars = drake::symbolic::Variables(); + // add the constraints for each convex set + for (int i = 0; i < n; ++i) { + auto cons = participating_sets_[i]->AddPointInNonnegativeScalingConstraints( + prog, x_sets.col(i), alpha(i)); + new_bindings.insert(new_bindings.end(), cons.begin(), cons.end()); + AddNewVariables(cons, &new_vars); + } + // Convert to std::vector because sets do not have + // contiguous memory. + std::vector new_vars_vec(new_vars.size()); + std::move(new_vars.begin(), new_vars.end(), new_vars_vec.begin()); + return std::make_pair(Eigen::Map>( + new_vars_vec.data(), new_vars_vec.size()), + std::move(new_bindings)); +} + +std::vector> +ConvexHull::DoAddPointInNonnegativeScalingConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& x, + const symbolic::Variable& t) const { + // Add the constraint that t >= 0, x = ∑ᵢ xᵢ, xᵢ ∈ 𝛼ᵢXᵢ, 𝛼ᵢ ≥ 0, ∑ᵢ 𝛼ᵢ = t. + const int n = std::ssize(participating_sets_); + const int d = ambient_dimension(); + // The new variable is d * n, each row is a variable for a convex set + auto x_sets = prog->NewContinuousVariables(d, n, "x_sets"); + auto alpha = prog->NewContinuousVariables(n, "alpha"); + const double inf = std::numeric_limits::infinity(); + std::vector> new_bindings; + // Constraint I: -x + ∑ᵢ xᵢ == 0 + Eigen::VectorXd a(n + 1); + a.head(n) = Eigen::VectorXd::Ones(n); + a(n) = -1; + for (int i = 0; i < d; ++i) { + solvers::VectorXDecisionVariable x_sets_i_x(n + 1); + x_sets_i_x.head(n) = x_sets.row(i); + x_sets_i_x(n) = x(i); + new_bindings.push_back(prog->AddLinearEqualityConstraint(a, 0, x_sets_i_x)); + } + // Constraint II: ∑ᵢ αᵢ = t + solvers::VectorXDecisionVariable alpha_t_vec(n + 1); + alpha_t_vec.head(n) = alpha; + alpha_t_vec(n) = t; + new_bindings.push_back(prog->AddLinearEqualityConstraint(a, 0, alpha_t_vec)); + // alpha should be positive. + new_bindings.push_back(prog->AddBoundingBoxConstraint(0, inf, alpha)); + // Finally add the constraints for each convex set. + for (int i = 0; i < n; ++i) { + auto cons = participating_sets_[i]->AddPointInNonnegativeScalingConstraints( + prog, x_sets.col(i), alpha(i)); + new_bindings.insert(new_bindings.end(), cons.begin(), cons.end()); + } + return new_bindings; +} + +std::vector> +ConvexHull::DoAddPointInNonnegativeScalingConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& A_x, + const Eigen::Ref& b_x, + const Eigen::Ref& c, double d, + const Eigen::Ref& x, + const Eigen::Ref& t) const { + // Add the constraint that A_x * x + b_x = ∑ᵢ xᵢ, xᵢ ∈ 𝛼ᵢXᵢ, 𝛼ᵢ ≥ 0, + // ∑ᵢ 𝛼ᵢ = c't + d. + const int dim = ambient_dimension(); + const int n = std::ssize(participating_sets_); + // The new variable is dim * n, each column belongs to a convex set. + auto x_sets = prog->NewContinuousVariables(dim, n, "x_sets"); + auto alpha = prog->NewContinuousVariables(n, "alpha"); + std::vector> new_bindings; + // constraint: A_x * x - ∑ᵢ sᵢ == -b_x + for (int i = 0; i < dim; ++i) { + // A_x.row(i) * x - (1, ..., 1) xᵢ == -b_x[i] + solvers::VectorXDecisionVariable x_sets_i_(dim + n); + x_sets_i_.head(dim) = x; + x_sets_i_.tail(n) = x_sets.row(i); + Eigen::VectorXd a_sum(A_x.cols() + n); + a_sum.head(A_x.cols()) = A_x.row(i); + a_sum.tail(n) = -Eigen::VectorXd::Ones(n); + new_bindings.push_back( + prog->AddLinearEqualityConstraint(a_sum, -b_x[i], x_sets_i_)); + } + // constraint: ∑ᵢ αᵢ = c't + d + const int p = c.size(); + Eigen::VectorXd a_con(n + p); + a_con.head(n) = Eigen::VectorXd::Ones(n); + a_con.tail(p) = -c; + solvers::VectorXDecisionVariable alpha_t_vec(n + p); + alpha_t_vec.head(n) = alpha; + alpha_t_vec.tail(p) = t; + new_bindings.push_back( + prog->AddLinearEqualityConstraint(a_con, d, alpha_t_vec)); + // Add the inclusion constraints for each convex set. + for (int i = 0; i < n; ++i) { + auto cons = participating_sets_[i]->AddPointInNonnegativeScalingConstraints( + prog, x_sets.col(i), alpha(i)); + new_bindings.insert(new_bindings.end(), cons.begin(), cons.end()); + } + return new_bindings; +} + +std::pair, math::RigidTransformd> +ConvexHull::DoToShapeWithPose() const { + throw std::runtime_error( + "ToShapeWithPose is not implemented yet for ConvexHull."); +} + +} // namespace optimization +} // namespace geometry +} // namespace drake diff --git a/geometry/optimization/convex_hull.h b/geometry/optimization/convex_hull.h new file mode 100644 index 000000000000..27b50eaf0433 --- /dev/null +++ b/geometry/optimization/convex_hull.h @@ -0,0 +1,111 @@ +#pragma once + +#include +#include +#include +#include + +#include "drake/geometry/optimization/convex_set.h" + +namespace drake { +namespace geometry { +namespace optimization { + +/** Implements the convex hull of a set of convex sets. The convex hull of +multiple sets is defined as the smallest convex set that contains all the sets. + Given non-empty convex sets {X₁, X₂, ..., Xₙ}, the convex hull is the set of +all convex combinations of points in the sets, i.e. {∑ᵢ λᵢ xᵢ | xᵢ ∈ Xᵢ, λᵢ ≥ 0, +∑ᵢ λᵢ = 1}. +@ingroup geometry_optimization */ +class ConvexHull final : public ConvexSet, private ShapeReifier { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(ConvexHull) + + /** Constructs the convex hull from a vector of convex sets. + @param sets A vector of convex sets that define the convex hull. + @param remove_empty_sets If true, the constructor will check if any of the + sets are empty and will not consider them. If false, the constructor will + not check if any of the sets are empty. + @warning If remove_empty_sets is set to false, but some of the sets are in + fact empty, then unexpected and incorrect results may occur. Only set this + flag to false if you are sure that your sets are non-empty and performance in + the constructor is critical. + */ + explicit ConvexHull(const ConvexSets& sets, + const bool remove_empty_sets = true); + + ~ConvexHull() final; + + /** Returns the participating convex sets. */ + const ConvexSets& sets() const { return sets_; } + + /** Returns the participating sets in the convex hull. If the constructor was + called with remove_empty_sets=false, this function will return the original + sets, including potentially empty sets. */ + const ConvexSets& participating_sets() const { return participating_sets_; } + + /** Returns true if `this` was constructed with remove_empty_sets=true. */ + bool empty_sets_removed() const { return empty_sets_removed_; } + + /** Returns a reference to the convex set at the given index (including empty + * sets). */ + const ConvexSet& element(int index) const; + + /** Returns the number of convex sets defining the convex hull (including + * empty sets). */ + int num_elements() const { return sets_.size(); } + + /** @note if called on an instance that was called with + remove_empty_sets=false, this function will reconstruct the convex hull with + remove_empty_sets=true. Therefore, it is recommended to call this function + only once. */ + using ConvexSet::IsEmpty; + + /** @throws Not implemented. */ + using ConvexSet::CalcVolume; + + private: + std::unique_ptr DoClone() const final; + + std::optional DoIsBoundedShortcut() const final; + + bool DoIsEmpty() const final; + + std::optional DoMaybeGetPoint() const final; + + bool DoPointInSet(const Eigen::Ref& x, + double tol) const final; + + std::pair, + std::vector>> + DoAddPointInSetConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& vars) + const final; + + std::vector> + DoAddPointInNonnegativeScalingConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& x, + const symbolic::Variable& t) const final; + + std::vector> + DoAddPointInNonnegativeScalingConstraints( + solvers::MathematicalProgram* prog, + const Eigen::Ref& A_x, + const Eigen::Ref& b_x, + const Eigen::Ref& c, double d, + const Eigen::Ref& x, + const Eigen::Ref& t) const final; + + std::pair, math::RigidTransformd> DoToShapeWithPose() + const final; + + ConvexSets sets_{}; + ConvexSets participating_sets_{}; + bool empty_sets_removed_; +}; + +} // namespace optimization +} // namespace geometry +} // namespace drake diff --git a/geometry/optimization/test/convex_hull_test.cc b/geometry/optimization/test/convex_hull_test.cc new file mode 100644 index 000000000000..420d97a41640 --- /dev/null +++ b/geometry/optimization/test/convex_hull_test.cc @@ -0,0 +1,222 @@ +#include "drake/geometry/optimization/convex_hull.h" + +#include + +#include "drake/common/is_approx_equal_abstol.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/geometry/optimization/hyperrectangle.h" +#include "drake/geometry/optimization/point.h" +#include "drake/geometry/optimization/test_utilities.h" +#include "drake/solvers/solve.h" + +namespace drake { +namespace geometry { +namespace optimization { + +GTEST_TEST(ConvexHullTest, BasicTests) { + // Empty convex hull without any sets. + ConvexHull null_hull({}); + EXPECT_TRUE(null_hull.IsEmpty()); + EXPECT_TRUE(null_hull.IsBounded()); + EXPECT_FALSE(null_hull.MaybeGetPoint().has_value()); + EXPECT_EQ(null_hull.num_elements(), 0); + EXPECT_EQ(null_hull.ambient_dimension(), 0); + // Convex hull with a point and a rectangle. + const Point point(Eigen::Vector2d(1.0, 2.0)); + const Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 1.0)); + ConvexHull hull(MakeConvexSets(point, rectangle)); + EXPECT_EQ(hull.num_elements(), 2); + EXPECT_EQ(hull.ambient_dimension(), 2); + EXPECT_FALSE(hull.IsEmpty()); + EXPECT_TRUE(hull.IsBounded()); + EXPECT_FALSE(hull.MaybeGetPoint().has_value()); + // Inppropriate dimensions. + Point point_3d = Point(Eigen::Vector3d(1.0, 2.0, 3.0)); + EXPECT_THROW(ConvexHull(MakeConvexSets(point, point_3d)), std::runtime_error); +} + +GTEST_TEST(ConvexHullTest, CheckEmpty) { + const Point point(Eigen::Vector2d(1.0, 2.0)); + const Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 1.0)); + // Add an HPolyhedron that is empty. + Eigen::MatrixXd A(2, 2); + A << 1, 0, -1, 0; + Eigen::VectorXd b(2); + b << -1, -1; + HPolyhedron empty_hpolyhedron(A, b); + ConvexHull hull_1(MakeConvexSets(point, rectangle, empty_hpolyhedron)); + EXPECT_FALSE(hull_1.IsEmpty()); + EXPECT_TRUE(hull_1.empty_sets_removed()); + EXPECT_EQ(hull_1.participating_sets().size(), 2); + EXPECT_TRUE(hull_1.PointInSet(Eigen::Vector2d(0.0, 1.5), 1e-6)); + // [0, 3] is not in the convex hull. + EXPECT_FALSE(hull_1.PointInSet(Eigen::Vector2d(0.0, 3.0), 1e-6)); + ConvexHull hull_2(MakeConvexSets(point, rectangle, empty_hpolyhedron), false); + EXPECT_FALSE(hull_2.IsEmpty()); + EXPECT_FALSE(hull_2.empty_sets_removed()); + EXPECT_EQ(hull_2.participating_sets().size(), 3); + EXPECT_TRUE(hull_2.PointInSet(Eigen::Vector2d(0.0, 1.5), 1e-6)); + // Unexpected behavior because the check was bypassed. + // [0, 3] is not in the convex hull, but it says it is because + // 0*empty_hpolyhedron contains the line. + EXPECT_TRUE(hull_2.PointInSet(Eigen::Vector2d(0.0, 3.0), 1e-6)); + // If only the empty_hpolyhedron is added, the convex hull should be empty. + ConvexHull hull_3(MakeConvexSets(empty_hpolyhedron)); + EXPECT_TRUE(hull_3.IsEmpty()); +} + +GTEST_TEST(ConvexHullTest, PointInSet1) { + // Case with 1 set in 2D. + Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 1.0)); + ConvexHull hull(MakeConvexSets(rectangle)); + EXPECT_TRUE(hull.PointInSet(Eigen::Vector2d(0.0, 1.0), 1e-6)); + EXPECT_FALSE(hull.PointInSet(Eigen::Vector2d(1.0, 1.1), 1e-6)); + // Test tolerance. + EXPECT_TRUE(hull.PointInSet(Eigen::Vector2d(1.0, 1.1), 0.1)); +} + +GTEST_TEST(ConvexHullTest, PointInSet2) { + // Case with 3 sets in 2D. + Point point1(Eigen::Vector2d(0.0, 0.0)); + Point point2(Eigen::Vector2d(1.0, 0.0)); + Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 1.0)); + ConvexHull hull(MakeConvexSets(point1, point2, rectangle)); + EXPECT_TRUE(hull.PointInSet(Eigen::Vector2d(0.0, 0.0), 1e-6)); + EXPECT_TRUE(hull.PointInSet(Eigen::Vector2d(1.0, 1.0), 1e-6)); + EXPECT_TRUE(hull.PointInSet(Eigen::Vector2d(0.9, 0.5), 1e-6)); + EXPECT_TRUE(hull.PointInSet(Eigen::Vector2d(-0.5, 0.5), 1e-6)); + EXPECT_FALSE(hull.PointInSet(Eigen::Vector2d(1.1, 0.5), 1e-6)); + // Test tolerances. + EXPECT_FALSE(hull.PointInSet(Eigen::Vector2d(-0.501, 0.5), 1e-4)); + EXPECT_TRUE(hull.PointInSet(Eigen::Vector2d(-0.501, 0.5), 1e-2)); +} + +GTEST_TEST(ConvexHullTest, AddPointInSetConstraints1) { + Point point(Eigen::Vector2d(0.0, 0.0)); + Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 1.0)); + ConvexHull hull(MakeConvexSets(point, rectangle)); + EXPECT_TRUE( + internal::CheckAddPointInSetConstraints(hull, Eigen::Vector2d(0.4, 0.4))); + EXPECT_FALSE( + internal::CheckAddPointInSetConstraints(hull, Eigen::Vector2d(0.6, 0.4))); +} + +GTEST_TEST(ConvexHullTest, AddPointInSetConstraints2) { + Point point(Eigen::Vector2d(0.0, 0.0)); + Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 1.0)); + ConvexHull hull(MakeConvexSets(point, rectangle)); + // Solves a mathematical program that finds a point in the convex hull with + // least L2 distance to (0.8,0) The result should be (0.4, 0.4). + solvers::MathematicalProgram prog; + auto x = prog.NewContinuousVariables(2, "x"); + auto [new_vars, new_constraints] = hull.AddPointInSetConstraints(&prog, x); + // How many new variables are added? + // 2 alphas, 2*2 x variables. Total 6. + EXPECT_EQ(new_vars.size(), 6); + prog.AddQuadraticCost((x - Eigen::Vector2d(0.8, 0.0)).squaredNorm()); + const auto result = Solve(prog); + EXPECT_TRUE(result.is_success()); + const Eigen::VectorXd x_sol = result.GetSolution(x); + EXPECT_TRUE(CompareMatrices(x_sol, Eigen::Vector2d(0.4, 0.4), 1e-6)); +} + +GTEST_TEST(ConvexHullTest, AddPointInSetConstraints3) { + // Makes convex hull from a point and another convex hull. Calls + // AddPointInNonnegativeScalingConstraints for the second convex hull. + Point point1(Eigen::Vector2d(0.0, 0.0)); + Point point2(Eigen::Vector2d(0.5, 0.0)); + Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 1.0)); + ConvexHull hull1(MakeConvexSets(point1, rectangle)); + ConvexHull hull2(MakeConvexSets(hull1, point2)); + // We know that (0.5, 0) to (1.0, 1.0) becomes a face + EXPECT_TRUE(internal::CheckAddPointInSetConstraints( + hull2, Eigen::Vector2d(0.3, 0.0))); + EXPECT_TRUE(internal::CheckAddPointInSetConstraints( + hull2, Eigen::Vector2d(0.6, 0.2))); + EXPECT_FALSE(internal::CheckAddPointInSetConstraints( + hull2, Eigen::Vector2d(0.6, 0.1))); +} + +GTEST_TEST(ConvexHullTest, AddPointInNonnegativeScalingConstraints1) { + Point point(Eigen::Vector2d(0.0, 0.0)); + Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 1.0)); + // Add an HPolyhedron that is empty. + Eigen::MatrixXd A(2, 2); + A << 1, 0, -1, 0; + Eigen::VectorXd b(2); + b << 1, -2; + HPolyhedron empty_hpolyhedron(A, b); + ConvexHull hull(MakeConvexSets(point, rectangle, empty_hpolyhedron)); + solvers::MathematicalProgram prog; + auto x = prog.NewContinuousVariables(2, "x"); + auto t = prog.NewContinuousVariables(1, "t"); + auto new_constraints = + hull.AddPointInNonnegativeScalingConstraints(&prog, x, t(0)); + EXPECT_GT(new_constraints.size(), 0); + // Solve the closest point to (2.0, 1.0). The closest point is (1.5, 1.5) when + // t = 1.5. + prog.AddQuadraticCost((x - Eigen::Vector2d(2.0, 1.0)).squaredNorm()); + const auto result = Solve(prog); + EXPECT_TRUE(result.is_success()); + const Eigen::VectorXd x_sol = result.GetSolution(x); + const Eigen::VectorXd t_sol = result.GetSolution(t); + EXPECT_TRUE(CompareMatrices(x_sol, Eigen::Vector2d(1.5, 1.5), 1e-4)); + EXPECT_GE(t_sol(0), 1.5); + // Adding negative constraints on t leads to infeasibility. + prog.AddLinearConstraint(t(0) <= -1.0); + const auto result2 = Solve(prog); + EXPECT_FALSE(result2.is_success()); +} + +GTEST_TEST(ConvexHullTest, AddPointInNonnegativeScalingConstraints2) { + // Verify by solving a 2D problem and verify the solution. + Point point1(Eigen::Vector2d(0.0, 0.0)); + Point point2(Eigen::Vector2d(0.0, 0.0)); + Hyperrectangle rectangle(Eigen::Vector2d(-1.0, 1.0), + Eigen::Vector2d(1.0, 2.0)); + ConvexHull hull(MakeConvexSets(point1, point2, rectangle)); + solvers::MathematicalProgram prog; + auto x = prog.NewContinuousVariables(2, "x"); + auto t = prog.NewContinuousVariables(3, "t"); + // CCW 90 degree rotation matrix + shift y by 2.0. + Eigen::MatrixXd A(2, 2); + A << 0, -1, 1, 0; + Eigen::Vector2d b(0.0, 2.0); + // Select a 3d vector c = [1, 2, -1] and d = 5.0, just to make the problem + // more interesting. + Eigen::Vector3d c(1.0, 1.0, 1.0); + const double d = 0.4; + auto new_constraints = + hull.AddPointInNonnegativeScalingConstraints(&prog, A, b, c, d, x, t); + EXPECT_GT(new_constraints.size(), 0); + // Pick a point: (-1.7 + a, -0.6). Ax+b will be (0.6, 0.3 + a). It would not + // be in the convex hull for a = 0. + auto a = prog.NewContinuousVariables(1, "a"); + prog.AddLinearEqualityConstraint(x(0) == -1.7 + a(0)); + prog.AddLinearEqualityConstraint(x(1) == -0.6); + // It would be in the convex hull for a = 0.3. The smallest (c't + d) that + // would allow this is 0.6. + prog.AddLinearCost(a(0)); + prog.AddL2NormCost(Eigen::MatrixXd::Identity(3, 3), Eigen::Vector3d::Zero(), + t); + const auto result = Solve(prog); + EXPECT_TRUE(result.is_success()); + const Eigen::VectorXd t_sol = result.GetSolution(t); + EXPECT_NEAR(result.GetSolution(a)(0), 0.3, 1e-6); + EXPECT_NEAR(c.transpose() * t_sol + d, 0.6, 1e-6); + // We know the t solution, all elements will be 0.2/3. + EXPECT_TRUE(CompareMatrices(t_sol, 0.2 / 3 * Eigen::Vector3d::Ones(), 1e-6)); +} + +} // namespace optimization +} // namespace geometry +} // namespace drake