From 42bee5e8ad899237d6bfb2a3cfebb7897b143fd1 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Thu, 24 Jun 2021 13:49:16 -0400 Subject: [PATCH 1/6] WIP: add IbexSolver --- solvers/BUILD.bazel | 41 ++++++++++++++++++++ solvers/ibex_solver.cc | 66 ++++++++++++++++++++++++++++++++ solvers/ibex_solver.h | 39 +++++++++++++++++++ solvers/ibex_solver_common.cc | 43 +++++++++++++++++++++ solvers/no_ibex.cc | 23 +++++++++++ solvers/test/ibex_solver_test.cc | 47 +++++++++++++++++++++++ 6 files changed, 259 insertions(+) create mode 100644 solvers/ibex_solver.cc create mode 100644 solvers/ibex_solver.h create mode 100644 solvers/ibex_solver_common.cc create mode 100644 solvers/no_ibex.cc create mode 100644 solvers/test/ibex_solver_test.cc diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index d55e2248ed80..80c6f98bc036 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -39,6 +39,7 @@ drake_cc_package_library( ":function", ":gurobi_qp", ":gurobi_solver", + ":ibex_solver", ":indeterminate", ":integer_inequality_solver", ":integer_optimization_util", @@ -280,6 +281,7 @@ drake_cc_library( ":dreal_solver", ":equality_constrained_qp_solver", ":gurobi_solver", + ":ibex_solver", ":ipopt_solver", ":linear_system_solver", ":moby_lcp_solver", @@ -719,6 +721,35 @@ drake_cc_library( }), ) +# Ibex is a dependency of dReal; disabling dReal disables Ibex. We could +# consider supporting Ibex w/o dReal, but have no use cases that need that +# support yet. +drake_cc_library( + name = "ibex_solver", + srcs = [ + "ibex_solver_common.cc", + ] + select({ + "//tools:no_dreal": [ + "no_ibex.cc", + ], + "//conditions:default": [ + "ibex_solver.cc", + ], + }), + hdrs = ["ibex_solver.h"], + deps = [ + ":mathematical_program", + ":solver_base", + "//common:essential", + ] + select({ + "//conditions:default": [ + "@ibex//:ibex", + ], + "//tools:no_dreal": [ + ], + }), +) + # External Solvers. # If --config gurobi is used, this target builds object code that satisfies @@ -1256,6 +1287,16 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "ibex_solver_test", + deps = [ + ":generic_trivial_constraint", + ":generic_trivial_cost", + ":ibex_solver", + ":mathematical_program", + ], +) + drake_cc_googletest( name = "integer_optimization_util_test", deps = [ diff --git a/solvers/ibex_solver.cc b/solvers/ibex_solver.cc new file mode 100644 index 000000000000..ef226410b9f4 --- /dev/null +++ b/solvers/ibex_solver.cc @@ -0,0 +1,66 @@ +#include "drake/solvers/ibex_solver.h" + +#include // To suppress cpplint. +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "./ibex.h" + +#include "drake/common/text_logging.h" +#include "drake/solvers/mathematical_program.h" + +namespace drake { +namespace solvers { + +using Eigen::VectorXd; + +bool IbexSolver::is_available() { return true; } + +void IbexSolver::DoSolve( + const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + const SolverOptions& merged_options, + MathematicalProgramResult* result) const { + if (!prog.GetVariableScaling().empty()) { + static const logging::Warn log_once( + "IbexSolver doesn't support the feature of variable scaling."); + } + + unused(initial_guess); + unused(merged_options); + + ibex::Variable ibex_vars(prog.num_vars(), "x"); + ibex::SystemFactory factory; + ibex::IntervalVector bounds(prog.num_vars()); + for (const auto& b : prog.bounding_box_constraints()) { + const auto& c = b.evaluator(); + const auto& lb = c->lower_bound(); + const auto& ub = c->upper_bound(); + + for (int k = 0; k < static_cast(b.GetNumElements()); ++k) { + const size_t index = + prog.FindDecisionVariableIndex(b.variables()[k]); + bounds[index] &= ibex::Interval(lb[k], ub[k]); + } + } + factory.add_var(ibex_vars, bounds); + + for (const auto&b : prog.linear_constraints()) { + symbolic::Formula f{b.evaluator()->CheckSatisfied(b.variables())}; + + } + + ibex::System sys(factory); + ibex::DefaultOptimizer o(sys, 1e-07); // TODO: take this as a parameter. + o.optimize(sys.box); +} + +} // namespace solvers +} // namespace drake diff --git a/solvers/ibex_solver.h b/solvers/ibex_solver.h new file mode 100644 index 000000000000..f22286ef59a6 --- /dev/null +++ b/solvers/ibex_solver.h @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include + +#include "drake/common/drake_copyable.h" +#include "drake/common/hash.h" +#include "drake/common/symbolic.h" +#include "drake/solvers/solver_base.h" + +namespace drake { +namespace solvers { + +class IbexSolver final : public SolverBase { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(IbexSolver) + + IbexSolver(); + ~IbexSolver() final; + + // A using-declaration adds these methods into our class's Doxygen. + using SolverBase::Solve; + + /// @name Static versions of the instance methods with similar names. + //@{ + static SolverId id(); + static bool is_available(); + static bool is_enabled(); + static bool ProgramAttributesSatisfied(const MathematicalProgram&); + //@} + + private: + void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, + const SolverOptions&, MathematicalProgramResult*) const final; +}; + +} // namespace solvers +} // namespace drake diff --git a/solvers/ibex_solver_common.cc b/solvers/ibex_solver_common.cc new file mode 100644 index 000000000000..268f4b8d4f54 --- /dev/null +++ b/solvers/ibex_solver_common.cc @@ -0,0 +1,43 @@ +/* clang-format off to disable clang-format-includes */ +#include "drake/solvers/ibex_solver.h" +/* clang-format on */ + +#include "drake/common/never_destroyed.h" +#include "drake/solvers/mathematical_program.h" + +// This file contains implementations that are common to both the available and +// unavailable flavor of this class. + +namespace drake { +namespace solvers { + +IbexSolver::IbexSolver() + : SolverBase(&id, &is_available, &is_enabled, + &ProgramAttributesSatisfied) {} + +IbexSolver::~IbexSolver() = default; + +SolverId IbexSolver::id() { + static const never_destroyed singleton{"dReal"}; + return singleton.access(); +} + +bool IbexSolver::is_enabled() { return true; } + +bool IbexSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) { + static const never_destroyed solver_capabilities( + std::initializer_list{ + ProgramAttribute::kGenericConstraint, + ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kLorentzConeConstraint, + ProgramAttribute::kRotatedLorentzConeConstraint, + ProgramAttribute::kLinearComplementarityConstraint, + ProgramAttribute::kLinearCost, ProgramAttribute::kQuadraticCost, + ProgramAttribute::kBinaryVariable}); + return AreRequiredAttributesSupported(prog.required_capabilities(), + solver_capabilities.access()); +} + +} // namespace solvers +} // namespace drake diff --git a/solvers/no_ibex.cc b/solvers/no_ibex.cc new file mode 100644 index 000000000000..8b06c8fffeb5 --- /dev/null +++ b/solvers/no_ibex.cc @@ -0,0 +1,23 @@ +/* clang-format off to disable clang-format-includes */ +#include "drake/solvers/ibex_solver.h" +/* clang-format on */ + +#include + +namespace drake { +namespace solvers { + +bool IbexSolver::is_available() { return false; } + +void IbexSolver::DoSolve( + const MathematicalProgram&, + const Eigen::VectorXd&, + const SolverOptions&, + MathematicalProgramResult*) const { + throw std::runtime_error( + "The Ibex bindings were not compiled. You'll need to use a different " + "solver."); +} + +} // namespace solvers +} // namespace drake diff --git a/solvers/test/ibex_solver_test.cc b/solvers/test/ibex_solver_test.cc new file mode 100644 index 000000000000..f52196493f36 --- /dev/null +++ b/solvers/test/ibex_solver_test.cc @@ -0,0 +1,47 @@ +#include "drake/solvers/ibex_solver.h" + +#include + +#include +#include + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/test/generic_trivial_constraints.h" +#include "drake/solvers/test/generic_trivial_costs.h" + +namespace drake { +namespace solvers { +namespace { + +using std::pow; + +// Reproduced from https://github.com/ibex-team/ibex-lib/blob/master/benchs/optim/easy/ex3_1_3.bch +GTEST_TEST(IbexSolverTest, IbexEasyEx3_1_3) { + MathematicalProgram prog; + auto x = prog.NewContinuousVariables(6, "x"); + Vector6d lb, ub, x_d, Q_diag; + lb << 0, 0, 1, 0, 1, 0; + ub << 1e8, 1e8, 5, 6, 5, 10; + Q_diag << 25, 1, 1, 1, 1, 1; + x_d << 2, 2, 1, 4, 1, 4; + prog.AddBoundingBoxConstraint(lb, ub, x); + prog.AddQuadraticErrorCost(Eigen::Matrix(Q_diag.asDiagonal()), + x_d, x); + prog.AddConstraint(pow(x[2] - 3, 2) + x[3] >= 4); + prog.AddConstraint(pow(x[4] - 3, 2) + x[5] >= 4); + prog.AddConstraint(x[0] - 3 * x[1] <= 2); + prog.AddConstraint(-x[0] + x[1] <= 2); + prog.AddConstraint(x[0] + x[1] <= 6); + prog.AddConstraint(x[0] + x[1] >= 2); + + IbexSolver solver; + if (solver.available()) { + auto result = solver.Solve(prog); + EXPECT_TRUE(result.is_success()); + } +} + +} // namespace + +} // namespace solvers +} // namespace drake From 3ec925056291c014087617b3b3b78595c90a9f17 Mon Sep 17 00:00:00 2001 From: Soonho Kong Date: Thu, 24 Jun 2021 15:04:10 -0400 Subject: [PATCH 2/6] cc --- solvers/ibex_solver.cc | 36 +++++++++++++++++++++++++----------- 1 file changed, 25 insertions(+), 11 deletions(-) diff --git a/solvers/ibex_solver.cc b/solvers/ibex_solver.cc index ef226410b9f4..264f5b983847 100644 --- a/solvers/ibex_solver.cc +++ b/solvers/ibex_solver.cc @@ -23,19 +23,18 @@ using Eigen::VectorXd; bool IbexSolver::is_available() { return true; } -void IbexSolver::DoSolve( - const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void IbexSolver::DoSolve(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + const SolverOptions& merged_options, + MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( - "IbexSolver doesn't support the feature of variable scaling."); + "IbexSolver doesn't support the feature of variable scaling."); } unused(initial_guess); unused(merged_options); - + ibex::Variable ibex_vars(prog.num_vars(), "x"); ibex::SystemFactory factory; ibex::IntervalVector bounds(prog.num_vars()); @@ -45,20 +44,35 @@ void IbexSolver::DoSolve( const auto& ub = c->upper_bound(); for (int k = 0; k < static_cast(b.GetNumElements()); ++k) { - const size_t index = - prog.FindDecisionVariableIndex(b.variables()[k]); + const size_t index = prog.FindDecisionVariableIndex(b.variables()[k]); bounds[index] &= ibex::Interval(lb[k], ub[k]); } } factory.add_var(ibex_vars, bounds); - for (const auto&b : prog.linear_constraints()) { + for (const auto& b : prog.linear_constraints()) { symbolic::Formula f{b.evaluator()->CheckSatisfied(b.variables())}; + } + // Build SystemFactory. Add variables and constraints. + system_factory_ = make_unique(); + system_factory_->add_var(ibex_converter_.variables()); + for (const Formula& f : formulas_) { + if (!is_forall(f)) { + unique_ptr expr_ctr{ + ibex_converter_.Convert(f)}; + if (expr_ctr) { + system_factory_->add_ctr(*expr_ctr); + // We need to postpone the destruction of expr_ctr as it is + // still used inside of system_factory_. + expr_ctrs_.push_back(std::move(expr_ctr)); + } + } } + ibex_converter_.set_need_to_delete_variables(true); ibex::System sys(factory); - ibex::DefaultOptimizer o(sys, 1e-07); // TODO: take this as a parameter. + ibex::DefaultOptimizer o(sys, 1e-07); // TODO: take this as a parameter. o.optimize(sys.box); } From 3e61acfa520bb49c43dbfbfcb713f076fee44e59 Mon Sep 17 00:00:00 2001 From: Soonho Kong Date: Thu, 24 Jun 2021 18:42:25 -0400 Subject: [PATCH 3/6] Add IbexConverter and Update IbexSolver --- solvers/BUILD.bazel | 6 +- solvers/ibex_converter.cc | 375 +++++++++++++++++++++++++++++++ solvers/ibex_converter.h | 146 ++++++++++++ solvers/ibex_solver.cc | 131 +++++++++-- solvers/test/ibex_solver_test.cc | 11 +- 5 files changed, 644 insertions(+), 25 deletions(-) create mode 100644 solvers/ibex_converter.cc create mode 100644 solvers/ibex_converter.h diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 80c6f98bc036..918a59329f3c 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -728,6 +728,7 @@ drake_cc_library( name = "ibex_solver", srcs = [ "ibex_solver_common.cc", + "ibex_converter.cc", ] + select({ "//tools:no_dreal": [ "no_ibex.cc", @@ -736,7 +737,10 @@ drake_cc_library( "ibex_solver.cc", ], }), - hdrs = ["ibex_solver.h"], + hdrs = [ + "ibex_converter.h", + "ibex_solver.h", + ], deps = [ ":mathematical_program", ":solver_base", diff --git a/solvers/ibex_converter.cc b/solvers/ibex_converter.cc new file mode 100644 index 000000000000..8bf40c8456b8 --- /dev/null +++ b/solvers/ibex_converter.cc @@ -0,0 +1,375 @@ +#include "drake/solvers/ibex_converter.h" + +#include +#include +#include + +namespace drake { + +namespace solvers { + +using std::numeric_limits; +using std::ostringstream; +using std::runtime_error; +using std::vector; + +using ibex::ExprCtr; +using ibex::ExprNode; + +using symbolic::Expression; +using symbolic::Formula; +using symbolic::Variable; + +namespace { + +/// Returns true if @p v is represented by `int`. +bool is_integer(double v) { + // v should be in [int_min, int_max]. + if (!((numeric_limits::lowest() <= v) && + (v <= numeric_limits::max()))) { + return false; + } + double intpart{}; // dummy variable. + return modf(v, &intpart) == 0.0; +} + +} // namespace + +IbexConverter::IbexConverter( + const Eigen::Ref>& variables) { + // Sets up var_array_ and symbolic_var_to_ibex_var_. + for (int i = 0; i < variables.size(); ++i) { + const Variable& var{variables[i]}; + std::cerr << "var = " << var << "\n"; + // This variable is new to this converter, we need to + // create a corresponding IBEX symbol. + const ibex::ExprSymbol* v{ + &ibex::ExprSymbol::new_(var.get_name().c_str(), ibex::Dim::scalar())}; + // Update the map, Variable → ibex::ExprSymbol*. + symbolic_var_to_ibex_var_.emplace(var.get_id(), v); + // Update the ibex::Array. + var_array_.add(*v); + } + zero_ = &ibex::ExprConstant::new_scalar(0.0); +} + +IbexConverter::~IbexConverter() { + if (need_to_delete_variables_) { + for (const auto& p : symbolic_var_to_ibex_var_) { + delete p.second; + } + } + delete zero_; +} + +const ExprCtr* IbexConverter::Convert(const Formula& f) { + const ExprCtr* expr_ctr{Visit(f, true)}; + if (expr_ctr) { + need_to_delete_variables_ = false; + } + return expr_ctr; +} + +const ExprNode* IbexConverter::Convert(const Expression& e) { + const ExprNode* expr_node{Visit(e)}; + if (expr_node) { + need_to_delete_variables_ = false; + } + return expr_node; +} + +const ibex::Array& IbexConverter::variables() const { + return var_array_; +} + +void IbexConverter::set_need_to_delete_variables(const bool value) { + need_to_delete_variables_ = value; +} + +const ExprNode* IbexConverter::Visit(const Expression& e) { + return ::drake::symbolic::VisitExpression(this, e); +} + +const ExprNode* IbexConverter::VisitVariable(const Expression& e) { + const Variable& var{get_variable(e)}; + auto const it = symbolic_var_to_ibex_var_.find(var.get_id()); + if (it == symbolic_var_to_ibex_var_.cend()) { + throw std::runtime_error(fmt::format( + "Variable {} does not appear in the existing list of variables.", + var.get_name())); + } + return it->second; +} + +const ExprNode* IbexConverter::VisitConstant(const Expression& e) { + return &ibex::ExprConstant::new_scalar(get_constant_value(e)); +} + +const ExprNode* IbexConverter::VisitAddition(const Expression& e) { + const double c{get_constant_in_addition(e)}; + const ExprNode* ret{nullptr}; + if (c != 0) { + ret = &ibex::ExprConstant::new_scalar(c); + } + for (const auto& p : get_expr_to_coeff_map_in_addition(e)) { + const Expression& e_i{p.first}; + const double coeff{p.second}; + if (coeff == 1.0) { + if (ret) { + ret = &(*ret + *Visit(e_i)); + } else { + ret = Visit(e_i); + } + } else if (coeff == -1.0) { + if (ret) { + ret = &(*ret - *Visit(e_i)); + } else { + ret = Visit(-e_i); + } + } else { + if (ret) { + ret = &(*ret + coeff * *Visit(e_i)); + } else { + ret = &(coeff * *Visit(e_i)); + } + } + } + return ret; +} + +const ExprNode* IbexConverter::ProcessPow(const Expression& base, + const Expression& exponent) { + // Note: IBEX provides the following four function signatures of pow + // in "ibex_Expr.h" file: + // + // 1. pow(EXPR, int) + // 2. pow(EXPR, double) + // 3. pow(double, EXPR) + // 4. pow(EXPR, EXPR) + // + // To avoid the loss of precision, we try to avoid calling the last one, + // "pow(EXPR, EXPR)". + if (is_constant(exponent)) { + const double v{get_constant_value(exponent)}; + if (is_integer(v)) { + // Call pow(EXPR, int). + return &pow(*Visit(base), static_cast(v)); + } + if (v == 0.5) { + // Call sqrt(base). + return &sqrt(*Visit(base)); + } else { + // Call pow(EXPR, double). + return &pow(*Visit(base), v); + } + } + if (is_constant(base)) { + // Call pow(double, EXPR). + const double v{get_constant_value(base)}; + return &pow(v, *Visit(exponent)); + } + // Call pow(EXPR, EXPR). + return &pow(*Visit(base), *Visit(exponent)); +} + +const ExprNode* IbexConverter::VisitMultiplication(const Expression& e) { + const double c{get_constant_in_multiplication(e)}; + const ExprNode* ret{nullptr}; + if (c != 1.0) { + ret = &ibex::ExprConstant::new_scalar(c); + } + for (const auto& p : get_base_to_exponent_map_in_multiplication(e)) { + const Expression& base{p.first}; + const Expression& exponent{p.second}; + if (ret) { + ret = &(*ret * *ProcessPow(base, exponent)); + } else { + ret = ProcessPow(base, exponent); + } + } + return ret; +} + +const ExprNode* IbexConverter::VisitDivision(const Expression& e) { + return &(*Visit(get_first_argument(e)) / *Visit(get_second_argument(e))); +} + +const ExprNode* IbexConverter::VisitLog(const Expression& e) { + return &log(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitAbs(const Expression& e) { + return &abs(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitExp(const Expression& e) { + return &exp(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitSqrt(const Expression& e) { + return &sqrt(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitPow(const Expression& e) { + const Expression& base{get_first_argument(e)}; + const Expression& exponent{get_second_argument(e)}; + return ProcessPow(base, exponent); +} + +const ExprNode* IbexConverter::VisitSin(const Expression& e) { + return &sin(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitCos(const Expression& e) { + return &cos(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitTan(const Expression& e) { + return &tan(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitAsin(const Expression& e) { + return &asin(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitAcos(const Expression& e) { + return &acos(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitAtan(const Expression& e) { + return &atan(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitAtan2(const Expression& e) { + return &atan2(*Visit(get_first_argument(e)), *Visit(get_second_argument(e))); +} + +const ExprNode* IbexConverter::VisitSinh(const Expression& e) { + return &sinh(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitCosh(const Expression& e) { + return &cosh(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitTanh(const Expression& e) { + return &tanh(*Visit(get_argument(e))); +} + +const ExprNode* IbexConverter::VisitMin(const Expression& e) { + return &min(*Visit(get_first_argument(e)), *Visit(get_second_argument(e))); +} + +const ExprNode* IbexConverter::VisitMax(const Expression& e) { + return &max(*Visit(get_first_argument(e)), *Visit(get_second_argument(e))); +} + +const ExprNode* IbexConverter::VisitCeil(const Expression& e) { + throw std::runtime_error( + "IbexConverter: Ceil expression is not supported yet."); +} + +const ExprNode* IbexConverter::VisitFloor(const Expression& e) { + throw std::runtime_error( + "IbexConverter: Floor expression is not supported yet."); +} + +const ExprNode* IbexConverter::VisitIfThenElse(const Expression&) { + throw std::runtime_error( + "IbexConverter: If-then-else expression is not supported yet."); +} + +const ExprNode* IbexConverter::VisitUninterpretedFunction(const Expression&) { + throw std::runtime_error( + "IbexConverter: Uninterpreted function is not supported."); +} + +// Visits @p e and converts it into ibex::ExprNode. +const ExprCtr* IbexConverter::Visit(const Formula& f, const bool polarity) { + return ::drake::symbolic::VisitFormula(this, f, polarity); +} + +const ExprCtr* IbexConverter::VisitFalse(const Formula&, const bool) { + throw std::runtime_error("IbexConverter: 'False' is detected."); +} + +const ExprCtr* IbexConverter::VisitTrue(const Formula&, const bool) { + return nullptr; +} + +const ExprCtr* IbexConverter::VisitVariable(const Formula&, const bool) { + throw std::runtime_error("IbexConverter: Boolean variable is detected."); +} + +const ExprCtr* IbexConverter::VisitEqualTo(const Formula& f, + const bool polarity) { + if (polarity) { + return &(*Visit(get_lhs_expression(f) - get_rhs_expression(f)) = *zero_); + } else { + return nullptr; + } +} + +const ExprCtr* IbexConverter::VisitNotEqualTo(const Formula& f, + const bool polarity) { + return VisitEqualTo(f, !polarity); +} + +const ExprCtr* IbexConverter::VisitGreaterThan(const Formula& f, + const bool polarity) { + if (polarity) { + return &(*Visit(get_lhs_expression(f) - get_rhs_expression(f)) > *zero_); + } else { + return &(*Visit(get_lhs_expression(f) - get_rhs_expression(f)) <= *zero_); + } +} + +const ExprCtr* IbexConverter::VisitGreaterThanOrEqualTo(const Formula& f, + const bool polarity) { + if (polarity) { + return &(*Visit(get_lhs_expression(f) - get_rhs_expression(f)) >= *zero_); + } else { + return &(*Visit(get_lhs_expression(f) - get_rhs_expression(f)) < *zero_); + } +} + +const ExprCtr* IbexConverter::VisitLessThan(const Formula& f, + const bool polarity) { + return VisitGreaterThanOrEqualTo(f, !polarity); +} + +const ExprCtr* IbexConverter::VisitLessThanOrEqualTo(const Formula& f, + const bool polarity) { + return VisitGreaterThan(f, !polarity); +} + +const ExprCtr* IbexConverter::VisitConjunction(const Formula&, const bool) { + throw std::runtime_error("IbexConverter: A conjunction is detected."); +} + +const ExprCtr* IbexConverter::VisitDisjunction(const Formula&, const bool) { + throw std::runtime_error("IbexConverter: A disjunction is detected."); +} + +const ExprCtr* IbexConverter::VisitNegation(const Formula& f, + const bool polarity) { + return Visit(get_operand(f), !polarity); +} + +const ExprCtr* IbexConverter::VisitForall(const Formula&, const bool) { + throw std::runtime_error( + "IbexConverter: forall constraint is not supported."); +} + +const ExprCtr* IbexConverter::VisitIsnan(const Formula&, const bool) { + throw std::runtime_error("IbexConverter: Isnan is not supported."); +} + +const ExprCtr* IbexConverter::VisitPositiveSemidefinite(const Formula&, + const bool) { + throw std::runtime_error( + "IbexConverter: PositiveSemidefinite constraint is not supported."); +} + +} // namespace solvers +} // namespace drake diff --git a/solvers/ibex_converter.h b/solvers/ibex_converter.h new file mode 100644 index 000000000000..5da744b86778 --- /dev/null +++ b/solvers/ibex_converter.h @@ -0,0 +1,146 @@ +#pragma once + +#include +#include +#include + +#include "./ibex.h" +#include + +#include "drake/common/symbolic.h" + +namespace drake { +namespace solvers { +/// Visitor class which converts a Drake's symbolic Formula into a corresponding +/// ibex::ExprCtr. +class IbexConverter { + public: + /// Delete the default constructor. + IbexConverter() = delete; + + /// Constructs a converter from @p variables. + explicit IbexConverter( + const Eigen::Ref>& variables); + + ///@{ Delete copy/move constructors and copy/move assign operations. + IbexConverter(const IbexConverter&) = delete; + IbexConverter& operator=(const IbexConverter&) = delete; + IbexConverter(IbexConverter&&) = delete; + IbexConverter& operator=(IbexConverter&&) = delete; + ///@} + + /// Destructor. If `need_to_delete_variables_` is set, delete + /// `ibex::ExprSymbol*` in `symbolic_var_to_ibex_var_`. + ~IbexConverter(); + + /// Converts @p f into a corresponding IBEX data structure, + /// ibex::ExprCtr*. + /// + /// @note It is the caller's responsibility to delete the return value. So + /// far, it is always the case that the returned value is used to construct an + /// `ibex::Function` object. Simply deleting `ibex::ExprCtr` does not destruct + /// the included `ibex::ExprNode` objects. However, `ibex::Function`'s + /// destructor does the job. As a result, we should not call `ibex::cleanup` + /// function explicitly with the return value of this method. + const ibex::ExprCtr* Convert(const symbolic::Formula& f); + + /// Converts @p e into the corresponding IBEX data structure, + /// ibex::ExprNode*. + /// + /// @note See the above note in `Convert(const Formula& f)`. + const ibex::ExprNode* Convert(const symbolic::Expression& e); + + const ibex::Array& variables() const; + + void set_need_to_delete_variables(bool value); + + private: + // Visits @p e and converts it into ibex::ExprNode. + const ibex::ExprNode* Visit(const symbolic::Expression& e); + const ibex::ExprNode* VisitVariable(const symbolic::Expression& e); + const ibex::ExprNode* VisitConstant(const symbolic::Expression& e); + const ibex::ExprNode* VisitRealConstant(const symbolic::Expression& e); + const ibex::ExprNode* VisitAddition(const symbolic::Expression& e); + const ibex::ExprNode* VisitMultiplication(const symbolic::Expression& e); + const ibex::ExprNode* VisitDivision(const symbolic::Expression& e); + const ibex::ExprNode* VisitLog(const symbolic::Expression& e); + const ibex::ExprNode* VisitAbs(const symbolic::Expression& e); + const ibex::ExprNode* VisitExp(const symbolic::Expression& e); + const ibex::ExprNode* VisitSqrt(const symbolic::Expression& e); + const ibex::ExprNode* ProcessPow(const symbolic::Expression& base, + const symbolic::Expression& exponent); + const ibex::ExprNode* VisitPow(const symbolic::Expression& e); + const ibex::ExprNode* VisitSin(const symbolic::Expression& e); + const ibex::ExprNode* VisitCos(const symbolic::Expression& e); + const ibex::ExprNode* VisitTan(const symbolic::Expression& e); + const ibex::ExprNode* VisitAsin(const symbolic::Expression& e); + const ibex::ExprNode* VisitAcos(const symbolic::Expression& e); + const ibex::ExprNode* VisitAtan(const symbolic::Expression& e); + const ibex::ExprNode* VisitAtan2(const symbolic::Expression& e); + const ibex::ExprNode* VisitSinh(const symbolic::Expression& e); + const ibex::ExprNode* VisitCosh(const symbolic::Expression& e); + const ibex::ExprNode* VisitTanh(const symbolic::Expression& e); + const ibex::ExprNode* VisitMin(const symbolic::Expression& e); + const ibex::ExprNode* VisitMax(const symbolic::Expression& e); + const ibex::ExprNode* VisitIfThenElse(const symbolic::Expression&); + const ibex::ExprNode* VisitCeil(const symbolic::Expression& e); + const ibex::ExprNode* VisitFloor(const symbolic::Expression& e); + const ibex::ExprNode* VisitUninterpretedFunction(const symbolic::Expression&); + + // Visits @p e and converts it into ibex::ibex::ExprNode. + const ibex::ExprCtr* Visit(const symbolic::Formula& f, bool polarity); + const ibex::ExprCtr* VisitFalse(const symbolic::Formula&, bool); + const ibex::ExprCtr* VisitTrue(const symbolic::Formula&, bool); + const ibex::ExprCtr* VisitVariable(const symbolic::Formula&, bool); + const ibex::ExprCtr* VisitEqualTo(const symbolic::Formula& f, bool polarity); + const ibex::ExprCtr* VisitNotEqualTo(const symbolic::Formula& f, + bool polarity); + const ibex::ExprCtr* VisitGreaterThan(const symbolic::Formula& f, + bool polarity); + const ibex::ExprCtr* VisitGreaterThanOrEqualTo(const symbolic::Formula& f, + bool polarity); + const ibex::ExprCtr* VisitLessThan(const symbolic::Formula& f, bool polarity); + const ibex::ExprCtr* VisitLessThanOrEqualTo(const symbolic::Formula& f, + bool polarity); + const ibex::ExprCtr* VisitConjunction(const symbolic::Formula&, bool); + const ibex::ExprCtr* VisitDisjunction(const symbolic::Formula&, bool); + const ibex::ExprCtr* VisitNegation(const symbolic::Formula& f, bool polarity); + const ibex::ExprCtr* VisitForall(const symbolic::Formula&, bool); + const ibex::ExprCtr* VisitIsnan(const symbolic::Formula&, bool); + const ibex::ExprCtr* VisitPositiveSemidefinite(const symbolic::Formula&, + bool); + + // --------------- + // Member fields + // --------------- + + // True if we need to delete the variables (ibex::ExprSymbol + // objects) in the destructor. At initialization, this is true. But + // when `Convert()` method returns a non-null pointer, it changes to + // false assuming that the caller will delete the variables. + bool need_to_delete_variables_{true}; + + // Map : symbolic::Variable → ibex::ExprSymbol*. + std::unordered_map + symbolic_var_to_ibex_var_; + + ibex::Array var_array_; + + // Represents the value `0.0`. We use this to avoid a possible + // memory leak caused by IBEX code: See + // https://github.com/ibex-team/ibex-lib/blob/af48e38847414818913b6954e1b1b3050aa14593/src/symbolic/ibex_ExprCtr.h#L53-L55 + const ibex::ExprNode* zero_; + + // Makes VisitFormula a friend of this class so that it can use private + // operator()s. + friend const ibex::ExprCtr* ::drake::symbolic::VisitFormula< + const ibex::ExprCtr*>(IbexConverter*, const symbolic::Formula&, + const bool&); + // Makes VisitExpression a friend of this class so that it can use private + // operator()s. + friend const ibex::ExprNode* ::drake::symbolic::VisitExpression< + const ibex::ExprNode*>(IbexConverter*, const symbolic::Expression&); +}; + +} // namespace solvers +} // namespace drake diff --git a/solvers/ibex_solver.cc b/solvers/ibex_solver.cc index 264f5b983847..66e6190dff5f 100644 --- a/solvers/ibex_solver.cc +++ b/solvers/ibex_solver.cc @@ -14,6 +14,7 @@ #include "./ibex.h" #include "drake/common/text_logging.h" +#include "drake/solvers/ibex_converter.h" #include "drake/solvers/mathematical_program.h" namespace drake { @@ -21,6 +22,53 @@ namespace solvers { using Eigen::VectorXd; +using std::shared_ptr; +using std::vector; + +using symbolic::Expression; +using symbolic::Formula; +using symbolic::Variable; + +namespace { +// Custom deleter for ibex::ExprCtr. It deletes the internal +// ibex::ExprNode while keeping the ExprSymbols intact. Note that the +// ExprSymbols will be deleted separately in +// ~ContractorIbexPolytope(). +struct ExprCtrDeleter { + void operator()(const ibex::ExprCtr* const p) const { + if (p) { + ibex::cleanup(p->e, false); + delete p; + } + } +}; + +// Extracts linear costs in @p prog and push them into @p costs vector. +void ExtractLinearCosts(const MathematicalProgram& prog, + vector* const costs) { + for (const Binding& binding : prog.linear_costs()) { + const VectorXDecisionVariable& x{binding.variables()}; + const shared_ptr& cost{binding.evaluator()}; + VectorX y; + cost->Eval(x, &y); + costs->push_back(y[0]); + } +} + +// Extracts quadratic costs in @p prog and push them into @p costs vector. +void ExtractQuadraticCosts(const MathematicalProgram& prog, + vector* const costs) { + for (const Binding& binding : prog.quadratic_costs()) { + const VectorXDecisionVariable& x{binding.variables()}; + const shared_ptr& cost{binding.evaluator()}; + VectorX y; + cost->Eval(x, &y); + costs->push_back(y[0]); + } +} + +} // namespace + bool IbexSolver::is_available() { return true; } void IbexSolver::DoSolve(const MathematicalProgram& prog, @@ -35,8 +83,12 @@ void IbexSolver::DoSolve(const MathematicalProgram& prog, unused(initial_guess); unused(merged_options); - ibex::Variable ibex_vars(prog.num_vars(), "x"); + // Creates a converter. Internally, it creates ibex variables. + IbexConverter ibex_converter(prog.decision_variables()); + ibex::SystemFactory factory; + + // Prepares bounds for variables. ibex::IntervalVector bounds(prog.num_vars()); for (const auto& b : prog.bounding_box_constraints()) { const auto& c = b.evaluator(); @@ -46,35 +98,76 @@ void IbexSolver::DoSolve(const MathematicalProgram& prog, for (int k = 0; k < static_cast(b.GetNumElements()); ++k) { const size_t index = prog.FindDecisionVariableIndex(b.variables()[k]); bounds[index] &= ibex::Interval(lb[k], ub[k]); + std::cerr << "Bound: " << b.variables()[k] << "[" << lb[k] << ", " + << ub[k] << "]\n"; } } - factory.add_var(ibex_vars, bounds); + // Adds ibex variables + bounds into the factory. + factory.add_var(ibex_converter.variables(), bounds); + + // This expr_ctrs holds the translated ibex::ExprCtr* objects. After adding + // the ibex::Ctrs to the factory, we should not destruct them at the end of + // the loop where they are created because they are used in the system factory + // and the system. + std::vector> expr_ctrs; + + // Add constraints. for (const auto& b : prog.linear_constraints()) { - symbolic::Formula f{b.evaluator()->CheckSatisfied(b.variables())}; + Formula f{b.evaluator()->CheckSatisfied(b.variables())}; + std::cerr << "Add constraint: " << f << "\n"; + std::unique_ptr expr_ctr{ + ibex_converter.Convert(f)}; + if (expr_ctr) { + factory.add_ctr(*expr_ctr); + expr_ctrs.push_back(std::move(expr_ctr)); + } + ibex_converter.set_need_to_delete_variables(true); } - // Build SystemFactory. Add variables and constraints. - system_factory_ = make_unique(); - system_factory_->add_var(ibex_converter_.variables()); - for (const Formula& f : formulas_) { - if (!is_forall(f)) { - unique_ptr expr_ctr{ - ibex_converter_.Convert(f)}; - if (expr_ctr) { - system_factory_->add_ctr(*expr_ctr); - // We need to postpone the destruction of expr_ctr as it is - // still used inside of system_factory_. - expr_ctrs_.push_back(std::move(expr_ctr)); - } + // Extract costs + vector costs; + ExtractLinearCosts(prog, &costs); + ExtractQuadraticCosts(prog, &costs); + + // Add costs into the factory. + std::vector> expr_nodes; + for (const Expression& cost : costs) { + std::cerr << "Add cost: " << cost << "\n"; + std::unique_ptr expr_node{ + ibex_converter.Convert(cost)}; + if (expr_node) { + factory.add_goal(*expr_node); + // We need to postpone the destruction of expr_ctr as it is + // still used inside of system_factory. + expr_nodes.push_back(std::move(expr_node)); } } - ibex_converter_.set_need_to_delete_variables(true); ibex::System sys(factory); - ibex::DefaultOptimizer o(sys, 1e-07); // TODO: take this as a parameter. + std::cerr << "Solving...\n"; + + const double rel_eps_f = ibex::Optimizer::default_rel_eps_f; + const double abs_eps_f = ibex::Optimizer::default_abs_eps_f; + const double eps_h = ibex::NormalizedSystem::default_eps_h; + const bool rigor = false; + const bool inHC4 = !(sys.nb_ctr > 0 && sys.nb_ctr < sys.f_ctrs.image_dim()); + const double random_seed = ibex::DefaultOptimizer::default_random_seed; + const double eps_x = ibex::Optimizer::default_eps_x; + const double timeout = 30.0; /* sec */ + const bool trace = false; + + std::cerr << sys << "\n"; + + ibex::DefaultOptimizer o(sys, rel_eps_f, abs_eps_f, eps_h, rigor, inHC4, + random_seed, eps_x); + o.trace = trace; + o.timeout = timeout; o.optimize(sys.box); -} + std::cerr << "Solving... Done.\n"; + o.report(); + // TODO: Need to set the result... +} } // namespace solvers } // namespace drake diff --git a/solvers/test/ibex_solver_test.cc b/solvers/test/ibex_solver_test.cc index f52196493f36..67256ba579f0 100644 --- a/solvers/test/ibex_solver_test.cc +++ b/solvers/test/ibex_solver_test.cc @@ -15,18 +15,19 @@ namespace { using std::pow; -// Reproduced from https://github.com/ibex-team/ibex-lib/blob/master/benchs/optim/easy/ex3_1_3.bch +// Reproduced from +// https://github.com/ibex-team/ibex-lib/blob/master/benchs/optim/easy/ex3_1_3.bch GTEST_TEST(IbexSolverTest, IbexEasyEx3_1_3) { MathematicalProgram prog; auto x = prog.NewContinuousVariables(6, "x"); Vector6d lb, ub, x_d, Q_diag; lb << 0, 0, 1, 0, 1, 0; ub << 1e8, 1e8, 5, 6, 5, 10; - Q_diag << 25, 1, 1, 1, 1, 1; - x_d << 2, 2, 1, 4, 1, 4; prog.AddBoundingBoxConstraint(lb, ub, x); - prog.AddQuadraticErrorCost(Eigen::Matrix(Q_diag.asDiagonal()), - x_d, x); + + prog.AddCost(-25 * pow(x[0] - 2, 2) - pow(x[1] - 2, 2) - pow(x[2] - 1, 2) - + pow(x[3] - 4, 2) - pow(x[4] - 1, 2) - pow(x[5] - 4, 2)); + prog.AddConstraint(pow(x[2] - 3, 2) + x[3] >= 4); prog.AddConstraint(pow(x[4] - 3, 2) + x[5] >= 4); prog.AddConstraint(x[0] - 3 * x[1] <= 2); From dbf0eadcf874de50567b5bc1dfadf063fcfcf715 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Sun, 11 Jul 2021 22:24:27 -0400 Subject: [PATCH 4/6] WIP: IRIS in configuration space --- geometry/optimization/BUILD.bazel | 5 + geometry/optimization/iris.cc | 218 ++++++++++++++++++++++-- geometry/optimization/iris.h | 16 ++ geometry/optimization/test/iris_test.cc | 190 ++++++++++++++++++++- 4 files changed, 409 insertions(+), 20 deletions(-) diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 733d67e26d4a..61a341ab903b 100644 --- a/geometry/optimization/BUILD.bazel +++ b/geometry/optimization/BUILD.bazel @@ -49,6 +49,11 @@ drake_cc_library( deps = [ ":convex_set", "//geometry:scene_graph", + "//multibody/plant", + "//multibody/parsing:parser", + "//solvers:ibex_solver", + "//solvers:snopt_solver", + "//systems/framework:diagram_builder", ], ) diff --git a/geometry/optimization/iris.cc b/geometry/optimization/iris.cc index 0fbbeecebceb..e20aaadb8f1b 100644 --- a/geometry/optimization/iris.cc +++ b/geometry/optimization/iris.cc @@ -7,6 +7,9 @@ #include #include "drake/geometry/optimization/convex_set.h" +#include "drake/multibody/plant/multibody_plant.h" +#include "drake/solvers/ibex_solver.h" +#include "drake/solvers/snopt_solver.h" namespace drake { namespace geometry { @@ -15,6 +18,7 @@ namespace optimization { using Eigen::MatrixXd; using Eigen::Ref; using Eigen::VectorXd; +using symbolic::Expression; HPolyhedron Iris(const ConvexSets& obstacles, const Ref& sample, const HPolyhedron& domain, const IrisOptions& options) { @@ -106,43 +110,53 @@ class IrisConvexSetMaker final : public ShapeReifier { std::optional reference_frame) : query_{query}, reference_frame_{reference_frame} {}; + void set_reference_frame(const FrameId& reference_frame) { + DRAKE_DEMAND(reference_frame.is_valid()); + *reference_frame_ = reference_frame; + } + + void set_geometry_id(const GeometryId& geom_id) { + geom_id_ = geom_id; + } + using ShapeReifier::ImplementGeometry; void ImplementGeometry(const Sphere&, void* data) { - GeometryId& geom_id = *static_cast(data); - sets_.emplace_back( - std::make_unique(query_, geom_id, reference_frame_)); + DRAKE_DEMAND(geom_id_.is_valid()); + auto& set = *static_cast*>(data); + set = + std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const HalfSpace&, void* data) { - GeometryId& geom_id = *static_cast(data); - sets_.emplace_back( - std::make_unique(query_, geom_id, reference_frame_)); + DRAKE_DEMAND(geom_id_.is_valid()); + auto& set = *static_cast*>(data); + set = + std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const Box&, void* data) { - GeometryId& geom_id = *static_cast(data); + DRAKE_DEMAND(geom_id_.is_valid()); + auto& set = *static_cast*>(data); // Note: We choose HPolyhedron over VPolytope here, but the IRIS paper // discusses a significant performance improvement using a "least-distance // programming" instance from CVXGEN that exploited the VPolytope // representation. So we may wish to revisit this. - sets_.emplace_back( - std::make_unique(query_, geom_id, reference_frame_)); + set = + std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const Ellipsoid&, void* data) { - GeometryId& geom_id = *static_cast(data); - sets_.emplace_back( - std::make_unique(query_, geom_id, reference_frame_)); + DRAKE_DEMAND(geom_id_.is_valid()); + auto& set = *static_cast*>(data); + set = + std::make_unique(query_, geom_id_, reference_frame_); } - // This must be called last; it transfers ownership. - ConvexSets claim_sets() { return std::move(sets_); } - private: const QueryObject& query_{}; std::optional reference_frame_{}; - ConvexSets sets_{}; + GeometryId geom_id_{}; }; } // namespace @@ -153,12 +167,180 @@ ConvexSets MakeIrisObstacles(const QueryObject& query_object, const GeometrySet all_ids(inspector.GetAllGeometryIds()); const std::unordered_set geom_ids = inspector.GetGeometryIds(all_ids, Role::kProximity); + ConvexSets sets(geom_ids.size()); IrisConvexSetMaker maker(query_object, reference_frame); + int count=0; + for (GeometryId geom_id : geom_ids) { + maker.set_geometry_id(geom_id); + inspector.GetShape(geom_id).Reify(&maker, &sets[count++]); + } + return sets; +} + +namespace { + +// Solves the optimization +// min_q (q-d)*CᵀC(q-d) +// s.t. setA on bodyA and setB on bodyB are in collision in q. +// Aq ≤ b. +// where C, d are the center and matrix from the hyperellipsoid E. +// Returns true iff a collision is found. +// Sets `closest` to an optimizing solution q*, if a solution is found. +bool FindClosestCollision(const multibody::MultibodyPlant& plant, + const multibody::Body& bodyA, + const multibody::Body& bodyB, + const ConvexSet& setA, const ConvexSet& setB, + const Hyperellipsoid& E, + const Eigen::Ref& A, + const Eigen::Ref& b, + const solvers::SolverBase& solver, + systems::Context* context, + VectorXd* closest) { + solvers::MathematicalProgram prog; + auto q = prog.NewContinuousVariables(plant.num_positions(), "q"); + + prog.AddLinearConstraint( + A, VectorXd::Constant(b.size(), -std::numeric_limits::infinity()), + b, q); + prog.AddQuadraticErrorCost(E.A().transpose() * E.A(), E.center(), q); + + auto p_AA = prog.NewContinuousVariables<3>("p_AA"); + auto p_BB = prog.NewContinuousVariables<3>("p_BB"); + setA.AddPointInSetConstraints(&prog, p_AA); + setB.AddPointInSetConstraints(&prog, p_BB); + + plant.SetPositions(context, q); + const math::RigidTransform X_WA = + plant.EvalBodyPoseInWorld(*context, bodyA); + const math::RigidTransform X_WB = + plant.EvalBodyPoseInWorld(*context, bodyB); + prog.AddConstraint(X_WA * p_AA.cast() == + X_WB * p_BB.cast()); + + solvers::MathematicalProgramResult result = solver.Solve(prog); + if (result.is_success()) { + *closest = result.GetSolution(q); + return true; + } + return false; +} + +} // namespace + +HPolyhedron IrisInConfigurationSpace( + const multibody::MultibodyPlant& plant, + const QueryObject& query_object, + const Eigen::Ref& sample, + const IrisOptions& options) { + const int nq = plant.num_positions(); + DRAKE_DEMAND(sample.size() == nq); + + // Note: We require finite joint limits to define the bounding box for the + // IRIS algorithm. + DRAKE_DEMAND(plant.GetPositionLowerLimits().array().isFinite().all()); + DRAKE_DEMAND(plant.GetPositionUpperLimits().array().isFinite().all()); + HPolyhedron P = HPolyhedron::MakeBox(plant.GetPositionLowerLimits(), + plant.GetPositionUpperLimits()); + DRAKE_DEMAND(P.A().rows() == 2*nq); + + const double kEpsilonEllipsoid = 1e-2; + Hyperellipsoid E = Hyperellipsoid::MakeHypersphere(kEpsilonEllipsoid, sample); + + const SceneGraphInspector& inspector = query_object.inspector(); + + // Make all of the convex sets. TODO(russt): Consider factoring this out so + // that I don't have to redo the work for every new region. + IrisConvexSetMaker maker(query_object, inspector.world_frame_id()); + std::unordered_map> sets{}; + std::unordered_map&> bodies{}; + const std::unordered_set geom_ids = inspector.GetGeometryIds( + GeometrySet(inspector.GetAllGeometryIds()), Role::kProximity); + copyable_unique_ptr temp_set; for (GeometryId geom_id : geom_ids) { - inspector.GetShape(geom_id).Reify(&maker, &geom_id); + // Make all sets in the local geometry frame. + FrameId frame_id = inspector.GetFrameId(geom_id); + maker.set_reference_frame(frame_id); + maker.set_geometry_id(geom_id); + inspector.GetShape(geom_id).Reify(&maker, &temp_set); + sets.emplace(geom_id, std::move(temp_set)); + bodies.emplace(geom_id, *plant.GetBodyFromFrameId(frame_id)); + } + + auto pairs = inspector.GetCollisionCandidates(); + const int N = static_cast(pairs.size()); + + // On each iteration, we will build the collision-free polytope represented as + // {x | A * x <= b}. Here we pre-allocate matrices of the maximum size. + Eigen::Matrix A( + P.A().rows() + N, nq); + VectorXd b(P.A().rows() + N); + A.topRows(P.A().rows()) = P.A(); + b.head(P.A().rows()) = P.b(); + + double best_volume = E.Volume(); + int iteration = 0; + MatrixXd tangent_matrix; + + //solvers::IbexSolver solver; + solvers::SnoptSolver solver; + DRAKE_DEMAND(solver.is_available() && solver.is_enabled()); + auto context = plant.CreateDefaultContext(); + VectorXd closest(nq); + + while (true) { + tangent_matrix = 2.0 * E.A().transpose() * E.A(); + int num_constraints = 2*nq; // Start with just the joint limits. + // Find separating hyperplanes + for (const auto [geomA, geomB] : pairs) { + while (true) { + const bool collision = FindClosestCollision( + plant, bodies.at(geomA), bodies.at(geomB), *sets.at(geomA), + *sets.at(geomB), E, A.topRows(num_constraints), + b.head(num_constraints), solver, context.get(), &closest); + std::cout << "geomA=" << inspector.GetName(geomA) + << ", geomB=" << inspector.GetName(geomB); + if (collision) { + std::cout << ", closest=" << closest.transpose() << std::endl; + // Add the tangent to the (scaled) ellipsoid at this point as a + // constraint. + A.row(num_constraints) = + (tangent_matrix * (closest - E.center())).normalized(); + b[num_constraints] = A.row(num_constraints) * closest - + options.configuration_space_margin; + std::cout << " " + << A.row(num_constraints) * + symbolic::MakeVectorVariable(nq, "q") + << " ≤ " << b[num_constraints] << std::endl; + num_constraints++; + } else { + std::cout << ". No collisions." << std::endl; + break; + } + } + } + + if (options.require_sample_point_is_contained && + ((A.topRows(num_constraints) * sample).array() >= + b.head(num_constraints).array()) + .any()) { + break; + } + P = HPolyhedron(A.topRows(num_constraints), b.head(num_constraints)); + + iteration++; + if (iteration >= options.iteration_limit) { + break; + } + + E = P.MaximumVolumeInscribedEllipsoid(); + const double volume = E.Volume(); + if (volume - best_volume <= options.termination_threshold) { + break; + } + best_volume = volume; } - return maker.claim_sets(); + return P; } } // namespace optimization diff --git a/geometry/optimization/iris.h b/geometry/optimization/iris.h index 30bffe95ecb5..ef6a58a1a044 100644 --- a/geometry/optimization/iris.h +++ b/geometry/optimization/iris.h @@ -4,8 +4,10 @@ #include #include +#include "drake/common/symbolic.h" #include "drake/geometry/optimization/convex_set.h" #include "drake/geometry/optimization/hpolyhedron.h" +#include "drake/multibody/plant/multibody_plant.h" namespace drake { namespace geometry { @@ -32,6 +34,12 @@ struct IrisOptions { /** IRIS will terminate if the change in the *volume* of the hyperellipsoid between iterations is less that this threshold. */ double termination_threshold{2e-2}; // from rdeits/iris-distro. + + /** For IRIS in configuration space, we retreat by this margin from each + C-space obstacle in order to avoid the possibility of requiring an infinite + number of faces to approximate a curved boundary. TODO: clarify the units. + */ + double configuration_space_margin{1e-2}; }; /** The IRIS (Iterative Region Inflation by Semidefinite programming) algorithm, @@ -85,6 +93,14 @@ ConvexSets MakeIrisObstacles( const QueryObject& query_object, std::optional reference_frame = std::nullopt); +// TODO(russt): Pass a context of the plant if/when we support setting kinematic +// parameters. +HPolyhedron IrisInConfigurationSpace( + const multibody::MultibodyPlant& plant, + const QueryObject& query_object, + const Eigen::Ref& sample, + const IrisOptions& options = IrisOptions()); + } // namespace optimization } // namespace geometry } // namespace drake diff --git a/geometry/optimization/test/iris_test.cc b/geometry/optimization/test/iris_test.cc index 818bfe5c701f..940d9d974923 100644 --- a/geometry/optimization/test/iris_test.cc +++ b/geometry/optimization/test/iris_test.cc @@ -7,6 +7,8 @@ #include "drake/geometry/optimization/vpolytope.h" #include "drake/geometry/scene_graph.h" #include "drake/math/rigid_transform.h" +#include "drake/multibody/parsing/parser.h" +#include "drake/systems/framework/diagram_builder.h" namespace drake { namespace geometry { @@ -110,8 +112,7 @@ GTEST_TEST(IrisTest, BallInBox) { └─────┴─┴─────┘ */ GTEST_TEST(IrisTest, MultipleBoxes) { ConvexSets obstacles; - obstacles.emplace_back( - VPolytope::MakeBox(Vector2d(.1, .5), Vector2d(1, 1))); + obstacles.emplace_back(VPolytope::MakeBox(Vector2d(.1, .5), Vector2d(1, 1))); obstacles.emplace_back( VPolytope::MakeBox(Vector2d(-1, -1), Vector2d(-.1, -.5))); obstacles.emplace_back( @@ -232,6 +233,191 @@ TEST_F(SceneGraphTester, MultipleBoxes) { EXPECT_TRUE(region.PointInSet(Vector3d(0.0, 0.0, -0.99))); } +namespace { + +// Helper method for testing IrisInConfigurationSpace from a urdf string. +HPolyhedron IrisFromUrdf(const std::string urdf, + const Eigen::Ref& sample) { + systems::DiagramBuilder builder; + auto [plant, scene_graph] = + multibody::AddMultibodyPlantSceneGraph(&builder, 0.0); + multibody::Parser(&plant).AddModelFromString(urdf, "urdf"); + plant.Finalize(); + auto diagram = builder.Build(); + + auto context = diagram->CreateDefaultContext(); + auto query_object = + scene_graph.get_query_output_port().Eval>( + scene_graph.GetMyContextFromRoot(*context)); + + std::unique_ptr> + symbolic_plant = systems::System::ToSymbolic(plant); + return IrisInConfigurationSpace(*symbolic_plant, query_object, sample); +} + +} // namespace + +// One prismatic link with joint limits. Iris should return the joint limits. +GTEST_TEST(IrisInConfigurationSpaceTest, JointLimits) { + const std::string limits_urdf = R"( + + + + + + + + + + + + + +)"; + + const Vector1d sample = Vector1d::Zero(); + HPolyhedron region = IrisFromUrdf(limits_urdf, sample); + + EXPECT_EQ(region.ambient_dimension(), 1); + EXPECT_TRUE(region.PointInSet(Vector1d{1.99})); + EXPECT_TRUE(region.PointInSet(Vector1d{-1.99})); + EXPECT_FALSE(region.PointInSet(Vector1d{2.01})); + EXPECT_FALSE(region.PointInSet(Vector1d{-2.01})); +} + +// Three boxes. Two on the outside are fixed. One in the middle on a prismatic +// joint. The configuration space is a (convex) line segment q ∈ (−1,1). +GTEST_TEST(IrisInConfigurationSpaceTest, BoxesPrismatic) { + const std::string boxes_urdf = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + const Vector1d sample = Vector1d::Zero(); + HPolyhedron region = IrisFromUrdf(boxes_urdf, sample); + + EXPECT_EQ(region.ambient_dimension(), 1); + EXPECT_TRUE(region.PointInSet(Vector1d{0.98})); + EXPECT_TRUE(region.PointInSet(Vector1d{-0.98})); + EXPECT_FALSE(region.PointInSet(Vector1d{1.02})); + EXPECT_FALSE(region.PointInSet(Vector1d{-1.02})); +} + +// Three spheres. Two on the outside are fixed. One in the middle on a +// prismatic joint. The configuration space is a (convex) line segment q ∈ +// (−1,1). +GTEST_TEST(IrisInConfigurationSpaceTest, SpheresPrismatic) { + const std::string spheres_urdf = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + const Vector1d sample = Vector1d::Zero(); + HPolyhedron region = IrisFromUrdf(spheres_urdf, sample); + + EXPECT_EQ(region.ambient_dimension(), 1); + EXPECT_TRUE(region.PointInSet(Vector1d{0.98})); + EXPECT_TRUE(region.PointInSet(Vector1d{-0.98})); + EXPECT_FALSE(region.PointInSet(Vector1d{1.02})); + EXPECT_FALSE(region.PointInSet(Vector1d{-1.02})); +} + +/* +// A pendulum between two (fixed) boxes. The configuration space is a (convex) +// line segment q ∈ (−1,1). +GTEST_TEST(IrisInConfigurationSpaceTest, Pendulum) { + const std::string pendulum_urdf = R"( + + + + TODO: Finish this... + + + + + + + + + + + + + + + + + + + + + + + + +)"; + + const Vector1d sample = Vector1d::Zero(); + HPolyhedron region = IrisFromUrdf(boxes_urdf, sample); + + EXPECT_EQ(region.ambient_dimension(), 1); + EXPECT_TRUE(region.PointInSet(Vector1d{0.98})); + EXPECT_TRUE(region.PointInSet(Vector1d{-0.98})); + EXPECT_FALSE(region.PointInSet(Vector1d{1.02})); + EXPECT_FALSE(region.PointInSet(Vector1d{-1.02})); +} +*/ + } // namespace } // namespace optimization } // namespace geometry From 15a55d303f483201b86f0f33875e18a6c9877d6f Mon Sep 17 00:00:00 2001 From: Soonho Kong Date: Sat, 24 Jul 2021 23:49:57 -0400 Subject: [PATCH 5/6] lint brush --- geometry/optimization/BUILD.bazel | 2 +- geometry/optimization/iris.cc | 48 ++++++++++++------------- geometry/optimization/test/iris_test.cc | 12 +++---- 3 files changed, 29 insertions(+), 33 deletions(-) diff --git a/geometry/optimization/BUILD.bazel b/geometry/optimization/BUILD.bazel index 61a341ab903b..34f7e1990ef0 100644 --- a/geometry/optimization/BUILD.bazel +++ b/geometry/optimization/BUILD.bazel @@ -49,8 +49,8 @@ drake_cc_library( deps = [ ":convex_set", "//geometry:scene_graph", - "//multibody/plant", "//multibody/parsing:parser", + "//multibody/plant", "//solvers:ibex_solver", "//solvers:snopt_solver", "//systems/framework:diagram_builder", diff --git a/geometry/optimization/iris.cc b/geometry/optimization/iris.cc index e20aaadb8f1b..c006b33d82d9 100644 --- a/geometry/optimization/iris.cc +++ b/geometry/optimization/iris.cc @@ -1,7 +1,9 @@ #include "drake/geometry/optimization/iris.h" #include +#include #include +#include #include #include #include @@ -115,24 +117,20 @@ class IrisConvexSetMaker final : public ShapeReifier { *reference_frame_ = reference_frame; } - void set_geometry_id(const GeometryId& geom_id) { - geom_id_ = geom_id; - } + void set_geometry_id(const GeometryId& geom_id) { geom_id_ = geom_id; } using ShapeReifier::ImplementGeometry; void ImplementGeometry(const Sphere&, void* data) { DRAKE_DEMAND(geom_id_.is_valid()); auto& set = *static_cast*>(data); - set = - std::make_unique(query_, geom_id_, reference_frame_); + set = std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const HalfSpace&, void* data) { DRAKE_DEMAND(geom_id_.is_valid()); auto& set = *static_cast*>(data); - set = - std::make_unique(query_, geom_id_, reference_frame_); + set = std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const Box&, void* data) { @@ -142,15 +140,13 @@ class IrisConvexSetMaker final : public ShapeReifier { // discusses a significant performance improvement using a "least-distance // programming" instance from CVXGEN that exploited the VPolytope // representation. So we may wish to revisit this. - set = - std::make_unique(query_, geom_id_, reference_frame_); + set = std::make_unique(query_, geom_id_, reference_frame_); } void ImplementGeometry(const Ellipsoid&, void* data) { DRAKE_DEMAND(geom_id_.is_valid()); auto& set = *static_cast*>(data); - set = - std::make_unique(query_, geom_id_, reference_frame_); + set = std::make_unique(query_, geom_id_, reference_frame_); } private: @@ -170,7 +166,7 @@ ConvexSets MakeIrisObstacles(const QueryObject& query_object, ConvexSets sets(geom_ids.size()); IrisConvexSetMaker maker(query_object, reference_frame); - int count=0; + int count = 0; for (GeometryId geom_id : geom_ids) { maker.set_geometry_id(geom_id); inspector.GetShape(geom_id).Reify(&maker, &sets[count++]); @@ -188,15 +184,15 @@ namespace { // Returns true iff a collision is found. // Sets `closest` to an optimizing solution q*, if a solution is found. bool FindClosestCollision(const multibody::MultibodyPlant& plant, - const multibody::Body& bodyA, - const multibody::Body& bodyB, - const ConvexSet& setA, const ConvexSet& setB, - const Hyperellipsoid& E, - const Eigen::Ref& A, - const Eigen::Ref& b, - const solvers::SolverBase& solver, - systems::Context* context, - VectorXd* closest) { + const multibody::Body& bodyA, + const multibody::Body& bodyB, + const ConvexSet& setA, const ConvexSet& setB, + const Hyperellipsoid& E, + const Eigen::Ref& A, + const Eigen::Ref& b, + const solvers::SolverBase& solver, + systems::Context* context, + VectorXd* closest) { solvers::MathematicalProgram prog; auto q = prog.NewContinuousVariables(plant.num_positions(), "q"); @@ -242,7 +238,7 @@ HPolyhedron IrisInConfigurationSpace( DRAKE_DEMAND(plant.GetPositionUpperLimits().array().isFinite().all()); HPolyhedron P = HPolyhedron::MakeBox(plant.GetPositionLowerLimits(), plant.GetPositionUpperLimits()); - DRAKE_DEMAND(P.A().rows() == 2*nq); + DRAKE_DEMAND(P.A().rows() == 2 * nq); const double kEpsilonEllipsoid = 1e-2; Hyperellipsoid E = Hyperellipsoid::MakeHypersphere(kEpsilonEllipsoid, sample); @@ -282,15 +278,15 @@ HPolyhedron IrisInConfigurationSpace( int iteration = 0; MatrixXd tangent_matrix; - //solvers::IbexSolver solver; - solvers::SnoptSolver solver; + // solvers::IbexSolver solver; + solvers::IbexSolver solver; DRAKE_DEMAND(solver.is_available() && solver.is_enabled()); auto context = plant.CreateDefaultContext(); VectorXd closest(nq); while (true) { tangent_matrix = 2.0 * E.A().transpose() * E.A(); - int num_constraints = 2*nq; // Start with just the joint limits. + int num_constraints = 2 * nq; // Start with just the joint limits. // Find separating hyperplanes for (const auto [geomA, geomB] : pairs) { while (true) { @@ -340,7 +336,7 @@ HPolyhedron IrisInConfigurationSpace( } best_volume = volume; } - return P; + return P; } } // namespace optimization diff --git a/geometry/optimization/test/iris_test.cc b/geometry/optimization/test/iris_test.cc index 940d9d974923..fec7e18521f3 100644 --- a/geometry/optimization/test/iris_test.cc +++ b/geometry/optimization/test/iris_test.cc @@ -237,7 +237,7 @@ namespace { // Helper method for testing IrisInConfigurationSpace from a urdf string. HPolyhedron IrisFromUrdf(const std::string urdf, - const Eigen::Ref& sample) { + const Eigen::Ref& sample) { systems::DiagramBuilder builder; auto [plant, scene_graph] = multibody::AddMultibodyPlantSceneGraph(&builder, 0.0); @@ -277,7 +277,7 @@ GTEST_TEST(IrisInConfigurationSpaceTest, JointLimits) { const Vector1d sample = Vector1d::Zero(); HPolyhedron region = IrisFromUrdf(limits_urdf, sample); - + EXPECT_EQ(region.ambient_dimension(), 1); EXPECT_TRUE(region.PointInSet(Vector1d{1.99})); EXPECT_TRUE(region.PointInSet(Vector1d{-1.99})); @@ -320,7 +320,7 @@ GTEST_TEST(IrisInConfigurationSpaceTest, BoxesPrismatic) { const Vector1d sample = Vector1d::Zero(); HPolyhedron region = IrisFromUrdf(boxes_urdf, sample); - + EXPECT_EQ(region.ambient_dimension(), 1); EXPECT_TRUE(region.PointInSet(Vector1d{0.98})); EXPECT_TRUE(region.PointInSet(Vector1d{-0.98})); @@ -364,7 +364,7 @@ GTEST_TEST(IrisInConfigurationSpaceTest, SpheresPrismatic) { const Vector1d sample = Vector1d::Zero(); HPolyhedron region = IrisFromUrdf(spheres_urdf, sample); - + EXPECT_EQ(region.ambient_dimension(), 1); EXPECT_TRUE(region.PointInSet(Vector1d{0.98})); EXPECT_TRUE(region.PointInSet(Vector1d{-0.98})); @@ -380,7 +380,7 @@ GTEST_TEST(IrisInConfigurationSpaceTest, Pendulum) { - TODO: Finish this... + // TODO(russt): Finish this... @@ -409,7 +409,7 @@ GTEST_TEST(IrisInConfigurationSpaceTest, Pendulum) { const Vector1d sample = Vector1d::Zero(); HPolyhedron region = IrisFromUrdf(boxes_urdf, sample); - + EXPECT_EQ(region.ambient_dimension(), 1); EXPECT_TRUE(region.PointInSet(Vector1d{0.98})); EXPECT_TRUE(region.PointInSet(Vector1d{-0.98})); From 9e59ccdf2bc2efe8719defa25e01cac607baab78 Mon Sep 17 00:00:00 2001 From: Soonho Kong Date: Sat, 24 Jul 2021 23:49:25 -0400 Subject: [PATCH 6/6] Add 'AddFormula' and handles conjunctions --- solvers/ibex_solver.cc | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/solvers/ibex_solver.cc b/solvers/ibex_solver.cc index 66e6190dff5f..d879388e5e41 100644 --- a/solvers/ibex_solver.cc +++ b/solvers/ibex_solver.cc @@ -67,6 +67,20 @@ void ExtractQuadraticCosts(const MathematicalProgram& prog, } } +// Adds @p f into @p factor using @p ibex_converter and @p expr_ctrs. +void AddFormula( + const Formula& f, IbexConverter* const ibex_converter, + ibex::SystemFactory* const factory, + std::vector>* const + expr_ctrs) { + std::unique_ptr expr_ctr{ + ibex_converter->Convert(f)}; + if (expr_ctr) { + factory->add_ctr(*expr_ctr); + expr_ctrs->push_back(std::move(expr_ctr)); + } +} + } // namespace bool IbexSolver::is_available() { return true; } @@ -116,11 +130,12 @@ void IbexSolver::DoSolve(const MathematicalProgram& prog, for (const auto& b : prog.linear_constraints()) { Formula f{b.evaluator()->CheckSatisfied(b.variables())}; std::cerr << "Add constraint: " << f << "\n"; - std::unique_ptr expr_ctr{ - ibex_converter.Convert(f)}; - if (expr_ctr) { - factory.add_ctr(*expr_ctr); - expr_ctrs.push_back(std::move(expr_ctr)); + if (symbolic::is_conjunction(f)) { + for (const auto& f_i : symbolic::get_operands(f)) { + AddFormula(f_i, &ibex_converter, &factory, &expr_ctrs); + } + } else { + AddFormula(f, &ibex_converter, &factory, &expr_ctrs); } ibex_converter.set_need_to_delete_variables(true); }