diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 0d15486d7697..a9985a2bbdb5 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -23,6 +23,7 @@ drake_cc_package_library( ":bilinear_product_util", ":binding", ":branch_and_bound", + ":choose_best_solver", ":constraint", ":cost", ":create_constraint", @@ -49,6 +50,7 @@ drake_cc_package_library( ":nlopt_solver", ":non_convex_optimization_util", ":osqp_solver", + ":program_attribute", ":rotation_constraint", ":scs_solver", ":snopt_solver", @@ -92,6 +94,27 @@ drake_cc_library( ], ) +drake_cc_library( + name = "choose_best_solver", + srcs = ["choose_best_solver.cc"], + hdrs = ["choose_best_solver.h"], + deps = [ + ":dreal_solver", + ":equality_constrained_qp_solver", + ":gurobi_solver", + ":ipopt_solver", + ":linear_system_solver", + ":mathematical_program_api", + ":moby_lcp_solver", + ":mosek_solver", + ":nlopt_solver", + ":osqp_solver", + ":scs_solver", + ":snopt_solver", + ":solver_id", + ], +) + drake_cc_library( name = "evaluator_base", srcs = ["evaluator_base.cc"], @@ -253,6 +276,7 @@ drake_cc_library( hdrs = [], deps = [ ":binding", + ":choose_best_solver", ":create_constraint", ":create_cost", ":decision_variable", @@ -267,6 +291,7 @@ drake_cc_library( ":mosek_solver", ":nlopt_solver", ":osqp_solver", + ":program_attribute", ":scs_solver", ":snopt_solver", ":solution_result", @@ -376,6 +401,15 @@ drake_cc_library( ], ) +drake_cc_library( + name = "program_attribute", + srcs = ["program_attribute.cc"], + hdrs = ["program_attribute.h"], + deps = [ + "//common:hash", + ], +) + drake_cc_library( name = "rotation_constraint_visualization", testonly = 1, @@ -541,6 +575,7 @@ drake_cc_library( ":function", ":indeterminate", ":mathematical_program_result", + ":program_attribute", ":solution_result", ":solver_id", ":solver_result", @@ -1417,6 +1452,15 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "choose_best_solver_test", + deps = [ + ":choose_best_solver", + ":mathematical_program", + "//common/test_utilities:expect_throws_message", + ], +) + # The extra_srcs are required here because add_lint_tests() doesn't understand # how to extract labels from select() functions yet. add_lint_tests(cpplint_extra_srcs = [ diff --git a/solvers/choose_best_solver.cc b/solvers/choose_best_solver.cc new file mode 100644 index 000000000000..dff550d12fbc --- /dev/null +++ b/solvers/choose_best_solver.cc @@ -0,0 +1,60 @@ +#include "drake/solvers/choose_best_solver.h" + +#include "drake/solvers/dreal_solver.h" +#include "drake/solvers/equality_constrained_qp_solver.h" +#include "drake/solvers/gurobi_solver.h" +#include "drake/solvers/ipopt_solver.h" +#include "drake/solvers/linear_system_solver.h" +#include "drake/solvers/moby_lcp_solver.h" +#include "drake/solvers/mosek_solver.h" +#include "drake/solvers/nlopt_solver.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/scs_solver.h" +#include "drake/solvers/snopt_solver.h" + +namespace drake { +namespace solvers { +SolverId ChooseBestSolver(const MathematicalProgram& prog) { + if (LinearSystemSolver::is_available() && + LinearSystemSolver::ProgramAttributesSatisfied(prog)) { + return LinearSystemSolver::id(); + } else if (EqualityConstrainedQPSolver::is_available() && + EqualityConstrainedQPSolver::ProgramAttributesSatisfied(prog)) { + return EqualityConstrainedQPSolver::id(); + } else if (MosekSolver::is_available() && + MosekSolver::ProgramAttributesSatisfied(prog)) { + // TODO(hongkai.dai@tri.global): based on my limited experience, Mosek is + // faster than Gurobi for convex optimization problem. But we should run + // a more thorough comparison. + return MosekSolver::id(); + } else if (GurobiSolver::is_available() && + GurobiSolver::ProgramAttributesSatisfied(prog)) { + return GurobiSolver::id(); + } else if (OsqpSolver::is_available() && + OsqpSolver::ProgramAttributesSatisfied(prog)) { + return OsqpSolver::id(); + } else if (MobyLCPSolver::is_available() && + MobyLCPSolver::ProgramAttributesSatisfied(prog)) { + return MobyLcpSolverId::id(); + } else if (SnoptSolver::is_available() && + SnoptSolver::ProgramAttributesSatisfied(prog)) { + return SnoptSolver::id(); + } else if (IpoptSolver::is_available() && + IpoptSolver::ProgramAttributesSatisfied(prog)) { + return IpoptSolver::id(); + } else if (NloptSolver::is_available() && + NloptSolver::ProgramAttributesSatisfied(prog)) { + return NloptSolver::id(); + } else if (ScsSolver::is_available() && + ScsSolver::ProgramAttributesSatisfied(prog)) { + // Use SCS as the last resort. SCS uses ADMM method, which converges fast to + // modest accuracy quite fast, but then slows down significantly if the user + // wants high accuracy. + return ScsSolver::id(); + } + + throw std::invalid_argument( + "There is no available solver for the optimization program"); +} +} // namespace solvers +} // namespace drake diff --git a/solvers/choose_best_solver.h b/solvers/choose_best_solver.h new file mode 100644 index 000000000000..433c10246a7c --- /dev/null +++ b/solvers/choose_best_solver.h @@ -0,0 +1,15 @@ +#pragma once + +#include "drake/solvers/mathematical_program.h" +#include "drake/solvers/solver_id.h" + +namespace drake { +namespace solvers { +/** + * Choose the best solver given the formulation in the optimization program and + * the availability of the solvers. + * @throw invalid_argument if there is no available solver for @p prog. + */ +SolverId ChooseBestSolver(const MathematicalProgram& prog); +} // namespace solvers +} // namespace drake diff --git a/solvers/dreal_solver.cc b/solvers/dreal_solver.cc index 4eb410a011b6..6a686b955c1b 100644 --- a/solvers/dreal_solver.cc +++ b/solvers/dreal_solver.cc @@ -416,7 +416,7 @@ optional DrealSolver::Minimize( } } -bool DrealSolver::available() const { return true; } +bool DrealSolver::is_available() { return true; } namespace { diff --git a/solvers/dreal_solver.h b/solvers/dreal_solver.h index 2c1441fc533c..be16fe4e8b2f 100644 --- a/solvers/dreal_solver.h +++ b/solvers/dreal_solver.h @@ -32,7 +32,9 @@ class DrealSolver : public MathematicalProgramSolverInterface { // This solver is implemented in various pieces depending on if // Dreal was available during compilation. - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); SolutionResult Solve(MathematicalProgram& prog) const override; @@ -41,6 +43,11 @@ class DrealSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); + /// Checks the satisfiability of a given formula @p f with a given precision /// @p delta. /// diff --git a/solvers/dreal_solver_common.cc b/solvers/dreal_solver_common.cc index ea3d52acb957..bc343b0f1e3a 100644 --- a/solvers/dreal_solver_common.cc +++ b/solvers/dreal_solver_common.cc @@ -3,6 +3,7 @@ /* 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. @@ -19,5 +20,25 @@ SolverId DrealSolver::id() { return singleton.access(); } +bool DrealSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return DrealSolver::ProgramAttributesSatisfied(prog); +} + +bool DrealSolver::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/equality_constrained_qp_solver.cc b/solvers/equality_constrained_qp_solver.cc index 27b9a2c92e8d..8d23e526393e 100644 --- a/solvers/equality_constrained_qp_solver.cc +++ b/solvers/equality_constrained_qp_solver.cc @@ -15,7 +15,7 @@ namespace drake { namespace solvers { -bool EqualityConstrainedQPSolver::available() const { return true; } +bool EqualityConstrainedQPSolver::is_available() { return true; } /** * Solves the un-constrained QP problem @@ -279,5 +279,22 @@ SolverId EqualityConstrainedQPSolver::id() { return singleton.access(); } +bool EqualityConstrainedQPSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return ProgramAttributesSatisfied(prog); +} + +bool EqualityConstrainedQPSolver::ProgramAttributesSatisfied( + const MathematicalProgram& prog) { + static const never_destroyed solver_capabilities( + std::initializer_list{ + ProgramAttribute::kQuadraticCost, ProgramAttribute::kLinearCost, + ProgramAttribute::kLinearEqualityConstraint}); + // TODO(hongkai.dai) also make sure that there exists at least a quadratic + // cost. + return AreRequiredAttributesSupported(prog.required_capabilities(), + solver_capabilities.access()); +} + } // namespace solvers } // namespace drake diff --git a/solvers/equality_constrained_qp_solver.h b/solvers/equality_constrained_qp_solver.h index d909d6c9c2e8..49ac7e7040aa 100644 --- a/solvers/equality_constrained_qp_solver.h +++ b/solvers/equality_constrained_qp_solver.h @@ -15,7 +15,9 @@ class EqualityConstrainedQPSolver : public MathematicalProgramSolverInterface { EqualityConstrainedQPSolver() = default; ~EqualityConstrainedQPSolver() override = default; - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); /** * Solve the qudratic program with equality constraint. @@ -30,6 +32,11 @@ class EqualityConstrainedQPSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); }; } // namespace solvers diff --git a/solvers/gurobi_solver.cc b/solvers/gurobi_solver.cc index fd5bee144fa9..70ed6c80f114 100644 --- a/solvers/gurobi_solver.cc +++ b/solvers/gurobi_solver.cc @@ -604,7 +604,7 @@ void AddSecondOrderConeVariables( } } // anonymous namespace -bool GurobiSolver::available() const { return true; } +bool GurobiSolver::is_available() { return true; } /* * Implements RAII for a Gurobi license / environment. diff --git a/solvers/gurobi_solver.h b/solvers/gurobi_solver.h index 66cbb760f372..400d970eec3f 100644 --- a/solvers/gurobi_solver.h +++ b/solvers/gurobi_solver.h @@ -21,7 +21,9 @@ class GurobiSolver : public MathematicalProgramSolverInterface { // This solver is implemented in various pieces depending on if // Gurobi was available during compilation. - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); /// Contains info returned to a user function that handles /// a Node or Solution callback. @@ -111,6 +113,11 @@ class GurobiSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); + /** * This type contains a valid Gurobi license environment, and is only to be * used from AcquireLicense(). diff --git a/solvers/gurobi_solver_common.cc b/solvers/gurobi_solver_common.cc index 6baf0424403a..a25bdbdfaf62 100644 --- a/solvers/gurobi_solver_common.cc +++ b/solvers/gurobi_solver_common.cc @@ -3,6 +3,7 @@ /* 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. @@ -19,5 +20,23 @@ SolverId GurobiSolver::id() { return singleton.access(); } +bool GurobiSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return GurobiSolver::ProgramAttributesSatisfied(prog); +} + +bool GurobiSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) { + // TODO(hongkai.dai): Gurobi supports callback. Add callback capability. + static const never_destroyed solver_capabilities( + std::initializer_list{ + ProgramAttribute::kLinearCost, ProgramAttribute::kQuadraticCost, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kLorentzConeConstraint, + ProgramAttribute::kRotatedLorentzConeConstraint, + ProgramAttribute::kBinaryVariable}); + return AreRequiredAttributesSupported(prog.required_capabilities(), + solver_capabilities.access()); +} } // namespace solvers } // namespace drake diff --git a/solvers/ipopt_solver.cc b/solvers/ipopt_solver.cc index 033250b850c2..e265cb018fd5 100644 --- a/solvers/ipopt_solver.cc +++ b/solvers/ipopt_solver.cc @@ -502,7 +502,7 @@ class IpoptSolver_NLP : public Ipopt::TNLP { } // namespace -bool IpoptSolver::available() const { return true; } +bool IpoptSolver::is_available() { return true; } SolutionResult IpoptSolver::Solve(MathematicalProgram& prog) const { DRAKE_ASSERT(prog.linear_complementarity_constraints().empty()); diff --git a/solvers/ipopt_solver.h b/solvers/ipopt_solver.h index 17f9a71ff445..573abfb45b4a 100644 --- a/solvers/ipopt_solver.h +++ b/solvers/ipopt_solver.h @@ -17,7 +17,9 @@ class IpoptSolver : public MathematicalProgramSolverInterface { // This solver is implemented in various pieces depending on if // Ipopt was available during compilation. - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); SolutionResult Solve(MathematicalProgram& prog) const override; @@ -25,6 +27,11 @@ class IpoptSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); }; } // namespace solvers diff --git a/solvers/ipopt_solver_common.cc b/solvers/ipopt_solver_common.cc index 1894699df0e8..c8c11cccd76b 100644 --- a/solvers/ipopt_solver_common.cc +++ b/solvers/ipopt_solver_common.cc @@ -3,6 +3,7 @@ /* clang-format on */ #include "drake/common/never_destroyed.h" +#include "drake/solvers/mathematical_program.h" namespace drake { namespace solvers { @@ -16,5 +17,24 @@ SolverId IpoptSolver::id() { return singleton.access(); } +bool IpoptSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return IpoptSolver::ProgramAttributesSatisfied(prog); +} + +bool IpoptSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) { + static const never_destroyed solver_capabilities( + std::initializer_list{ + ProgramAttribute::kGenericConstraint, + ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kQuadraticConstraint, + ProgramAttribute::kLorentzConeConstraint, + ProgramAttribute::kRotatedLorentzConeConstraint, + ProgramAttribute::kGenericCost, ProgramAttribute::kLinearCost, + ProgramAttribute::kQuadraticCost, ProgramAttribute::kCallback}); + return AreRequiredAttributesSupported(prog.required_capabilities(), + solver_capabilities.access()); +} } // namespace solvers } // namespace drake diff --git a/solvers/linear_system_solver.cc b/solvers/linear_system_solver.cc index ab144e2e612e..55065f3be3b8 100644 --- a/solvers/linear_system_solver.cc +++ b/solvers/linear_system_solver.cc @@ -12,7 +12,7 @@ namespace drake { namespace solvers { -bool LinearSystemSolver::available() const { return true; } +bool LinearSystemSolver::is_available() { return true; } SolutionResult LinearSystemSolver::Solve(MathematicalProgram& prog) const { size_t num_constraints = 0; @@ -70,5 +70,18 @@ SolverId LinearSystemSolver::id() { return singleton.access(); } +bool LinearSystemSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return ProgramAttributesSatisfied(prog); +} + +bool LinearSystemSolver::ProgramAttributesSatisfied( + const MathematicalProgram& prog) { + static const never_destroyed solver_capability( + std::initializer_list{ + ProgramAttribute::kLinearEqualityConstraint}); + return prog.required_capabilities() == solver_capability.access(); +} + } // namespace solvers } // namespace drake diff --git a/solvers/linear_system_solver.h b/solvers/linear_system_solver.h index df5d956d0bd8..c2b70f428616 100644 --- a/solvers/linear_system_solver.h +++ b/solvers/linear_system_solver.h @@ -15,7 +15,9 @@ class LinearSystemSolver : public MathematicalProgramSolverInterface { LinearSystemSolver() = default; ~LinearSystemSolver() override = default; - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); /// Find the least-square solution to the linear system A * x = b. SolutionResult Solve(MathematicalProgram& prog) const override; @@ -24,6 +26,11 @@ class LinearSystemSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); }; } // namespace solvers diff --git a/solvers/mathematical_program.cc b/solvers/mathematical_program.cc index 6a9a54d32610..071ae50f36ba 100644 --- a/solvers/mathematical_program.cc +++ b/solvers/mathematical_program.cc @@ -19,6 +19,7 @@ #include "drake/common/eigen_types.h" #include "drake/common/symbolic.h" #include "drake/math/matrix_util.h" +#include "drake/solvers/choose_best_solver.h" #include "drake/solvers/equality_constrained_qp_solver.h" #include "drake/solvers/gurobi_solver.h" #include "drake/solvers/ipopt_solver.h" @@ -64,57 +65,6 @@ using internal::CreateBinding; using internal::DecomposeLinearExpression; using internal::SymbolicError; -namespace { - -// Solver for simple linear systems of equalities -AttributesSet kLinearSystemSolverCapabilities = kLinearEqualityConstraint; - -// Solver for equality-constrained QPs -AttributesSet kEqualityConstrainedQPCapabilities = - (kQuadraticCost | kLinearCost | kLinearEqualityConstraint); - -// Solver for Linear Complementarity Problems (LCPs) -AttributesSet kMobyLcpCapabilities = kLinearComplementarityConstraint; - -// Gurobi solver capabilities. -AttributesSet kGurobiCapabilities = - (kLinearEqualityConstraint | kLinearConstraint | kLorentzConeConstraint | - kRotatedLorentzConeConstraint | kLinearCost | kQuadraticCost | - kBinaryVariable); - -// OSQP solver capabilities. This is a QP solver. -AttributesSet kOsqpCapabilities = - (kLinearEqualityConstraint | kLinearConstraint | kLinearCost | - kQuadraticCost); - -// Mosek solver capabilities. -AttributesSet kMosekCapabilities = - (kLinearEqualityConstraint | kLinearConstraint | kLorentzConeConstraint | - kRotatedLorentzConeConstraint | kLinearCost | kQuadraticCost | - kPositiveSemidefiniteConstraint | kBinaryVariable); - -// Scs solver capatilities. -AttributesSet kScsCapabilities = - (kLinearEqualityConstraint | kLinearConstraint | kLorentzConeConstraint | - kRotatedLorentzConeConstraint | kLinearCost | kQuadraticCost | - kPositiveSemidefiniteConstraint); - -// Solvers for generic systems of constraints and costs. -AttributesSet kGenericSolverCapabilities = - (kGenericCost | kGenericConstraint | kQuadraticCost | kQuadraticConstraint | - kLorentzConeConstraint | kRotatedLorentzConeConstraint | kLinearCost | - kLinearConstraint | kLinearEqualityConstraint | kCallback); - -// Snopt solver capabilities. -AttributesSet kSnoptCapabilities = - (kGenericSolverCapabilities | kLinearComplementarityConstraint); - -// Returns true iff no capabilities are in required and not in available. -bool is_satisfied(AttributesSet required, AttributesSet available) { - return ((required & ~available) == kNoCapabilities); -} -} // namespace - constexpr double MathematicalProgram::kGlobalInfeasibleCost; constexpr double MathematicalProgram::kUnboundedCost; @@ -122,7 +72,7 @@ MathematicalProgram::MathematicalProgram() : x_initial_guess_(0), optimal_cost_(numeric_limits::quiet_NaN()), lower_bound_cost_(-numeric_limits::infinity()), - required_capabilities_(kNoCapabilities), + required_capabilities_{}, ipopt_solver_(new IpoptSolver()), nlopt_solver_(new NloptSolver()), snopt_solver_(new SnoptSolver()), @@ -331,7 +281,7 @@ Binding MathematicalProgram::AddVisualizationCallback( visualization_callbacks_.push_back( internal::CreateBinding( make_shared(vars.size(), callback), vars)); - required_capabilities_ |= kCallback; + required_capabilities_.insert(ProgramAttribute::kCallback); return visualization_callbacks_.back(); } @@ -344,7 +294,7 @@ Binding MathematicalProgram::AddCost(const Binding& binding) { return AddCost(internal::BindingDynamicCast(binding)); } else { CheckBinding(binding); - required_capabilities_ |= kGenericCost; + required_capabilities_.insert(ProgramAttribute::kGenericCost); generic_costs_.push_back(binding); return generic_costs_.back(); } @@ -353,7 +303,7 @@ Binding MathematicalProgram::AddCost(const Binding& binding) { Binding MathematicalProgram::AddCost( const Binding& binding) { CheckBinding(binding); - required_capabilities_ |= kLinearCost; + required_capabilities_.insert(ProgramAttribute::kLinearCost); linear_costs_.push_back(binding); return linear_costs_.back(); } @@ -371,7 +321,7 @@ Binding MathematicalProgram::AddLinearCost( Binding MathematicalProgram::AddCost( const Binding& binding) { CheckBinding(binding); - required_capabilities_ |= kQuadraticCost; + required_capabilities_.insert(ProgramAttribute::kQuadraticCost); DRAKE_ASSERT(binding.evaluator()->Q().rows() == static_cast(binding.GetNumElements()) && binding.evaluator()->b().rows() == @@ -448,7 +398,7 @@ Binding MathematicalProgram::AddConstraint( internal::BindingDynamicCast(binding)); } else { CheckBinding(binding); - required_capabilities_ |= kGenericConstraint; + required_capabilities_.insert(ProgramAttribute::kGenericConstraint); generic_constraints_.push_back(binding); return generic_constraints_.back(); } @@ -538,7 +488,7 @@ Binding MathematicalProgram::AddConstraint( DRAKE_ASSERT(binding.evaluator()->A().cols() == static_cast(binding.GetNumElements())); CheckBinding(binding); - required_capabilities_ |= kLinearConstraint; + required_capabilities_.insert(ProgramAttribute::kLinearConstraint); linear_constraints_.push_back(binding); return linear_constraints_.back(); } @@ -557,7 +507,7 @@ Binding MathematicalProgram::AddConstraint( DRAKE_ASSERT(binding.evaluator()->A().cols() == static_cast(binding.GetNumElements())); CheckBinding(binding); - required_capabilities_ |= kLinearEqualityConstraint; + required_capabilities_.insert(ProgramAttribute::kLinearEqualityConstraint); linear_equality_constraints_.push_back(binding); return linear_equality_constraints_.back(); } @@ -591,7 +541,7 @@ Binding MathematicalProgram::AddConstraint( CheckBinding(binding); DRAKE_ASSERT(binding.evaluator()->num_outputs() == static_cast(binding.GetNumElements())); - required_capabilities_ |= kLinearConstraint; + required_capabilities_.insert(ProgramAttribute::kLinearConstraint); bbox_constraints_.push_back(binding); return bbox_constraints_.back(); } @@ -599,7 +549,7 @@ Binding MathematicalProgram::AddConstraint( Binding MathematicalProgram::AddConstraint( const Binding& binding) { CheckBinding(binding); - required_capabilities_ |= kLorentzConeConstraint; + required_capabilities_.insert(ProgramAttribute::kLorentzConeConstraint); lorentz_cone_constraint_.push_back(binding); return lorentz_cone_constraint_.back(); } @@ -628,7 +578,8 @@ Binding MathematicalProgram::AddLorentzConeConstraint( Binding MathematicalProgram::AddConstraint( const Binding& binding) { CheckBinding(binding); - required_capabilities_ |= kRotatedLorentzConeConstraint; + required_capabilities_.insert( + ProgramAttribute::kRotatedLorentzConeConstraint); rotated_lorentz_cone_constraint_.push_back(binding); return rotated_lorentz_cone_constraint_.back(); } @@ -675,7 +626,8 @@ Binding MathematicalProgram::AddConstraint( const Binding& binding) { CheckBinding(binding); - required_capabilities_ |= kLinearComplementarityConstraint; + required_capabilities_.insert( + ProgramAttribute::kLinearComplementarityConstraint); linear_complementarity_constraints_.push_back(binding); return linear_complementarity_constraints_.back(); @@ -707,7 +659,8 @@ Binding MathematicalProgram::AddConstraint( DRAKE_ASSERT(math::IsSymmetric(Eigen::Map( binding.variables().data(), binding.evaluator()->matrix_rows(), binding.evaluator()->matrix_rows()))); - required_capabilities_ |= kPositiveSemidefiniteConstraint; + required_capabilities_.insert( + ProgramAttribute::kPositiveSemidefiniteConstraint); positive_semidefinite_constraint_.push_back(binding); return positive_semidefinite_constraint_.back(); } @@ -740,7 +693,8 @@ Binding MathematicalProgram::AddConstraint( CheckBinding(binding); DRAKE_ASSERT(static_cast(binding.evaluator()->F().size()) == static_cast(binding.GetNumElements()) + 1); - required_capabilities_ |= kPositiveSemidefiniteConstraint; + required_capabilities_.insert( + ProgramAttribute::kPositiveSemidefiniteConstraint); linear_matrix_inequality_constraint_.push_back(binding); return linear_matrix_inequality_constraint_.back(); } @@ -939,55 +893,29 @@ void MathematicalProgram::SetInitialGuess( // implemented in mathematical_program_api.cc instead of this file. SolutionResult MathematicalProgram::Solve() { - // This implementation is simply copypasta for now; in the future we will - // want to tweak the order of preference of solvers based on the types of - // constraints present. - - if (is_satisfied(required_capabilities_, kLinearSystemSolverCapabilities) && - linear_system_solver_->available()) { - // TODO(ggould-tri) Also allow quadratic objectives whose matrix is - // Identity: This is the objective function the solver uses anyway when - // underconstrainted, and is fairly common in real-world problems. + const SolverId solver_id = ChooseBestSolver(*this); + if (solver_id == LinearSystemSolver::id()) { return linear_system_solver_->Solve(*this); - } else if (is_satisfied(required_capabilities_, - kEqualityConstrainedQPCapabilities) && - equality_constrained_qp_solver_->available()) { + } else if (solver_id == EqualityConstrainedQPSolver::id()) { return equality_constrained_qp_solver_->Solve(*this); - } else if (is_satisfied(required_capabilities_, kMosekCapabilities) && - mosek_solver_->available()) { - // TODO(hongkai.dai@tri.global): based on my limited experience, Mosek is - // faster than Gurobi for convex optimization problem. But we should run - // a more thorough comparison. + } else if (solver_id == MosekSolver::id()) { return mosek_solver_->Solve(*this); - } else if (is_satisfied(required_capabilities_, kGurobiCapabilities) && - gurobi_solver_->available()) { + } else if (solver_id == GurobiSolver::id()) { return gurobi_solver_->Solve(*this); - } else if ((required_capabilities_ & kQuadraticCost) && - is_satisfied(required_capabilities_, kOsqpCapabilities) && - osqp_solver_->available()) { + } else if (solver_id == OsqpSolver::id()) { return osqp_solver_->Solve(*this); - } else if (is_satisfied(required_capabilities_, kMobyLcpCapabilities) && - moby_lcp_solver_->available()) { + } else if (solver_id == MobyLcpSolverId::id()) { return moby_lcp_solver_->Solve(*this); - } else if (is_satisfied(required_capabilities_, kSnoptCapabilities) && - snopt_solver_->available()) { + } else if (solver_id == SnoptSolver::id()) { return snopt_solver_->Solve(*this); - } else if (is_satisfied(required_capabilities_, kGenericSolverCapabilities) && - ipopt_solver_->available()) { + } else if (solver_id == IpoptSolver::id()) { return ipopt_solver_->Solve(*this); - } else if (is_satisfied(required_capabilities_, kGenericSolverCapabilities) && - nlopt_solver_->available()) { + } else if (solver_id == NloptSolver::id()) { return nlopt_solver_->Solve(*this); - } else if (is_satisfied(required_capabilities_, kScsCapabilities) && - scs_solver_->available()) { - // Use SCS as the last resort. SCS uses ADMM method, which converges fast to - // modest accuracy quite fast, but then slows down significantly if the user - // wants high accuracy. + } else if (solver_id == ScsSolver::id()) { return scs_solver_->Solve(*this); } else { - throw runtime_error( - "MathematicalProgram::Solve: " - "No solver available for the given optimization problem!"); + throw std::runtime_error("Unknown solver."); } } diff --git a/solvers/mathematical_program.h b/solvers/mathematical_program.h index 0b3b9a60e7af..bfb3fd8e4ee5 100644 --- a/solvers/mathematical_program.h +++ b/solvers/mathematical_program.h @@ -35,6 +35,7 @@ #include "drake/solvers/indeterminate.h" #include "drake/solvers/mathematical_program_result.h" #include "drake/solvers/mathematical_program_solver_interface.h" +#include "drake/solvers/program_attribute.h" #include "drake/solvers/solution_result.h" #include "drake/solvers/solver_result.h" @@ -182,25 +183,6 @@ namespace solvers { */ class MathematicalProgram; -enum ProgramAttributes { - kNoCapabilities = 0, - kError = 1 << 0, ///< Do not use, to avoid & vs. && typos. - kGenericCost = 1 << 1, - kGenericConstraint = 1 << 2, - kQuadraticCost = 1 << 3, - kQuadraticConstraint = 1 << 4, - kLinearCost = 1 << 5, - kLinearConstraint = 1 << 6, - kLinearEqualityConstraint = 1 << 7, - kLinearComplementarityConstraint = 1 << 8, - kLorentzConeConstraint = 1 << 9, - kRotatedLorentzConeConstraint = 1 << 10, - kPositiveSemidefiniteConstraint = 1 << 11, - kBinaryVariable = 1 << 12, - kCallback = 1 << 13 -}; -typedef uint32_t AttributesSet; - template struct NewVariableNames {}; /** @@ -2720,6 +2702,12 @@ class MathematicalProgram { // of MathematicalProgram. void SetSolverResult(const SolverResult& solver_result); + /// Getter for the required capability on the solver, given the + /// cost/constraint/variable types in the program. + const ProgramAttributes& required_capabilities() const { + return required_capabilities_; + } + private: static void AppendNanToEnd(int new_var_size, Eigen::VectorXd* vector); // maps the ID of a symbolic variable to the index of the variable stored in @@ -2777,7 +2765,7 @@ class MathematicalProgram { const std::map solver_options_int_empty_; const std::map solver_options_str_empty_; - AttributesSet required_capabilities_{0}; + ProgramAttributes required_capabilities_{}; std::unique_ptr ipopt_solver_; std::unique_ptr nlopt_solver_; @@ -2799,7 +2787,7 @@ class MathematicalProgram { case VarType::CONTINUOUS: break; case VarType::BINARY: - required_capabilities_ |= kBinaryVariable; + required_capabilities_.insert(ProgramAttribute::kBinaryVariable); break; case VarType::INTEGER: throw std::runtime_error( diff --git a/solvers/mathematical_program_solver_interface.h b/solvers/mathematical_program_solver_interface.h index 9f3e21dff485..fedfb6f8cb1a 100644 --- a/solvers/mathematical_program_solver_interface.h +++ b/solvers/mathematical_program_solver_interface.h @@ -31,6 +31,11 @@ class MathematicalProgramSolverInterface { /// Returns the identifier of this solver. virtual SolverId solver_id() const = 0; + + /// Returns true if the program attributes are satisfied by the solver's + /// capability. + virtual bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const = 0; }; } // namespace solvers diff --git a/solvers/moby_lcp_solver.cc b/solvers/moby_lcp_solver.cc index 8a9776f75946..04ef9340d60f 100644 --- a/solvers/moby_lcp_solver.cc +++ b/solvers/moby_lcp_solver.cc @@ -1069,6 +1069,20 @@ SolverId MobyLcpSolverId::id() { return singleton.access(); } +template +bool MobyLCPSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return MobyLCPSolver::ProgramAttributesSatisfied(prog); +} + +template +bool MobyLCPSolver::ProgramAttributesSatisfied( + const MathematicalProgram& prog) { + return prog.required_capabilities() == + ProgramAttributes( + {ProgramAttribute::kLinearComplementarityConstraint}); +} + // Instantiate templates. template class MobyLCPSolver; template class diff --git a/solvers/moby_lcp_solver.h b/solvers/moby_lcp_solver.h index 7459dadc8dc5..f94340b1048c 100644 --- a/solvers/moby_lcp_solver.h +++ b/solvers/moby_lcp_solver.h @@ -266,12 +266,19 @@ class MobyLCPSolver : public MathematicalProgramSolverInterface { int max_exp = 1, const T& piv_tol = T(-1), const T& zero_tol = T(-1)) const; - bool available() const override { return true; } + bool available() const override { return is_available(); }; + + static bool is_available() { return true; } SolutionResult Solve(MathematicalProgram& prog) const override; SolverId solver_id() const override; + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); + /// Returns the number of pivoting operations made by the last LCP solve. int get_num_pivots() const { return pivots_; } diff --git a/solvers/mosek_solver.cc b/solvers/mosek_solver.cc index ebe3d038150b..e99bf483e84d 100644 --- a/solvers/mosek_solver.cc +++ b/solvers/mosek_solver.cc @@ -650,7 +650,7 @@ std::shared_ptr MosekSolver::AcquireLicense() { return GetScopedSingleton(); } -bool MosekSolver::available() const { return true; } +bool MosekSolver::is_available() { return true; } MathematicalProgramResult MosekSolver::SolveConstProg( const MathematicalProgram& prog) const { diff --git a/solvers/mosek_solver.h b/solvers/mosek_solver.h index f1471cafd716..01308c5fc73f 100644 --- a/solvers/mosek_solver.h +++ b/solvers/mosek_solver.h @@ -35,7 +35,9 @@ class MosekSolver : public MathematicalProgramSolverInterface { /** * Defined true if Mosek was included during compilation, false otherwise. */ - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); MathematicalProgramResult Solve(const MathematicalProgram& prog) const; @@ -48,6 +50,11 @@ class MosekSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); + /** * Control stream logging. Refer to * https://docs.mosek.com/8.1/capi/solver-io.html for more details. diff --git a/solvers/mosek_solver_common.cc b/solvers/mosek_solver_common.cc index e0f2c054fa73..905dae692fb3 100644 --- a/solvers/mosek_solver_common.cc +++ b/solvers/mosek_solver_common.cc @@ -3,6 +3,7 @@ /* clang-format on */ #include "drake/common/never_destroyed.h" +#include "drake/solvers/mathematical_program.h" namespace drake { namespace solvers { @@ -16,5 +17,24 @@ SolverId MosekSolver::id() { return singleton.access(); } +bool MosekSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return MosekSolver::ProgramAttributesSatisfied(prog); +} + +bool MosekSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) { + static const never_destroyed solver_capabilities( + std::initializer_list{ + ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kLorentzConeConstraint, + ProgramAttribute::kRotatedLorentzConeConstraint, + ProgramAttribute::kPositiveSemidefiniteConstraint, + ProgramAttribute::kLinearCost, ProgramAttribute::kQuadraticCost, + ProgramAttribute::kBinaryVariable}); + return AreRequiredAttributesSupported(prog.required_capabilities(), + solver_capabilities.access()); +} + } // namespace solvers } // namespace drake diff --git a/solvers/nlopt_solver.cc b/solvers/nlopt_solver.cc index e2cc2513ee84..8f0e1275ae67 100644 --- a/solvers/nlopt_solver.cc +++ b/solvers/nlopt_solver.cc @@ -318,7 +318,7 @@ bool IsVectorOfConstraintsSatisfiedAtSolution( } } // anonymous namespace -bool NloptSolver::available() const { return true; } +bool NloptSolver::is_available() { return true; } SolutionResult NloptSolver::Solve(MathematicalProgram& prog) const { const int nx = prog.num_vars(); diff --git a/solvers/nlopt_solver.h b/solvers/nlopt_solver.h index c97f5f509947..f06e8778c130 100644 --- a/solvers/nlopt_solver.h +++ b/solvers/nlopt_solver.h @@ -17,7 +17,9 @@ class NloptSolver : public MathematicalProgramSolverInterface { // This solver is implemented in various pieces depending on if // NLOpt was available during compilation. - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); SolutionResult Solve(MathematicalProgram& prog) const override; @@ -25,6 +27,11 @@ class NloptSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); }; } // namespace solvers diff --git a/solvers/nlopt_solver_common.cc b/solvers/nlopt_solver_common.cc index 0bc311d343a5..131e3aa23654 100644 --- a/solvers/nlopt_solver_common.cc +++ b/solvers/nlopt_solver_common.cc @@ -3,6 +3,7 @@ /* clang-format on */ #include "drake/common/never_destroyed.h" +#include "drake/solvers/mathematical_program.h" namespace drake { namespace solvers { @@ -16,5 +17,24 @@ SolverId NloptSolver::id() { return singleton.access(); } +bool NloptSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return NloptSolver::ProgramAttributesSatisfied(prog); +} + +bool NloptSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) { + static const never_destroyed solver_capabilities( + std::initializer_list{ + ProgramAttribute::kGenericConstraint, + ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kQuadraticConstraint, + ProgramAttribute::kLorentzConeConstraint, + ProgramAttribute::kRotatedLorentzConeConstraint, + ProgramAttribute::kGenericCost, ProgramAttribute::kLinearCost, + ProgramAttribute::kQuadraticCost, ProgramAttribute::kCallback}); + return AreRequiredAttributesSupported(prog.required_capabilities(), + solver_capabilities.access()); +} } // namespace solvers } // namespace drake diff --git a/solvers/no_gurobi.cc b/solvers/no_gurobi.cc index ef0c7d37099e..a1a3647d4a86 100644 --- a/solvers/no_gurobi.cc +++ b/solvers/no_gurobi.cc @@ -10,9 +10,7 @@ std::shared_ptr GurobiSolver::AcquireLicense() { return {}; } -bool GurobiSolver::available() const { - return false; -} +bool GurobiSolver::is_available() { return false; } SolutionResult GurobiSolver::Solve(MathematicalProgram&) const { throw std::runtime_error( diff --git a/solvers/no_ipopt.cc b/solvers/no_ipopt.cc index aa17f9880722..448d4615d755 100644 --- a/solvers/no_ipopt.cc +++ b/solvers/no_ipopt.cc @@ -7,9 +7,7 @@ namespace drake { namespace solvers { -bool IpoptSolver::available() const { - return false; -} +bool IpoptSolver::is_available() { return false; } SolutionResult IpoptSolver::Solve(MathematicalProgram&) const { throw std::runtime_error( diff --git a/solvers/no_mosek.cc b/solvers/no_mosek.cc index ddebb3076f08..d1eec9be41b6 100644 --- a/solvers/no_mosek.cc +++ b/solvers/no_mosek.cc @@ -16,9 +16,7 @@ shared_ptr MosekSolver::AcquireLicense() { return shared_ptr(); } -bool MosekSolver::available() const { - return false; -} +bool MosekSolver::is_available() { return false; } SolutionResult MosekSolver::Solve(MathematicalProgram&) const { throw runtime_error( diff --git a/solvers/no_nlopt.cc b/solvers/no_nlopt.cc index d398ed673ab2..5225946d27ce 100644 --- a/solvers/no_nlopt.cc +++ b/solvers/no_nlopt.cc @@ -7,9 +7,7 @@ namespace drake { namespace solvers { -bool NloptSolver::available() const { - return false; -} +bool NloptSolver::is_available() { return false; } SolutionResult NloptSolver::Solve(MathematicalProgram&) const { throw std::runtime_error( diff --git a/solvers/no_osqp.cc b/solvers/no_osqp.cc index b5fee5869286..6e56f065fb08 100644 --- a/solvers/no_osqp.cc +++ b/solvers/no_osqp.cc @@ -9,7 +9,7 @@ namespace drake { namespace solvers { -bool OsqpSolver::available() const { return false; } +bool OsqpSolver::is_available() { return false; } SolutionResult OsqpSolver::Solve(MathematicalProgram&) const { throw std::runtime_error( diff --git a/solvers/no_scs.cc b/solvers/no_scs.cc index b58eecaa8087..b83d99868d36 100644 --- a/solvers/no_scs.cc +++ b/solvers/no_scs.cc @@ -9,7 +9,7 @@ namespace drake { namespace solvers { -bool ScsSolver::available() const { return false; } +bool ScsSolver::is_available() { return false; } SolutionResult ScsSolver::Solve(MathematicalProgram&) const { throw std::runtime_error( diff --git a/solvers/no_snopt.cc b/solvers/no_snopt.cc index 7bbdcb20839f..f1a79ec6a36e 100644 --- a/solvers/no_snopt.cc +++ b/solvers/no_snopt.cc @@ -7,9 +7,7 @@ namespace drake { namespace solvers { -bool SnoptSolver::available() const { - return false; -} +bool SnoptSolver::is_available() { return false; } SolutionResult SnoptSolver::Solve(MathematicalProgram&) const { throw std::runtime_error( diff --git a/solvers/osqp_solver.cc b/solvers/osqp_solver.cc index 8975b6e63e25..8c248acac2b2 100644 --- a/solvers/osqp_solver.cc +++ b/solvers/osqp_solver.cc @@ -199,7 +199,7 @@ void SetOsqpSolverSettings(MathematicalProgram* prog, OSQPSettings* settings) { } } // namespace -bool OsqpSolver::available() const { return true; } +bool OsqpSolver::is_available() { return true; } SolutionResult OsqpSolver::Solve(MathematicalProgram& prog) const { // OSQP solves a convex quadratic programming problem diff --git a/solvers/osqp_solver.h b/solvers/osqp_solver.h index 29781a824606..ac4a3ebc2b5e 100644 --- a/solvers/osqp_solver.h +++ b/solvers/osqp_solver.h @@ -15,7 +15,9 @@ class OsqpSolver : public MathematicalProgramSolverInterface { // This solver is implemented in various pieces depending on if Osqp was // available during compilation. - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); SolutionResult Solve(MathematicalProgram& prog) const override; @@ -23,6 +25,11 @@ class OsqpSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); }; } // namespace solvers } // namespace drake diff --git a/solvers/osqp_solver_common.cc b/solvers/osqp_solver_common.cc index 715c883af016..76a2364c653d 100644 --- a/solvers/osqp_solver_common.cc +++ b/solvers/osqp_solver_common.cc @@ -3,6 +3,7 @@ /* 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. @@ -19,5 +20,22 @@ SolverId OsqpSolver::id() { return singleton.access(); } +bool OsqpSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return OsqpSolver::ProgramAttributesSatisfied(prog); +} + +bool OsqpSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) { + static const never_destroyed solver_capabilities( + std::initializer_list{ + ProgramAttribute::kLinearCost, ProgramAttribute::kQuadraticCost, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kLinearEqualityConstraint}); + return AreRequiredAttributesSupported(prog.required_capabilities(), + solver_capabilities.access()) && + prog.required_capabilities().count(ProgramAttribute::kQuadraticCost) > + 0; +} + } // namespace solvers } // namespace drake diff --git a/solvers/program_attribute.cc b/solvers/program_attribute.cc new file mode 100644 index 000000000000..4ad7e4799b80 --- /dev/null +++ b/solvers/program_attribute.cc @@ -0,0 +1,18 @@ +#include "drake/solvers/program_attribute.h" + +namespace drake { +namespace solvers { +bool AreRequiredAttributesSupported(const ProgramAttributes& required, + const ProgramAttributes& supported) { + if (required.size() > supported.size()) { + return false; + } + for (const auto& attribute : required) { + if (supported.find(attribute) == supported.end()) { + return false; + } + } + return true; +} +} // namespace solvers +} // namespace drake diff --git a/solvers/program_attribute.h b/solvers/program_attribute.h new file mode 100644 index 000000000000..0099b020f8ec --- /dev/null +++ b/solvers/program_attribute.h @@ -0,0 +1,43 @@ +#pragma once + +#include + +#include "drake/common/hash.h" + +namespace drake { +namespace solvers { +enum class ProgramAttribute { + kGenericCost, ///< A generic cost, doesn't belong to any specific cost type + /// below. + kGenericConstraint, ///< A generic constraint, doesn't belong to any specific + /// constraint type below. + + kQuadraticCost, ///< A quadratic function as the cost. + kQuadraticConstraint, ///< A constraint on a quadratic function. + + kLinearCost, ///< A linear function as the cost. + kLinearConstraint, ///< A constraint on a linear function. + kLinearEqualityConstraint, ///< An equality constraint on a linear function. + + kLinearComplementarityConstraint, ///< A linear complementarity constraint in + /// the form 0 ≤ z ⊥ Mz+q ≥ 0. + + kLorentzConeConstraint, ///< A Lorentz cone constraint. + kRotatedLorentzConeConstraint, ///< A rotated Lorentz cone constraint. + + kPositiveSemidefiniteConstraint, /// A positive semidefinite constraint. + + kBinaryVariable, /// variable taking binary value {0, 1}. + + kCallback, /// support callback during solving the problem. +}; + +using ProgramAttributes = std::unordered_set; + +/** + * Returns true if @p required is a subset of @p supported. + */ +bool AreRequiredAttributesSupported(const ProgramAttributes& required, + const ProgramAttributes& supported); +} // namespace solvers +} // namespace drake diff --git a/solvers/scs_solver.cc b/solvers/scs_solver.cc index c4552aabdb68..e58b53294b61 100644 --- a/solvers/scs_solver.cc +++ b/solvers/scs_solver.cc @@ -499,7 +499,7 @@ void SetScsProblemData(int A_row_count, int num_vars, } } // namespace -bool ScsSolver::available() const { return true; } +bool ScsSolver::is_available() { return true; } SolutionResult ScsSolver::Solve(MathematicalProgram& prog) const { // SCS solves the problem in this form diff --git a/solvers/scs_solver.h b/solvers/scs_solver.h index 07ea88b67224..823ea3e58054 100644 --- a/solvers/scs_solver.h +++ b/solvers/scs_solver.h @@ -15,7 +15,9 @@ class ScsSolver : public MathematicalProgramSolverInterface { // This solver is implemented in various pieces depending on if // SCS was available during compilation. - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); SolutionResult Solve(MathematicalProgram& prog) const override; @@ -24,6 +26,11 @@ class ScsSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); + // If the user want the solver to print out debug message, then set this to // true; otherwise set it to false. The default is false. // TODO(hongkai.dai): add function to set the option through diff --git a/solvers/scs_solver_common.cc b/solvers/scs_solver_common.cc index 5baa0dc60d23..c5756fe4d490 100644 --- a/solvers/scs_solver_common.cc +++ b/solvers/scs_solver_common.cc @@ -3,6 +3,7 @@ /* clang-format on */ #include "drake/common/never_destroyed.h" +#include "drake/solvers/mathematical_program.h" namespace drake { namespace solvers { @@ -16,5 +17,22 @@ SolverId ScsSolver::id() { return singleton.access(); } +bool ScsSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return ScsSolver::ProgramAttributesSatisfied(prog); +} + +bool ScsSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) { + return AreRequiredAttributesSupported( + prog.required_capabilities(), + ProgramAttributes({ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kLorentzConeConstraint, + ProgramAttribute::kRotatedLorentzConeConstraint, + ProgramAttribute::kPositiveSemidefiniteConstraint, + ProgramAttribute::kLinearCost, + ProgramAttribute::kQuadraticCost})); +} + } // namespace solvers } // namespace drake diff --git a/solvers/snopt_solver.h b/solvers/snopt_solver.h index d3f857ea1cff..200721852d38 100644 --- a/solvers/snopt_solver.h +++ b/solvers/snopt_solver.h @@ -17,7 +17,9 @@ class SnoptSolver : public MathematicalProgramSolverInterface { // This solver is implemented in various pieces depending on if // SNOPT was available during compilation. - bool available() const override; + bool available() const override { return is_available(); }; + + static bool is_available(); SolutionResult Solve(MathematicalProgram& prog) const override; @@ -25,6 +27,11 @@ class SnoptSolver : public MathematicalProgramSolverInterface { /// @return same as MathematicalProgramSolverInterface::solver_id() static SolverId id(); + + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); }; } // namespace solvers diff --git a/solvers/snopt_solver_common.cc b/solvers/snopt_solver_common.cc index 8841ba6aef90..6783b36cb7d1 100644 --- a/solvers/snopt_solver_common.cc +++ b/solvers/snopt_solver_common.cc @@ -3,6 +3,7 @@ /* clang-format on */ #include "drake/common/never_destroyed.h" +#include "drake/solvers/mathematical_program.h" namespace drake { namespace solvers { @@ -16,5 +17,26 @@ SolverId SnoptSolver::id() { return singleton.access(); } +bool SnoptSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return SnoptSolver::ProgramAttributesSatisfied(prog); +} + +bool SnoptSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) { + static const never_destroyed solver_capabilities( + std::initializer_list{ + ProgramAttribute::kGenericConstraint, + ProgramAttribute::kLinearEqualityConstraint, + ProgramAttribute::kLinearConstraint, + ProgramAttribute::kQuadraticConstraint, + ProgramAttribute::kLorentzConeConstraint, + ProgramAttribute::kRotatedLorentzConeConstraint, + ProgramAttribute::kLinearComplementarityConstraint, + ProgramAttribute::kGenericCost, ProgramAttribute::kLinearCost, + ProgramAttribute::kQuadraticCost, ProgramAttribute::kCallback}); + return AreRequiredAttributesSupported(prog.required_capabilities(), + solver_capabilities.access()); +} + } // namespace solvers } // namespace drake diff --git a/solvers/snopt_solver_f2c.cc b/solvers/snopt_solver_f2c.cc index e52cde107d17..1db60d1b5b4c 100644 --- a/solvers/snopt_solver_f2c.cc +++ b/solvers/snopt_solver_f2c.cc @@ -599,7 +599,7 @@ void UpdateLinearConstraint(const MathematicalProgram& prog, } } // anon namespace -bool SnoptSolver::available() const { return true; } +bool SnoptSolver::is_available() { return true; } SolutionResult SnoptSolver::Solve(MathematicalProgram& prog) const { auto d = prog.GetSolverData(); diff --git a/solvers/test/choose_best_solver_test.cc b/solvers/test/choose_best_solver_test.cc new file mode 100644 index 000000000000..35859d557d7a --- /dev/null +++ b/solvers/test/choose_best_solver_test.cc @@ -0,0 +1,156 @@ +#include "drake/solvers/choose_best_solver.h" + +#include + +#include "drake/common/test_utilities/expect_throws_message.h" +#include "drake/solvers/equality_constrained_qp_solver.h" +#include "drake/solvers/gurobi_solver.h" +#include "drake/solvers/ipopt_solver.h" +#include "drake/solvers/linear_system_solver.h" +#include "drake/solvers/moby_lcp_solver.h" +#include "drake/solvers/mosek_solver.h" +#include "drake/solvers/nlopt_solver.h" +#include "drake/solvers/osqp_solver.h" +#include "drake/solvers/scs_solver.h" +#include "drake/solvers/snopt_solver.h" + +namespace drake { +namespace solvers { +class ChooseBestSolverTest : public ::testing::Test { + public: + ChooseBestSolverTest() + : prog_{}, + x_{prog_.NewContinuousVariables<3>()}, + linear_system_solver_{std::make_unique()}, + equality_constrained_qp_solver_{ + std::make_unique()}, + mosek_solver_{std::make_unique()}, + gurobi_solver_{std::make_unique()}, + osqp_solver_{std::make_unique()}, + moby_lcp_solver_{std::make_unique>()}, + snopt_solver_{std::make_unique()}, + ipopt_solver_{std::make_unique()}, + nlopt_solver_{std::make_unique()}, + scs_solver_{std::make_unique()} {} + + ~ChooseBestSolverTest() {} + + void CheckBestSolver(const SolverId& expected_solver_id) { + const SolverId solver_id = ChooseBestSolver(prog_); + EXPECT_EQ(solver_id, expected_solver_id); + } + + void CheckBestSolver( + const std::vector& solvers) { + bool is_any_solver_available = false; + for (const auto solver : solvers) { + if (solver->available()) { + is_any_solver_available = true; + break; + } + } + if (!is_any_solver_available) { + DRAKE_EXPECT_THROWS_MESSAGE( + ChooseBestSolver(prog_), std::invalid_argument, + "There is no available solver for the optimization program"); + } else { + const SolverId solver_id = ChooseBestSolver(prog_); + for (const auto solver : solvers) { + if (solver->available()) { + EXPECT_EQ(solver_id, solver->solver_id()); + return; + } + } + } + } + + protected: + MathematicalProgram prog_; + VectorDecisionVariable<3> x_; + std::unique_ptr linear_system_solver_; + std::unique_ptr equality_constrained_qp_solver_; + std::unique_ptr mosek_solver_; + std::unique_ptr gurobi_solver_; + std::unique_ptr osqp_solver_; + std::unique_ptr> moby_lcp_solver_; + std::unique_ptr snopt_solver_; + std::unique_ptr ipopt_solver_; + std::unique_ptr nlopt_solver_; + std::unique_ptr scs_solver_; +}; + +TEST_F(ChooseBestSolverTest, LinearSystemSolver) { + prog_.AddLinearEqualityConstraint(x_(0) + x_(1), 1); + CheckBestSolver(LinearSystemSolver::id()); +} + +TEST_F(ChooseBestSolverTest, EqualityConstrainedQPSolver) { + prog_.AddQuadraticCost(x_(0) * x_(0)); + prog_.AddLinearEqualityConstraint(x_(0) + x_(1), 1); + CheckBestSolver(EqualityConstrainedQPSolver::id()); +} + +TEST_F(ChooseBestSolverTest, QPsolver) { + prog_.AddLinearConstraint(x_(0) + x_(1) >= 1); + prog_.AddQuadraticCost(x_(0) * x_(0)); + CheckBestSolver({mosek_solver_.get(), gurobi_solver_.get(), + osqp_solver_.get(), snopt_solver_.get(), ipopt_solver_.get(), + nlopt_solver_.get(), scs_solver_.get()}); +} + +TEST_F(ChooseBestSolverTest, LorentzCone) { + prog_.AddLorentzConeConstraint(x_.cast()); + CheckBestSolver({mosek_solver_.get(), gurobi_solver_.get(), + snopt_solver_.get(), ipopt_solver_.get(), + nlopt_solver_.get(), scs_solver_.get()}); + prog_.AddRotatedLorentzConeConstraint(x_.cast()); + CheckBestSolver({mosek_solver_.get(), gurobi_solver_.get(), + snopt_solver_.get(), ipopt_solver_.get(), + nlopt_solver_.get(), scs_solver_.get()}); + + prog_.AddPolynomialCost(pow(x_(0), 3)); + CheckBestSolver( + {snopt_solver_.get(), ipopt_solver_.get(), nlopt_solver_.get()}); +} + +TEST_F(ChooseBestSolverTest, LinearComplementarityConstraint) { + prog_.AddLinearComplementarityConstraint(Eigen::Matrix3d::Identity(), + Eigen::Vector3d::Ones(), x_); + CheckBestSolver({moby_lcp_solver_.get(), snopt_solver_.get()}); + + prog_.AddLinearCost(x_(0) + 1); + CheckBestSolver({snopt_solver_.get()}); +} + +TEST_F(ChooseBestSolverTest, PositiveSemidefiniteConstraint) { + prog_.AddPositiveSemidefiniteConstraint( + (Matrix2() << x_(0), x_(1), x_(1), x_(2)).finished()); + CheckBestSolver({mosek_solver_.get(), scs_solver_.get()}); +} + +TEST_F(ChooseBestSolverTest, BinaryVariable) { + prog_.NewBinaryVariables<1>(); + prog_.AddLinearConstraint(x_(0) + x_(1) == 1); + if (MosekSolver::is_available()) { + CheckBestSolver(MosekSolver::id()); + } else if (GurobiSolver::is_available()) { + CheckBestSolver(GurobiSolver::id()); + } else { + DRAKE_EXPECT_THROWS_MESSAGE( + ChooseBestSolver(prog_), std::invalid_argument, + "There is no available solver for the optimization program"); + } +} + +TEST_F(ChooseBestSolverTest, NoAvailableSolver) { + // We don't have a solver for problem with both linear complementarity + // constraint and binary variables. + prog_.AddLinearComplementarityConstraint( + Eigen::Matrix2d::Identity(), Eigen::Vector2d::Ones(), x_.tail<2>()); + prog_.NewBinaryVariables<2>(); + DRAKE_EXPECT_THROWS_MESSAGE( + ChooseBestSolver(prog_), std::invalid_argument, + "There is no available solver for the optimization program"); +} +} // namespace solvers +} // namespace drake diff --git a/solvers/unrevised_lemke_solver.cc b/solvers/unrevised_lemke_solver.cc index 7c56ee851291..7cc7e6df6a58 100644 --- a/solvers/unrevised_lemke_solver.cc +++ b/solvers/unrevised_lemke_solver.cc @@ -919,6 +919,21 @@ SolverId UnrevisedLemkeSolverId::id() { return singleton.access(); } +template +bool UnrevisedLemkeSolver::AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const { + return UnrevisedLemkeSolver::ProgramAttributesSatisfied(prog); +} + +template +bool UnrevisedLemkeSolver::ProgramAttributesSatisfied( + const MathematicalProgram& prog) { + static const never_destroyed solver_capability( + std::initializer_list{ + ProgramAttribute::kLinearComplementarityConstraint}); + return prog.required_capabilities() == solver_capability.access(); +} + } // namespace solvers } // namespace drake diff --git a/solvers/unrevised_lemke_solver.h b/solvers/unrevised_lemke_solver.h index 22a935e89290..69b4a7163e8b 100644 --- a/solvers/unrevised_lemke_solver.h +++ b/solvers/unrevised_lemke_solver.h @@ -89,12 +89,19 @@ class UnrevisedLemkeSolver : public MathematicalProgramSolverInterface { VectorX* z, int* num_pivots, const T& zero_tol = T(-1)) const; - bool available() const override { return true; } + bool available() const override { return is_available(); }; + + static bool is_available() { return true; } SolutionResult Solve(MathematicalProgram& prog) const override; SolverId solver_id() const override; + bool AreProgramAttributesSatisfied( + const MathematicalProgram& prog) const override; + + static bool ProgramAttributesSatisfied(const MathematicalProgram& prog); + private: friend class UnrevisedLemkePrivateTests; friend class UnrevisedLemkePrivateTests_SelectSubMatrixWithCovering_Test;