From ea392f0c4defaea15bbde723f4455e677fb0b293 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Thu, 25 Apr 2024 17:01:35 -0700 Subject: [PATCH] [solvers] Port solver back-ends to use SpecificOptions In the near future, we anticipate changing the SolverOptions API in support of loading and saving (i.e., serialization). That's especially troublesome for our solver back-ends that consume its information, given the low-level and hodge-podge ways in which they hunt for and apply their specific options. This commit introduces a higher-level intermediary between solver back-ends and the program's options. Now SolverOptions are solely a user-facing aspect of defining a program; the solver back-ends never touch SolverOptions anymore. The new SolverBase::DoSolve2 virtual provides the mechanism for back-ends to take advantage of the new API. All solvers have been ported to use it. The old API remains intact for any out-of-tree solvers. The new API is also designed to improve uniformity of common errors, such as unknown names or wrongly-typed values. The new API also lays the groundwork for more efficient processing, as future work. It removes the need for the "Merge" (copy) operation in the hot path -- since it only provides a *view* of the options, it can easily keep track of several dictionaries and query them in order, with no copying. --- solvers/BUILD.bazel | 22 +- solvers/clarabel_solver.cc | 105 +------ solvers/clarabel_solver.h | 5 +- solvers/clp_solver.cc | 57 ++-- solvers/clp_solver.h | 5 +- solvers/common_solver_option.h | 11 + solvers/csdp_solver.cc | 73 +++-- solvers/csdp_solver.h | 5 +- solvers/equality_constrained_qp_solver.cc | 54 ++-- solvers/equality_constrained_qp_solver.h | 5 +- solvers/gurobi_solver.cc | 101 +++---- solvers/gurobi_solver.h | 5 +- solvers/ipopt_solver.cc | 67 +++-- solvers/ipopt_solver.h | 5 +- solvers/mosek_solver.cc | 16 +- solvers/mosek_solver.h | 5 +- solvers/mosek_solver_internal.cc | 106 ++++--- solvers/mosek_solver_internal.h | 17 +- solvers/nlopt_solver.cc | 99 ++++--- solvers/nlopt_solver.h | 5 +- solvers/no_clarabel.cc | 7 +- solvers/no_clp.cc | 6 +- solvers/no_csdp.cc | 6 +- solvers/no_gurobi.cc | 6 +- solvers/no_ipopt.cc | 6 +- solvers/no_mosek.cc | 6 +- solvers/no_nlopt.cc | 6 +- solvers/no_osqp.cc | 6 +- solvers/no_scs.cc | 6 +- solvers/no_snopt.cc | 6 +- solvers/osqp_solver.cc | 126 ++++---- solvers/osqp_solver.h | 5 +- solvers/scs_solver.cc | 168 ++++------- solvers/scs_solver.h | 5 +- solvers/snopt_solver.cc | 136 ++++----- solvers/snopt_solver.h | 5 +- solvers/solver_base.cc | 15 + solvers/solver_base.h | 17 +- solvers/specific_options.cc | 276 ++++++++++++++++++ solvers/specific_options.h | 169 +++++++++++ solvers/test/clarabel_solver_test.cc | 2 +- .../equality_constrained_qp_solver_test.cc | 2 +- solvers/test/mosek_solver_test.cc | 2 +- solvers/test/snopt_solver_test.cc | 14 +- solvers/test/solver_base_test.cc | 157 ++++++---- .../clarabel_cpp_internal/gen_serialize.py | 10 +- .../clarabel_cpp_internal/serialize.h | 2 +- 47 files changed, 1175 insertions(+), 765 deletions(-) create mode 100644 solvers/specific_options.cc create mode 100644 solvers/specific_options.h diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index 39fd59ae2399..6f7b1d4d7ab2 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -80,6 +80,7 @@ drake_cc_package_library( ":solver_type_converter", ":sos_basis_generator", ":sparse_and_dense_matrix", + ":specific_options", ":unrevised_lemke_solver", ], ) @@ -319,6 +320,19 @@ drake_cc_library( deps = [], ) +drake_cc_library( + name = "specific_options", + srcs = ["specific_options.cc"], + hdrs = ["specific_options.h"], + deps = [ + ":solver_id", + ":solver_options", + "//common:name_value", + "//common:overloaded", + "//common:string_container", + ], +) + drake_cc_library( name = "indeterminate", srcs = ["indeterminate.cc"], @@ -327,7 +341,7 @@ drake_cc_library( "//common/symbolic:expression", ], deps = [ - "//solvers:decision_variable", + ":decision_variable", ], ) @@ -725,6 +739,7 @@ drake_cc_library( interface_deps = [ ":mathematical_program", ":solver_interface", + ":specific_options", ], deps = [], ) @@ -877,6 +892,7 @@ drake_cc_optional_library( deps = [ ":aggregate_costs_constraints", ":mathematical_program", + ":specific_options", "//math:quadratic_form", "@mosek", ], @@ -920,9 +936,9 @@ drake_cc_variant_library( ":mathematical_program", ], deps_enabled = [ - "@clp_internal//:clp", "//common:unused", "//math:autodiff", + "@clp_internal//:clp", ], ) @@ -940,9 +956,9 @@ drake_cc_variant_library( ":mathematical_program", ], deps_enabled = [ - "@ipopt", "//common:unused", "//math:autodiff", + "@ipopt", ], ) diff --git a/solvers/clarabel_solver.cc b/solvers/clarabel_solver.cc index 137a87b22271..acbb45f34857 100644 --- a/solvers/clarabel_solver.cc +++ b/solvers/clarabel_solver.cc @@ -148,98 +148,6 @@ void SetSolverDetails( solver_details->status = SolverStatusToString(clarabel_solution.status); } -class SettingsConverter { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SettingsConverter); - - explicit SettingsConverter(const SolverOptions& solver_options) { - // Propagate Drake's common options into `settings_`. - settings_.verbose = solver_options.get_print_to_console(); - // TODO(jwnimmer-tri) Handle get_print_file_name(). - - // Copy the Clarabel-specific `solver_options` to pending maps. - pending_options_double_ = - solver_options.GetOptionsDouble(ClarabelSolver::id()); - pending_options_int_ = solver_options.GetOptionsInt(ClarabelSolver::id()); - pending_options_str_ = solver_options.GetOptionsStr(ClarabelSolver::id()); - - // Move options from `pending_..._` to `settings_`. - Serialize(this, settings_); - - // Identify any unsupported names (i.e., any leftovers in `pending_..._`). - std::vector unknown_names; - for (const auto& [name, _] : pending_options_double_) { - unknown_names.push_back(name); - } - for (const auto& [name, _] : pending_options_int_) { - unknown_names.push_back(name); - } - for (const auto& [name, _] : pending_options_str_) { - unknown_names.push_back(name); - } - if (unknown_names.size() > 0) { - throw std::logic_error(fmt::format( - "ClarabelSolver: unrecognized solver options {}. Please check " - "https://oxfordcontrol.github.io/ClarabelDocs/stable/api_settings/ " - "for the supported solver options.", - fmt::join(unknown_names, ", "))); - } - } - - const clarabel::DefaultSettings& settings() const { - return settings_; - } - - void Visit(const NameValue& x) { - this->SetFromDoubleMap(x.name(), x.value()); - } - void Visit(const NameValue& x) { - auto it = pending_options_int_.find(x.name()); - if (it != pending_options_int_.end()) { - const int option_value = it->second; - DRAKE_THROW_UNLESS(option_value == 0 || option_value == 1); - } - this->SetFromIntMap(x.name(), x.value()); - } - void Visit(const NameValue& x) { - auto it = pending_options_int_.find(x.name()); - if (it != pending_options_int_.end()) { - const int option_value = it->second; - DRAKE_THROW_UNLESS(option_value >= 0); - } - this->SetFromIntMap(x.name(), x.value()); - } - void Visit(const NameValue& x) { - DRAKE_THROW_UNLESS(x.name() == std::string{"direct_solve_method"}); - // TODO(jwnimmer-tri) Add support for this option. - // For now it is unsupported and will throw (as an unknown name, below). - } - - private: - void SetFromDoubleMap(const char* name, double* clarabel_value) { - auto it = pending_options_double_.find(name); - if (it != pending_options_double_.end()) { - *clarabel_value = it->second; - pending_options_double_.erase(it); - } - } - template - void SetFromIntMap(const char* name, T* clarabel_value) { - auto it = pending_options_int_.find(name); - if (it != pending_options_int_.end()) { - *clarabel_value = it->second; - pending_options_int_.erase(it); - } - } - - std::unordered_map pending_options_double_; - std::unordered_map pending_options_int_; - std::unordered_map pending_options_str_; - - clarabel::DefaultSettings settings_ = - clarabel::DefaultSettingsBuilder::default_settings().build(); -}; - // See ParseBoundingBoxConstraints for the meaning of bbcon_dual_indices. void SetBoundingBoxDualSolution( const MathematicalProgram& prog, @@ -294,10 +202,10 @@ bool ClarabelSolver::is_available() { return true; } -void ClarabelSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void ClarabelSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( "ClarabelSolver doesn't support the feature of variable scaling."); @@ -447,8 +355,9 @@ void ClarabelSolver::DoSolve(const MathematicalProgram& prog, A.setFromTriplets(A_triplets.begin(), A_triplets.end()); const Eigen::Map b_vec{b.data(), ssize(b)}; - const SettingsConverter settings_converter(merged_options); - clarabel::DefaultSettings settings = settings_converter.settings(); + clarabel::DefaultSettings settings = + clarabel::DefaultSettingsBuilder::default_settings().build(); + options->CopyToSerializableStruct(&settings); clarabel::DefaultSolver solver(P, q_vec, A, b_vec, cones, settings); diff --git a/solvers/clarabel_solver.h b/solvers/clarabel_solver.h index 8294c44724fc..57e3ef22263f 100644 --- a/solvers/clarabel_solver.h +++ b/solvers/clarabel_solver.h @@ -44,8 +44,9 @@ class ClarabelSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers } // namespace drake diff --git a/solvers/clp_solver.cc b/solvers/clp_solver.cc index 851f4c3606e1..15d44c343eef 100644 --- a/solvers/clp_solver.cc +++ b/solvers/clp_solver.cc @@ -299,38 +299,50 @@ void ParseModelExceptLinearConstraints( } } -int ChooseLogLevel(const SolverOptions& options) { - if (options.get_print_to_console()) { - // Documented as "factorizations plus a bit more" in ClpModel.hpp. - return 3; - } - return 0; +struct KnownOptions { + int log_level{0}; + + // As suggested by the CLP author, we should call scaling() to handle tiny (or + // huge) number in program data. See https://github.com/coin-or/Clp/issues/217 + // for the discussion. + int scaling{1}; +}; + +void Serialize(internal::SpecificOptions* archive, + // NOLINTNEXTLINE(runtime/references) to match Serialize concept. + KnownOptions& options) { + archive->Visit(MakeNameValue("log_level", &options.log_level)); + archive->Visit(MakeNameValue("scaling", &options.scaling)); } -int ChooseScaling(const SolverOptions& options) { - const auto& clp_options = options.GetOptionsInt(ClpSolver::id()); - auto it = clp_options.find("scaling"); - if (it == clp_options.end()) { - // Default scaling is 1. - return 1; - } else { - return it->second; - } +KnownOptions ParseOptions(internal::SpecificOptions* options) { + KnownOptions result; + options->Respell([](const auto& common) { + string_unordered_map respelled; + // '3' is documented as "factorizations plus a bit more" in ClpModel.hpp. + respelled.emplace("log_level", common.print_to_console ? 3 : 0); + return respelled; + }); + options->CopyToSerializableStruct(&result); + return result; } + } // namespace bool ClpSolver::is_available() { return true; } -void ClpSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void ClpSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { // TODO(hongkai.dai): use initial guess and more of the merged options. unused(initial_guess); + const KnownOptions known_options = ParseOptions(options); + ClpSimplex model; - model.setLogLevel(ChooseLogLevel(merged_options)); + model.setLogLevel(known_options.log_level); Eigen::VectorXd xlow(prog.num_vars()); Eigen::VectorXd xupp(prog.num_vars()); Eigen::VectorXd objective_coeff = Eigen::VectorXd::Zero(prog.num_vars()); @@ -359,10 +371,7 @@ void ClpSolver::DoSolve(const MathematicalProgram& prog, &bb_con_dual_variable_indices); } - // As suggested by the CLP author, we should call scaling() to handle tiny (or - // huge) number in program data. See https://github.com/coin-or/Clp/issues/217 - // for the discussion. - model.scaling(ChooseScaling(merged_options)); + model.scaling(known_options.scaling); // Solve model.primal(); diff --git a/solvers/clp_solver.h b/solvers/clp_solver.h index fd493e45111c..77265a5499f5 100644 --- a/solvers/clp_solver.h +++ b/solvers/clp_solver.h @@ -64,8 +64,9 @@ class ClpSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers } // namespace drake diff --git a/solvers/common_solver_option.h b/solvers/common_solver_option.h index 3acded91a14b..702cc0ef8d20 100644 --- a/solvers/common_solver_option.h +++ b/solvers/common_solver_option.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "drake/common/fmt_ostream.h" @@ -31,6 +32,16 @@ enum class CommonSolverOption { std::ostream& operator<<(std::ostream& os, CommonSolverOption common_solver_option); + +namespace internal { + +/* Aggregated values for CommonSolverOption, for Drake-internal use only. */ +struct CommonSolverOptionValues { + std::string print_file_name; + bool print_to_console{false}; +}; + +} // namespace internal } // namespace solvers } // namespace drake diff --git a/solvers/csdp_solver.cc b/solvers/csdp_solver.cc index 83354ec1a43c..8b6fe62a4169 100644 --- a/solvers/csdp_solver.cc +++ b/solvers/csdp_solver.cc @@ -404,17 +404,46 @@ void SolveProgramThroughLorentzConeSlackApproach( constraints_csdp, X_csdp, y, Z); } -std::string MaybeWriteCsdpParams(const SolverOptions& options) { - // We'll consolidate options into this config string to pass to CSDP. - std::string all_csdp_params; - - // Map the common options into CSDP's conventions. - if (options.get_print_to_console()) { - all_csdp_params += "printlevel=1\n"; +// If options has any CSDP settings, writes those settings to a tempfile and +// returns the filename. If not, then returns an empty string. In all cases, +// the `method_out` is overwritten with the selected (or default) method. +std::string MaybeWriteCsdpParams(internal::SpecificOptions* options, + RemoveFreeVariableMethod* method_out) { + DRAKE_DEMAND(method_out != nullptr); + + // Handle drake-specific option. + const int method = + options->template Pop("drake::RemoveFreeVariableMethod") + .value_or(static_cast(RemoveFreeVariableMethod::kNullspace)); + if (!(method >= 1 && method <= 3)) { + throw std::logic_error(fmt::format( + "CsdpSolver: Bad value ({}) for drake::RemoveFreeVariableMethod", + method)); } + *method_out = static_cast(method); + + // All CSDP options are appended to this buffer, which we'll feed in to CSDP + // using a params file on disk. + std::string all_csdp_params; - // TODO(jwnimmer-tri) We could pass through the other named options here, - // if we wanted to. + // Process the user-supplied options. + options->Respell([](const auto& common) { + string_unordered_map respelled; + // Only set the level when printing (i.e., we don't set it zero here), so + // that we can skip writing a temp file when not strictly necessary. + if (common.print_to_console) { + respelled.emplace("printlevel", 1); + } + return respelled; + }); + options->CopyToCallbacks( + [&all_csdp_params](const std::string& key, double value) { + all_csdp_params += fmt::format("{}={}\n", key, value); + }, + [&all_csdp_params](const std::string& key, int value) { + all_csdp_params += fmt::format("{}={}\n", key, value); + }, + /* string options are not allowed */ nullptr); if (all_csdp_params.empty()) { // No need to write a temporary file. @@ -446,19 +475,21 @@ std::string MaybeWriteCsdpParams(const SolverOptions& options) { } // namespace } // namespace internal -void CsdpSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd&, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void CsdpSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd&, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( "CsdpSolver doesn't support the feature of variable scaling."); } // If necessary, write the custom CSDP parameters to a temporary file, which - // we should remove when this function returns. + // we should remove when this function returns. It's convenient to also find + // the Drake-specific RemoveFreeVariableMethod option at the same time. + RemoveFreeVariableMethod method; const std::string csdp_params_pathname = - internal::MaybeWriteCsdpParams(merged_options); + internal::MaybeWriteCsdpParams(options, &method); ScopeExit guard([&csdp_params_pathname]() { if (!csdp_params_pathname.empty()) { ::unlink(csdp_params_pathname.c_str()); @@ -471,18 +502,6 @@ void CsdpSolver::DoSolve(const MathematicalProgram& prog, internal::SolveProgramWithNoFreeVariables(prog, sdpa_free_format, csdp_params_pathname, result); } else { - const auto int_options = merged_options.GetOptionsInt(CsdpSolver::id()); - const auto it_method = int_options.find("drake::RemoveFreeVariableMethod"); - RemoveFreeVariableMethod method = RemoveFreeVariableMethod::kNullspace; - if (it_method != int_options.end()) { - if (it_method->second >= 1 && it_method->second <= 3) { - method = static_cast(it_method->second); - } else { - throw std::runtime_error( - "CsdpSolver::sol(), unknown value for " - "drake::RemoveFreeVariableMethod"); - } - } switch (method) { case RemoveFreeVariableMethod::kNullspace: { internal::SolveProgramThroughNullspaceApproach( diff --git a/solvers/csdp_solver.h b/solvers/csdp_solver.h index 81d34e55bb56..fc4d465f285d 100644 --- a/solvers/csdp_solver.h +++ b/solvers/csdp_solver.h @@ -86,8 +86,9 @@ class CsdpSolver final : public SolverBase { using Details = CsdpSolverDetails; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers } // namespace drake diff --git a/solvers/equality_constrained_qp_solver.cc b/solvers/equality_constrained_qp_solver.cc index 2cc9bd4a56e3..01b057bb9cb4 100644 --- a/solvers/equality_constrained_qp_solver.cc +++ b/solvers/equality_constrained_qp_solver.cc @@ -18,6 +18,8 @@ namespace drake { namespace solvers { namespace { +constexpr const char kFeasibilityTolOptionName[] = "FeasibilityTol"; + // Solves the un-constrained QP problem // min 0.5 * xᵀ * G * x + cᵀ * x SolutionResult SolveUnconstrainedQP(const Eigen::Ref& G, @@ -83,30 +85,25 @@ SolutionResult SolveUnconstrainedQP(const Eigen::Ref& G, } } -struct EqualityConstrainedQPSolverOptions { - // The default tolerance is Eigen's dummy precision. +struct KnownOptions { double feasibility_tol{Eigen::NumTraits::dummy_precision()}; }; -void GetEqualityConstrainedQPSolverOptions( - const SolverOptions& solver_options, - EqualityConstrainedQPSolverOptions* equality_qp_solver_options) { - DRAKE_ASSERT_VOID(solver_options.CheckOptionKeysForSolver( - EqualityConstrainedQPSolver::id(), - {EqualityConstrainedQPSolver::FeasibilityTolOptionName()}, {}, {})); - - const auto& options_double = - solver_options.GetOptionsDouble(EqualityConstrainedQPSolver::id()); - auto it = options_double.find( - EqualityConstrainedQPSolver::FeasibilityTolOptionName()); - if (it != options_double.end()) { - if (it->second >= 0) { - equality_qp_solver_options->feasibility_tol = it->second; - } else { - throw std::invalid_argument( - "FeasibilityTol should be a non-negative number."); - } +void Serialize(internal::SpecificOptions* archive, + // NOLINTNEXTLINE(runtime/references) to match Serialize concept. + KnownOptions& options) { + archive->Visit(MakeNameValue(kFeasibilityTolOptionName, // BR + &options.feasibility_tol)); +} + +KnownOptions ParseOptions(internal::SpecificOptions* options) { + KnownOptions result; + options->CopyToSerializableStruct(&result); + if (!(result.feasibility_tol >= 0)) { + throw std::invalid_argument( + "FeasibilityTol should be a non-negative number."); } + return result; } void SetDualSolutions(const MathematicalProgram& prog, @@ -128,9 +125,9 @@ EqualityConstrainedQPSolver::EqualityConstrainedQPSolver() EqualityConstrainedQPSolver::~EqualityConstrainedQPSolver() = default; -void EqualityConstrainedQPSolver::DoSolve( +void EqualityConstrainedQPSolver::DoSolve2( const MathematicalProgram& prog, const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, + internal::SpecificOptions* options, MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( @@ -167,8 +164,7 @@ void EqualityConstrainedQPSolver::DoSolve( // programming before proceeding. // - J. Nocedal and S. Wright. Numerical Optimization. Springer, 1999. - EqualityConstrainedQPSolverOptions solver_options_struct{}; - GetEqualityConstrainedQPSolverOptions(merged_options, &solver_options_struct); + const KnownOptions parsed_options = ParseOptions(options); size_t num_constraints = 0; for (auto const& binding : prog.linear_equality_constraints()) { @@ -253,7 +249,7 @@ void EqualityConstrainedQPSolver::DoSolve( lambda = qr.solve(rhs); solution_result = - rhs.isApprox(A_iG_A_T * lambda, solver_options_struct.feasibility_tol) + rhs.isApprox(A_iG_A_T * lambda, parsed_options.feasibility_tol) ? SolutionResult::kSolutionFound : SolutionResult::kInfeasibleConstraints; @@ -270,7 +266,7 @@ void EqualityConstrainedQPSolver::DoSolve( Eigen::JacobiSVD svd_A_thin( A, Eigen::ComputeThinU | Eigen::ComputeThinV); const Eigen::VectorXd x0 = svd_A_thin.solve(b); - if (!b.isApprox(A * x0, solver_options_struct.feasibility_tol)) { + if (!b.isApprox(A * x0, parsed_options.feasibility_tol)) { solution_result = SolutionResult::kInfeasibleConstraints; x = x0; Eigen::ColPivHouseholderQR qr_A(A.transpose()); @@ -301,7 +297,7 @@ void EqualityConstrainedQPSolver::DoSolve( Eigen::VectorXd y(N.cols()); solution_result = SolveUnconstrainedQP( N.transpose() * G * N, x0.transpose() * G * N + c.transpose() * N, - solver_options_struct.feasibility_tol, &y); + parsed_options.feasibility_tol, &y); x = x0 + N * y; lambda = qr_A.solve(G * x + c); } @@ -310,7 +306,7 @@ void EqualityConstrainedQPSolver::DoSolve( } else { // num_constraints = 0 solution_result = - SolveUnconstrainedQP(G, c, solver_options_struct.feasibility_tol, &x); + SolveUnconstrainedQP(G, c, parsed_options.feasibility_tol, &x); } result->set_x_val(x); @@ -338,7 +334,7 @@ void EqualityConstrainedQPSolver::DoSolve( } std::string EqualityConstrainedQPSolver::FeasibilityTolOptionName() { - return "FeasibilityTol"; + return kFeasibilityTolOptionName; } SolverId EqualityConstrainedQPSolver::id() { diff --git a/solvers/equality_constrained_qp_solver.h b/solvers/equality_constrained_qp_solver.h index 7da3e390eed0..5bece7f73f94 100644 --- a/solvers/equality_constrained_qp_solver.h +++ b/solvers/equality_constrained_qp_solver.h @@ -41,8 +41,9 @@ class EqualityConstrainedQPSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers diff --git a/solvers/gurobi_solver.cc b/solvers/gurobi_solver.cc index c3046ec9dd55..170e8f4d3e04 100644 --- a/solvers/gurobi_solver.cc +++ b/solvers/gurobi_solver.cc @@ -762,10 +762,10 @@ std::shared_ptr GurobiSolver::AcquireLicense() { // TODO(hongkai.dai@tri.global): break this large DoSolve function to smaller // ones. -void GurobiSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void GurobiSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( "GurobiSolver doesn't support the feature of variable scaling."); @@ -937,71 +937,54 @@ void GurobiSolver::DoSolve(const MathematicalProgram& prog, GRBenv* model_env = GRBgetenv(model); DRAKE_DEMAND(model_env != nullptr); - // Handle common solver options before gurobi-specific options stored in - // merged_options, so that gurobi-specific options can overwrite common solver - // options. - // Gurobi creates a new log file every time we set "LogFile" parameter through - // GRBsetstrparam(). So in order to avoid creating log files repeatedly, we - // store the log file name in @p log_file variable, and only call - // GRBsetstrparam(model_env, "LogFile", log_file) for once. - std::string log_file = merged_options.get_print_file_name(); - if (!error) { - SetOptionOrThrow(model_env, "LogToConsole", - static_cast(merged_options.get_print_to_console())); - } - - // Default the option for number of threads based on an environment variable - // (but only if the user hasn't set the option directly already). - if (!merged_options.GetOptionsInt(id()).contains("Threads")) { + // A couple options don't use the standard GRBset{...}param API. + const bool compute_iis = [&options]() { + const int value = options->template Pop("GRBcomputeIIS").value_or(0); + if (!(value == 0 || value == 1)) { + throw std::runtime_error(fmt::format( + "GurobiSolver(): option GRBcomputeIIS should be either 0 or 1, but " + "is incorrectly set to {}", + value)); + } + return value; + }(); + const std::optional grb_write = + options->template Pop("GRBwrite"); + + // Copy the remaining options into model_env. + options->Respell([](const auto& common) { + string_unordered_map respelled; + if (common.print_to_console) { + respelled.emplace("LogToConsole", 1); + } + if (!common.print_file_name.empty()) { + respelled.emplace("LogFile", common.print_file_name); + } + // Default the option for number of threads based on an environment + // variable. if (char* num_threads_str = std::getenv("GUROBI_NUM_THREADS")) { const std::optional num_threads = ParseInt(num_threads_str); if (num_threads.has_value()) { - SetOptionOrThrow(model_env, "Threads", *num_threads); log()->debug("Using GUROBI_NUM_THREADS={}", *num_threads); + respelled.emplace("Threads", *num_threads); } else { static const logging::Warn log_once( "Ignoring unparseable value '{}' for GUROBI_NUM_THREADS", num_threads_str); } } - } - - for (const auto& it : merged_options.GetOptionsDouble(id())) { - if (!error) { - SetOptionOrThrow(model_env, it.first, it.second); - } - } - bool compute_iis = false; - for (const auto& it : merged_options.GetOptionsInt(id())) { - if (!error) { - if (it.first == "GRBcomputeIIS") { - compute_iis = static_cast(it.second); - if (!(it.second == 0 || it.second == 1)) { - throw std::runtime_error(fmt::format( - "GurobiSolver(): option GRBcomputeIIS should be either " - "0 or 1, but is incorrectly set to {}", - it.second)); - } - } else { - SetOptionOrThrow(model_env, it.first, it.second); - } - } - } - std::optional grb_write; - for (const auto& it : merged_options.GetOptionsStr(id())) { - if (!error) { - if (it.first == "GRBwrite") { - if (it.second != "") { - grb_write = it.second; - } - } else if (it.first == "LogFile") { - log_file = it.second; - } else { - SetOptionOrThrow(model_env, it.first, it.second); - } - } - } - SetOptionOrThrow(model_env, "LogFile", log_file); + return respelled; + }); + options->CopyToCallbacks( + [&model_env](const std::string& key, double value) { + SetOptionOrThrow(model_env, key, value); + }, + [&model_env](const std::string& key, int value) { + SetOptionOrThrow(model_env, key, value); + }, + [&model_env](const std::string& key, const std::string& value) { + SetOptionOrThrow(model_env, key, value); + }); for (int i = 0; i < static_cast(prog.num_vars()); ++i) { if (!error && !std::isnan(initial_guess(i))) { diff --git a/solvers/gurobi_solver.h b/solvers/gurobi_solver.h index c19edf1b6eb6..3988b7b6e603 100644 --- a/solvers/gurobi_solver.h +++ b/solvers/gurobi_solver.h @@ -200,8 +200,9 @@ class GurobiSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; // Note that this is mutable to allow latching the allocation of env_ // during the first call of Solve() (which avoids grabbing a Gurobi license diff --git a/solvers/ipopt_solver.cc b/solvers/ipopt_solver.cc index d0aa35495255..00c5d77cf649 100644 --- a/solvers/ipopt_solver.cc +++ b/solvers/ipopt_solver.cc @@ -758,7 +758,12 @@ class IpoptSolver_NLP : public Ipopt::TNLP { std::unordered_map, int> constraint_dual_start_index_; }; -void SetAppOptions(const SolverOptions& options, Ipopt::IpoptApplication* app) { +// @param[in] options The options to copy into our task. It is mutable so that +// we can mutate it for efficiency; the data may be invalid afterwards, so the +// caller should not use it for anything after we return. +// @param[in,out] app The application to copy the options into. +void SetAppOptions(internal::SpecificOptions* options, + Ipopt::IpoptApplication* app) { // Turn off the banner. app->Options()->SetStringValue("sb", "yes"); @@ -781,30 +786,32 @@ void SetAppOptions(const SolverOptions& options, Ipopt::IpoptApplication* app) { app->Options()->SetStringValue("hessian_approximation", "limited-memory"); - // Note: 0 <= print_level <= 12, with higher numbers more verbose; 4 is very - // useful for debugging. Otherwise, we default to printing nothing. The user - // can always select an arbitrary print level, by setting the ipopt-specific - // option name directly. - const int verbose_level = 4; - const int print_level = options.get_print_to_console() ? verbose_level : 0; - app->Options()->SetIntegerValue("print_level", print_level); - const std::string& output_file = options.get_print_file_name(); - if (!output_file.empty()) { - app->Options()->SetStringValue("output_file", output_file); - app->Options()->SetIntegerValue("file_print_level", verbose_level); - } - - // The solver-specific options will trump our defaults. - const SolverId self = IpoptSolver::id(); - for (const auto& [name, value] : options.GetOptionsDouble(self)) { - app->Options()->SetNumericValue(name, value); - } - for (const auto& [name, value] : options.GetOptionsInt(self)) { - app->Options()->SetIntegerValue(name, value); - } - for (const auto& [name, value] : options.GetOptionsStr(self)) { - app->Options()->SetStringValue(name, value); - } + // Any user-supplied options handled below will overwrite the above defaults. + + options->Respell([](const auto& common) { + // Note: 0 <= print_level <= 12, with higher numbers more verbose; 4 is very + // useful for debugging. Otherwise, we default to printing nothing. The user + // can always select an arbitrary print level, by setting the ipopt-specific + // option name directly. + const int verbose = 4; + string_unordered_map respelled; + respelled.emplace("print_level", common.print_to_console ? verbose : 0); + if (!common.print_file_name.empty()) { + respelled.emplace("output_file", common.print_file_name); + respelled.emplace("file_print_level", verbose); + } + return respelled; + }); + options->CopyToCallbacks( + [&app](const std::string& key, double value) { + app->Options()->SetNumericValue(key, value); + }, + [&app](const std::string& key, int value) { + app->Options()->SetIntegerValue(key, value); + }, + [&app](const std::string& key, const std::string& value) { + app->Options()->SetStringValue(key, value); + }); } } // namespace @@ -870,10 +877,10 @@ bool IpoptSolver::is_available() { return true; } -void IpoptSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void IpoptSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( "IpoptSolver doesn't support the feature of variable scaling."); @@ -882,7 +889,7 @@ void IpoptSolver::DoSolve(const MathematicalProgram& prog, Ipopt::SmartPtr app = IpoptApplicationFactory(); app->RethrowNonIpoptException(true); - SetAppOptions(merged_options, &(*app)); + SetAppOptions(options, &(*app)); Ipopt::ApplicationReturnStatus status = app->Initialize(); if (status != Ipopt::Solve_Succeeded) { diff --git a/solvers/ipopt_solver.h b/solvers/ipopt_solver.h index 9fa6ccff0b0f..a89a67cf743a 100644 --- a/solvers/ipopt_solver.h +++ b/solvers/ipopt_solver.h @@ -62,8 +62,9 @@ class IpoptSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers diff --git a/solvers/mosek_solver.cc b/solvers/mosek_solver.cc index 13fd5da27238..374c1da7612a 100644 --- a/solvers/mosek_solver.cc +++ b/solvers/mosek_solver.cc @@ -83,10 +83,10 @@ bool MosekSolver::is_available() { return true; } -void MosekSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void MosekSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( "MosekSolver doesn't support the feature of variable scaling."); @@ -109,12 +109,10 @@ void MosekSolver::DoSolve(const MathematicalProgram& prog, impl.decision_variable_to_mosek_nonmatrix_variable().size(); // Set the options (parameters). - bool print_to_console{false}; - std::string print_file_name{}; + bool is_printing{}; std::optional msk_writedata; if (rescode == MSK_RES_OK) { - rescode = impl.UpdateOptions(merged_options, id(), &print_to_console, - &print_file_name, &msk_writedata); + impl.UpdateOptions(options, &is_printing, &msk_writedata); } // Always check if rescode is MSK_RES_OK before we call any Mosek functions. @@ -273,7 +271,7 @@ void MosekSolver::DoSolve(const MathematicalProgram& prog, // Refer to // https://docs.mosek.com/latest/capi/debugging-tutorials.html#debugging-tutorials // on printing the solution summary. - if (print_to_console || !print_file_name.empty()) { + if (is_printing) { if (rescode == MSK_RES_OK) { rescode = MSK_solutionsummary(impl.task(), MSK_STREAM_LOG); } diff --git a/solvers/mosek_solver.h b/solvers/mosek_solver.h index aae70537d9f2..2feb8890c202 100644 --- a/solvers/mosek_solver.h +++ b/solvers/mosek_solver.h @@ -116,8 +116,9 @@ class MosekSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; // Note that this is mutable to allow latching the allocation of mosek_env_ // during the first call of Solve() (which avoids grabbing a MOSEK™ license diff --git a/solvers/mosek_solver_internal.cc b/solvers/mosek_solver_internal.cc index e69666a49942..9bec20f91325 100644 --- a/solvers/mosek_solver_internal.cc +++ b/solvers/mosek_solver_internal.cc @@ -1489,65 +1489,63 @@ void MSKAPI printstr(void*, const char str[]) { } // namespace -MSKrescodee MosekSolverProgram::UpdateOptions( - const SolverOptions& merged_options, const SolverId mosek_id, - bool* print_to_console, std::string* print_file_name, +void MosekSolverProgram::UpdateOptions( + internal::SpecificOptions* options, bool* is_printing, std::optional* msk_writedata) { - MSKrescodee rescode{MSK_RES_OK}; - for (const auto& double_options : merged_options.GetOptionsDouble(mosek_id)) { - if (rescode == MSK_RES_OK) { - rescode = MSK_putnadouparam(task_, double_options.first.c_str(), - double_options.second); - ThrowForInvalidOption(rescode, double_options.first, - double_options.second); - } - } - for (const auto& int_options : merged_options.GetOptionsInt(mosek_id)) { - if (rescode == MSK_RES_OK) { - rescode = MSK_putnaintparam(task_, int_options.first.c_str(), - int_options.second); - ThrowForInvalidOption(rescode, int_options.first, int_options.second); - } - } - for (const auto& str_options : merged_options.GetOptionsStr(mosek_id)) { - if (rescode == MSK_RES_OK) { - if (str_options.first == "writedata") { - if (str_options.second != "") { - msk_writedata->emplace(str_options.second); - } - } else { - rescode = MSK_putnastrparam(task_, str_options.first.c_str(), - str_options.second.c_str()); - ThrowForInvalidOption(rescode, str_options.first, str_options.second); - } + // The "writedata" option needs special handling. + *msk_writedata = options->template Pop("writedata"); + + // Copy all remaining options into our `task_`. + options->Respell([&](const auto& common) { + // This is a convenient place to configure printing (i.e., logging); see + // https://docs.mosek.com/10.1/capi/solver-io.html#stream-logging. + // Printing to console vs file are mutually exclusive; if the user has + // requested both, then we must throw an error BEFORE we create the log + // file; otherwise we might create it but never close it. + if (common.print_to_console && common.print_file_name.size()) { + throw std::logic_error( + "MosekSolver: cannot print to both the console and a file"); } - } - // log file. - *print_to_console = merged_options.get_print_to_console(); - *print_file_name = merged_options.get_print_file_name(); - // Refer to https://docs.mosek.com/10.1/capi/solver-io.html#stream-logging - // for Mosek stream logging. - // First we check if the user wants to print to both the console and the file. - // If true, throw an error BEFORE we create the log file through - // MSK_linkfiletotaskstream. Otherwise we might create the log file but cannot - // close it. - if (*print_to_console && !print_file_name->empty()) { - throw std::runtime_error( - "MosekSolver::Solve(): cannot print to both the console and the log " - "file."); - } else if (*print_to_console) { - if (rescode == MSK_RES_OK) { - rescode = + *is_printing = false; + if (common.print_to_console) { + DRAKE_DEMAND(common.print_file_name.empty()); + MSKrescodee rescode = MSK_linkfunctotaskstream(task_, MSK_STREAM_LOG, nullptr, printstr); + if (rescode != MSK_RES_OK) { + throw std::runtime_error(fmt::format( + "MosekSolver(): kPrintToConsole=1 failed with response code {}", + rescode)); + } + *is_printing = true; } - } else if (!print_file_name->empty()) { - if (rescode == MSK_RES_OK) { - rescode = MSK_linkfiletotaskstream(task_, MSK_STREAM_LOG, - print_file_name->c_str(), 0); + if (!common.print_file_name.empty()) { + DRAKE_DEMAND(common.print_to_console == false); + MSKrescodee rescode = MSK_linkfiletotaskstream( + task_, MSK_STREAM_LOG, common.print_file_name.c_str(), 0); + if (rescode != MSK_RES_OK) { + throw std::runtime_error(fmt::format( + "MosekSolver(): kPrintToFile={} failed with response code {}", + common.print_file_name, rescode)); + } + *is_printing = true; } - } - - return rescode; + // None of the common options get converted into new option names. + return string_unordered_map{}; + }); + options->CopyToCallbacks( + [&](const std::string& key, double value) { + MSKrescodee rescode = MSK_putnadouparam(task_, key.c_str(), value); + ThrowForInvalidOption(rescode, key, value); + }, + [&](const std::string& key, int value) { + MSKrescodee rescode = MSK_putnaintparam(task_, key.c_str(), value); + ThrowForInvalidOption(rescode, key, value); + }, + [&](const std::string& key, const std::string& value) { + MSKrescodee rescode = + MSK_putnastrparam(task_, key.c_str(), value.c_str()); + ThrowForInvalidOption(rescode, key, value); + }); } MSKrescodee MosekSolverProgram::SetDualSolution( diff --git a/solvers/mosek_solver_internal.h b/solvers/mosek_solver_internal.h index 18746f77ad55..fc5e473c1124 100644 --- a/solvers/mosek_solver_internal.h +++ b/solvers/mosek_solver_internal.h @@ -14,6 +14,7 @@ #include "drake/solvers/constraint.h" #include "drake/solvers/mathematical_program.h" #include "drake/solvers/mathematical_program_result.h" +#include "drake/solvers/specific_options.h" namespace drake { namespace solvers { @@ -355,17 +356,15 @@ class MosekSolverProgram { MSKint32t>& psd_barvar_indices, MathematicalProgramResult* result) const; - // @param[out] print_to_console Set to true if solver options requires - // printing the log to the console. - // @param[out] print_file_name Set to the name of the print file store in - // solver options. If solver options doesn't store the print file name, then - // set *print_file_name to an empty string. + // @param[in] options The options to copy into our task. It is mutable so + // that we can mutate it for efficiency; the data may be invalid afterwards, + // so the caller should not use it for anything after we return. + // @param[out] is_printing Set to true iff solver is printing to console or + // file, to indicate we want more details when possible. // @param[out] msk_writedata If solver options stores the file for writing // data, then put the file name to msk_writedata for later use. - MSKrescodee UpdateOptions(const SolverOptions& solver_options, - SolverId mosek_id, bool* print_to_console, - std::string* print_file_name, - std::optional* msk_writedata); + void UpdateOptions(internal::SpecificOptions* options, bool* is_printing, + std::optional* msk_writedata); MSKtask_t task() const { return task_; } diff --git a/solvers/nlopt_solver.cc b/solvers/nlopt_solver.cc index 7c811e99a40f..88c3b471b0e2 100644 --- a/solvers/nlopt_solver.cc +++ b/solvers/nlopt_solver.cc @@ -333,48 +333,82 @@ T GetOptionValueWithDefault(const std::unordered_map& options, return it->second; } -nlopt::algorithm GetNloptAlgorithm(const SolverOptions& merged_options) { - const auto& options_str = merged_options.GetOptionsStr(NloptSolver::id()); - auto it = options_str.find(NloptSolver::AlgorithmName()); - if (it == options_str.end()) { - // Use SLSQP for default; +nlopt::algorithm ParseNloptAlgorithm(const std::string& algorithm) { + // Fast path for our default value. + if (algorithm == "LD_SLSQP") { return nlopt::algorithm::LD_SLSQP; - } else { - const std::string& requested_algorithm = it->second; - for (int i = 0; i < nlopt::algorithm::NUM_ALGORITHMS; ++i) { - if (requested_algorithm == - nlopt_algorithm_to_string(static_cast(i))) { - return static_cast(i); - } + } + + // Otherwise, scan for a match. + for (int i = 0; i < nlopt::algorithm::NUM_ALGORITHMS; ++i) { + auto c_enum = static_cast(i); + auto cxx_enum = static_cast(i); + if (algorithm == nlopt_algorithm_to_string(c_enum)) { + return cxx_enum; } - throw std::runtime_error(fmt::format( - "Unknown NLopt algorithm {}, please check " - "nlopt_algorithm_to_string() function " - "github.com/stevengj/nlopt/blob/master/src/api/general.c for " - "all supported algorithm names", - it->second)); } + + // No match; throw a nice error message. + std::vector known; + for (int i = 0; i < nlopt::algorithm::NUM_ALGORITHMS; ++i) { + auto c_enum = static_cast(i); + known.push_back(nlopt_algorithm_to_string(c_enum)); + } + throw std::logic_error(fmt::format( + "Unknown NLopt algorithm {}; valid choices are: {}", + algorithm, fmt::join(known, ", "))); } + +struct KnownOptions { + std::string algorithm{"LD_SLSQP"}; + double constraint_tol{1e-6}; + double xtol_rel{1e-6}; + double xtol_abs{1e-6}; + int max_eval{1000}; +}; + +void Serialize(internal::SpecificOptions* archive, + // NOLINTNEXTLINE(runtime/references) to match Serialize concept. + KnownOptions& options) { + archive->Visit(MakeNameValue(NloptSolver::AlgorithmName().c_str(), // BR + &options.algorithm)); + archive->Visit(MakeNameValue(NloptSolver::ConstraintToleranceName().c_str(), + &options.constraint_tol)); + archive->Visit(MakeNameValue(NloptSolver::XRelativeToleranceName().c_str(), + &options.xtol_rel)); + archive->Visit(MakeNameValue(NloptSolver::XAbsoluteToleranceName().c_str(), + &options.xtol_abs)); + archive->Visit(MakeNameValue(NloptSolver::MaxEvalName().c_str(), // BR + &options.max_eval)); +} + +KnownOptions ParseOptions(internal::SpecificOptions* options) { + KnownOptions result; + options->CopyToSerializableStruct(&result); + return result; +} + } // namespace bool NloptSolver::is_available() { return true; } -void NloptSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void NloptSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( "NloptSolver doesn't support the feature of variable scaling."); } + const KnownOptions parsed_options = ParseOptions(options); + const double constraint_tol = parsed_options.constraint_tol; const int nx = prog.num_vars(); // Load the algo to use and the size. - const nlopt::algorithm algorithm = GetNloptAlgorithm(merged_options); - nlopt::opt opt(algorithm, nx); + nlopt::opt opt(ParseNloptAlgorithm(parsed_options.algorithm), nx); std::vector x(initial_guess.size()); for (size_t i = 0; i < x.size(); i++) { @@ -411,17 +445,6 @@ void NloptSolver::DoSolve(const MathematicalProgram& prog, opt.set_min_objective(EvaluateCosts, const_cast(&prog)); - const auto& nlopt_options_double = merged_options.GetOptionsDouble(id()); - const auto& nlopt_options_int = merged_options.GetOptionsInt(id()); - const double constraint_tol = GetOptionValueWithDefault( - nlopt_options_double, ConstraintToleranceName(), 1e-6); - const double xtol_rel = GetOptionValueWithDefault( - nlopt_options_double, XRelativeToleranceName(), 1e-6); - const double xtol_abs = GetOptionValueWithDefault( - nlopt_options_double, XAbsoluteToleranceName(), 1e-6); - const int max_eval = - GetOptionValueWithDefault(nlopt_options_int, MaxEvalName(), 1000); - std::list wrapped_vector; // TODO(sam.creasey): Missing test coverage for generic constraints @@ -452,9 +475,9 @@ void NloptSolver::DoSolve(const MathematicalProgram& prog, WrapConstraint(prog, c, constraint_tol, &opt, &wrapped_vector); } - opt.set_xtol_rel(xtol_rel); - opt.set_xtol_abs(xtol_abs); - opt.set_maxeval(max_eval); + opt.set_xtol_rel(parsed_options.xtol_rel); + opt.set_xtol_abs(parsed_options.xtol_abs); + opt.set_maxeval(parsed_options.max_eval); result->set_solution_result(SolutionResult::kSolutionFound); diff --git a/solvers/nlopt_solver.h b/solvers/nlopt_solver.h index 72b8c51fe8f4..736727167be7 100644 --- a/solvers/nlopt_solver.h +++ b/solvers/nlopt_solver.h @@ -55,8 +55,9 @@ class NloptSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers diff --git a/solvers/no_clarabel.cc b/solvers/no_clarabel.cc index d052074445ac..720aeff31b51 100644 --- a/solvers/no_clarabel.cc +++ b/solvers/no_clarabel.cc @@ -10,9 +10,10 @@ bool ClarabelSolver::is_available() { return false; } -void ClarabelSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void ClarabelSolver::DoSolve2(const MathematicalProgram&, + const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The Clarabel bindings were not compiled. You'll need to use a " "different solver."); diff --git a/solvers/no_clp.cc b/solvers/no_clp.cc index a77eeae65a7f..78c14bb1679a 100644 --- a/solvers/no_clp.cc +++ b/solvers/no_clp.cc @@ -10,9 +10,9 @@ bool ClpSolver::is_available() { return false; } -void ClpSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void ClpSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The CLP bindings were not compiled. You'll need to use a different " "solver."); diff --git a/solvers/no_csdp.cc b/solvers/no_csdp.cc index 723b5318eedb..6951032bbac9 100644 --- a/solvers/no_csdp.cc +++ b/solvers/no_csdp.cc @@ -11,9 +11,9 @@ bool CsdpSolver::is_available() { return false; } -void CsdpSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void CsdpSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The CSDP bindings were not compiled. You'll need to use a different " "solver."); diff --git a/solvers/no_gurobi.cc b/solvers/no_gurobi.cc index 13e3fb4c2b5c..f819b91ef621 100644 --- a/solvers/no_gurobi.cc +++ b/solvers/no_gurobi.cc @@ -14,9 +14,9 @@ bool GurobiSolver::is_available() { return false; } -void GurobiSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void GurobiSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The Gurobi bindings were not compiled. You'll need to use a different " "solver."); diff --git a/solvers/no_ipopt.cc b/solvers/no_ipopt.cc index 3de57bd5b79e..a355b241a4a8 100644 --- a/solvers/no_ipopt.cc +++ b/solvers/no_ipopt.cc @@ -17,9 +17,9 @@ bool IpoptSolver::is_available() { return false; } -void IpoptSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void IpoptSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The IPOPT bindings were not compiled. You'll need to use a different " "solver."); diff --git a/solvers/no_mosek.cc b/solvers/no_mosek.cc index 06102cdfac10..aabcc358cf22 100644 --- a/solvers/no_mosek.cc +++ b/solvers/no_mosek.cc @@ -18,9 +18,9 @@ bool MosekSolver::is_available() { return false; } -void MosekSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void MosekSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw runtime_error( "Mosek is not installed in your build. You'll need to use a different " "solver."); diff --git a/solvers/no_nlopt.cc b/solvers/no_nlopt.cc index 06cb3ffc8030..5b237c4a9c48 100644 --- a/solvers/no_nlopt.cc +++ b/solvers/no_nlopt.cc @@ -11,9 +11,9 @@ bool NloptSolver::is_available() { return false; } -void NloptSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void NloptSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The Nlopt bindings were not compiled. You'll need to use a different " "solver."); diff --git a/solvers/no_osqp.cc b/solvers/no_osqp.cc index 868f3d44ba39..2d45f1c6bdb2 100644 --- a/solvers/no_osqp.cc +++ b/solvers/no_osqp.cc @@ -11,9 +11,9 @@ bool OsqpSolver::is_available() { return false; } -void OsqpSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void OsqpSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The OSQP bindings were not compiled. You'll need to use a different " "solver."); diff --git a/solvers/no_scs.cc b/solvers/no_scs.cc index c0b1a5eaf73e..09f836c35669 100644 --- a/solvers/no_scs.cc +++ b/solvers/no_scs.cc @@ -11,9 +11,9 @@ bool ScsSolver::is_available() { return false; } -void ScsSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void ScsSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The SCS bindings were not compiled. You'll need to use a different " "solver."); diff --git a/solvers/no_snopt.cc b/solvers/no_snopt.cc index 8475ab33d6a2..6fb1cd09b9de 100644 --- a/solvers/no_snopt.cc +++ b/solvers/no_snopt.cc @@ -11,9 +11,9 @@ bool SnoptSolver::is_available() { return false; } -void SnoptSolver::DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, - MathematicalProgramResult*) const { +void SnoptSolver::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { throw std::runtime_error( "The SNOPT bindings were not compiled. You'll need to use a different " "solver."); diff --git a/solvers/osqp_solver.cc b/solvers/osqp_solver.cc index 3ebebb22d101..2c2a9ac7bfe9 100644 --- a/solvers/osqp_solver.cc +++ b/solvers/osqp_solver.cc @@ -9,6 +9,44 @@ #include "drake/common/text_logging.h" #include "drake/math/eigen_sparse_triplet.h" #include "drake/solvers/mathematical_program.h" +#include "drake/solvers/specific_options.h" + +// This function must appear in the global namespace -- the Serialize pattern +// uses ADL (argument-dependent lookup) and the namespace for the OSQPSettings +// struct is the global namespace. (We can't even use an anonymous namespace!) +static void Serialize( + drake::solvers::internal::SpecificOptions* archive, + // NOLINTNEXTLINE(runtime/references) to match Serialize concept. + OSQPSettings& settings) { + using drake::MakeNameValue; + archive->Visit(MakeNameValue("rho", &settings.rho)); + archive->Visit(MakeNameValue("sigma", &settings.sigma)); + archive->Visit(MakeNameValue("max_iter", &settings.max_iter)); + archive->Visit(MakeNameValue("eps_abs", &settings.eps_abs)); + archive->Visit(MakeNameValue("eps_rel", &settings.eps_rel)); + archive->Visit(MakeNameValue("eps_prim_inf", &settings.eps_prim_inf)); + archive->Visit(MakeNameValue("eps_dual_inf", &settings.eps_dual_inf)); + archive->Visit(MakeNameValue("alpha", &settings.alpha)); + archive->Visit(MakeNameValue("delta", &settings.delta)); + archive->Visit(MakeNameValue("polish", &settings.polish)); + archive->Visit(MakeNameValue("polish_refine_iter", // BR + &settings.polish_refine_iter)); + archive->Visit(MakeNameValue("verbose", &settings.verbose)); + archive->Visit(MakeNameValue("scaled_termination", // BR + &settings.scaled_termination)); + archive->Visit(MakeNameValue("check_termination", // BR + &settings.check_termination)); + archive->Visit(MakeNameValue("warm_start", &settings.warm_start)); + archive->Visit(MakeNameValue("scaling", &settings.scaling)); + archive->Visit(MakeNameValue("adaptive_rho", &settings.adaptive_rho)); + archive->Visit(MakeNameValue("adaptive_rho_interval", // BR + &settings.adaptive_rho_interval)); + archive->Visit(MakeNameValue("adaptive_rho_tolerance", // BR + &settings.adaptive_rho_tolerance)); + archive->Visit(MakeNameValue("adaptive_rho_fraction", // BR + &settings.adaptive_rho_fraction)); + archive->Visit(MakeNameValue("time_limit", &settings.time_limit)); +} namespace drake { namespace solvers { @@ -236,71 +274,6 @@ csc* EigenSparseToCSC(const Eigen::SparseMatrix& mat) { inner_indices, outer_indices); } -template -void SetOsqpSolverSetting(const std::unordered_map& options, - const std::string& option_name, - T2* osqp_setting_field) { - const auto it = options.find(option_name); - if (it != options.end()) { - *osqp_setting_field = it->second; - } -} - -template -void SetOsqpSolverSettingWithDefaultValue( - const std::unordered_map& options, - const std::string& option_name, T2* osqp_setting_field, - const T1& default_field_value) { - const auto it = options.find(option_name); - if (it != options.end()) { - *osqp_setting_field = it->second; - } else { - *osqp_setting_field = default_field_value; - } -} - -void SetOsqpSolverSettings(const SolverOptions& solver_options, - OSQPSettings* settings) { - const std::unordered_map& options_double = - solver_options.GetOptionsDouble(OsqpSolver::id()); - const std::unordered_map& options_int = - solver_options.GetOptionsInt(OsqpSolver::id()); - SetOsqpSolverSetting(options_double, "rho", &(settings->rho)); - SetOsqpSolverSetting(options_double, "sigma", &(settings->sigma)); - SetOsqpSolverSetting(options_int, "max_iter", &(settings->max_iter)); - SetOsqpSolverSetting(options_double, "eps_abs", &(settings->eps_abs)); - SetOsqpSolverSetting(options_double, "eps_rel", &(settings->eps_rel)); - SetOsqpSolverSetting(options_double, "eps_prim_inf", - &(settings->eps_prim_inf)); - SetOsqpSolverSetting(options_double, "eps_dual_inf", - &(settings->eps_dual_inf)); - SetOsqpSolverSetting(options_double, "alpha", &(settings->alpha)); - SetOsqpSolverSetting(options_double, "delta", &(settings->delta)); - // Default polish to true, to get an accurate solution. - SetOsqpSolverSettingWithDefaultValue(options_int, "polish", - &(settings->polish), 1); - SetOsqpSolverSetting(options_int, "polish_refine_iter", - &(settings->polish_refine_iter)); - // The fallback value for console verbosity is the value set by drake options. - int verbose_console = solver_options.get_print_to_console() != 0; - SetOsqpSolverSettingWithDefaultValue(options_int, "verbose", - &(settings->verbose), verbose_console); - SetOsqpSolverSetting(options_int, "scaled_termination", - &(settings->scaled_termination)); - SetOsqpSolverSetting(options_int, "check_termination", - &(settings->check_termination)); - SetOsqpSolverSetting(options_int, "warm_start", &(settings->warm_start)); - SetOsqpSolverSetting(options_int, "scaling", &(settings->scaling)); - SetOsqpSolverSetting(options_int, "adaptive_rho", &(settings->adaptive_rho)); - SetOsqpSolverSetting(options_double, "adaptive_rho_interval", - &(settings->adaptive_rho_interval)); - SetOsqpSolverSetting(options_double, "adaptive_rho_tolerance", - &(settings->adaptive_rho_tolerance)); - SetOsqpSolverSetting(options_double, "adaptive_rho_fraction", - &(settings->adaptive_rho_fraction)); - SetOsqpSolverSetting(options_double, "time_limit", &(settings->time_limit)); -} - template void SetDualSolution( const std::vector>& constraints, @@ -325,10 +298,10 @@ bool OsqpSolver::is_available() { return true; } -void OsqpSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void OsqpSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { OsqpSolverDetails& solver_details = result->SetSolverDetailsType(); @@ -368,13 +341,20 @@ void OsqpSolver::DoSolve(const MathematicalProgram& prog, data->l = l.data(); data->u = u.data(); - // Define Solver settings as default. - // Problem settings + // Create the settings, initialized to the upstream defaults. OSQPSettings* settings = static_cast(c_malloc(sizeof(OSQPSettings))); osqp_set_default_settings(settings); - - SetOsqpSolverSettings(merged_options, settings); + // Customize the defaults for Drake. + // - Default polish to true, to get an accurate solution. + settings->polish = 1; + // Apply the user's additional options (if any). + options->Respell([](const auto& common) { + string_unordered_map respelled; + respelled.emplace("verbose", common.print_to_console ? 1 : 0); + return respelled; + }); + options->CopyToSerializableStruct(settings); // If any step fails, it will set the solution_result and skip other steps. std::optional solution_result; diff --git a/solvers/osqp_solver.h b/solvers/osqp_solver.h index 21d4fda8ad86..365ee2dda84e 100644 --- a/solvers/osqp_solver.h +++ b/solvers/osqp_solver.h @@ -60,8 +60,9 @@ class OsqpSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers } // namespace drake diff --git a/solvers/scs_solver.cc b/solvers/scs_solver.cc index 143f198a19aa..c0f3d8c85bbe 100644 --- a/solvers/scs_solver.cc +++ b/solvers/scs_solver.cc @@ -24,6 +24,34 @@ #include "drake/solvers/mathematical_program_result.h" #include "drake/solvers/scs_clarabel_common.h" +// This function must appear in the global namespace -- the Serialize pattern +// uses ADL (argument-dependent lookup) and the namespace for the ScsSettings +// struct is the global namespace. (We can't even use an anonymous namespace!) +static void Serialize( + drake::solvers::internal::SpecificOptions* archive, + // NOLINTNEXTLINE(runtime/references) to match Serialize concept. + ScsSettings& settings) { + using drake::MakeNameValue; + archive->Visit(MakeNameValue("normalize", &settings.normalize)); + archive->Visit(MakeNameValue("scale", &settings.scale)); + archive->Visit(MakeNameValue("adaptive_scale", &settings.adaptive_scale)); + archive->Visit(MakeNameValue("rho_x", &settings.rho_x)); + archive->Visit(MakeNameValue("max_iters", &settings.max_iters)); + archive->Visit(MakeNameValue("eps_abs", &settings.eps_abs)); + archive->Visit(MakeNameValue("eps_rel", &settings.eps_rel)); + archive->Visit(MakeNameValue("eps_infeas", &settings.eps_infeas)); + archive->Visit(MakeNameValue("alpha", &settings.alpha)); + archive->Visit(MakeNameValue("time_limit_secs", &settings.time_limit_secs)); + archive->Visit(MakeNameValue("verbose", &settings.verbose)); + archive->Visit(MakeNameValue("warm_start", &settings.warm_start)); + archive->Visit(MakeNameValue("acceleration_lookback", // BR + &settings.acceleration_lookback)); + archive->Visit(MakeNameValue("acceleration_interval", // BR + &settings.acceleration_interval)); + // TODO(jwnimmer-tri) Handle write_data_filename. + // TODO(jwnimmer-tri) Handle log_csv_filename. +} + namespace drake { namespace solvers { namespace { @@ -252,112 +280,6 @@ bool ScsSolver::is_available() { return true; } -namespace { -// This should be invoked only once on each unique instance of ScsSettings. -// Namely, only call this function for once in DoSolve. -void SetScsSettings(std::unordered_map* solver_options_int, - const bool print_to_console, ScsSettings* scs_settings) { - auto it = solver_options_int->find("normalize"); - if (it != solver_options_int->end()) { - scs_settings->normalize = it->second; - solver_options_int->erase(it); - } - it = solver_options_int->find("adaptive_scale;"); - if (it != solver_options_int->end()) { - scs_settings->adaptive_scale = it->second; - solver_options_int->erase(it); - } - it = solver_options_int->find("max_iters"); - if (it != solver_options_int->end()) { - scs_settings->max_iters = it->second; - solver_options_int->erase(it); - } - it = solver_options_int->find("verbose"); - if (it != solver_options_int->end()) { - // The solver specific option has the highest priority. - scs_settings->verbose = it->second; - solver_options_int->erase(it); - } else { - // The common option has the second highest priority. - scs_settings->verbose = print_to_console ? 1 : 0; - } - it = solver_options_int->find("warm_start"); - if (it != solver_options_int->end()) { - scs_settings->warm_start = it->second; - solver_options_int->erase(it); - } - it = solver_options_int->find("acceleration_lookback"); - if (it != solver_options_int->end()) { - scs_settings->acceleration_lookback = it->second; - solver_options_int->erase(it); - } - it = solver_options_int->find("acceleration_interval"); - if (it != solver_options_int->end()) { - scs_settings->acceleration_interval = it->second; - solver_options_int->erase(it); - } - if (!solver_options_int->empty()) { - throw std::invalid_argument("Unsupported SCS solver options."); - } -} - -// This should be invoked only once on each unique instance of ScsSettings. -// Namely, only call this function for once in DoSolve. -void SetScsSettings( - std::unordered_map* solver_options_double, - ScsSettings* scs_settings) { - auto it = solver_options_double->find("scale"); - if (it != solver_options_double->end()) { - scs_settings->scale = it->second; - solver_options_double->erase(it); - } - it = solver_options_double->find("rho_x"); - if (it != solver_options_double->end()) { - scs_settings->rho_x = it->second; - solver_options_double->erase(it); - } - it = solver_options_double->find("eps_abs"); - if (it != solver_options_double->end()) { - scs_settings->eps_abs = it->second; - solver_options_double->erase(it); - } else { - // SCS 3.0 uses 1E-4 as the default value, see - // https://www.cvxgrp.org/scs/api/settings.html?highlight=eps_abs). This - // tolerance is too loose. We set the default tolerance to 1E-5 for better - // accuracy. - scs_settings->eps_abs = 1E-5; - } - it = solver_options_double->find("eps_rel"); - if (it != solver_options_double->end()) { - scs_settings->eps_rel = it->second; - solver_options_double->erase(it); - } else { - // SCS 3.0 uses 1E-4 as the default value, see - // https://www.cvxgrp.org/scs/api/settings.html?highlight=eps_rel). This - // tolerance is too loose. We set the default tolerance to 1E-5 for better - // accuracy. - scs_settings->eps_rel = 1E-5; - } - it = solver_options_double->find("eps_infeas"); - if (it != solver_options_double->end()) { - scs_settings->eps_infeas = it->second; - solver_options_double->erase(it); - } - it = solver_options_double->find("alpha"); - if (it != solver_options_double->end()) { - scs_settings->alpha = it->second; - solver_options_double->erase(it); - } - it = solver_options_double->find("time_limit_secs"); - if (it != solver_options_double->end()) { - scs_settings->time_limit_secs = it->second; - solver_options_double->erase(it); - } - if (!solver_options_double->empty()) { - throw std::invalid_argument("Unsupported SCS solver options."); - } -} - void SetBoundingBoxDualSolution( const MathematicalProgram& prog, const Eigen::Ref& y, const std::vector>>& bbcon_dual_indices, @@ -384,12 +306,11 @@ void SetBoundingBoxDualSolution( result->set_dual_solution(prog.bounding_box_constraints()[i], bbcon_dual); } } -} // namespace -void ScsSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { +void ScsSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { if (!prog.GetVariableScaling().empty()) { static const logging::Warn log_once( "ScsSolver doesn't support the feature of variable scaling."); @@ -465,6 +386,24 @@ void ScsSolver::DoSolve(const MathematicalProgram& prog, // Set the parameters to default values. scs_set_default_settings(scs_stgs); + // Customize the defaults for Drake: + // - SCS 3.0 uses 1E-4 as the default value, see + // https://www.cvxgrp.org/scs/api/settings.html?highlight=eps_abs). This + // tolerance is too loose. We set the default tolerance to 1E-5 for better + // accuracy. + scs_stgs->eps_abs = 1E-5; + // - SCS 3.0 uses 1E-4 as the default value, see + // https://www.cvxgrp.org/scs/api/settings.html?highlight=eps_rel). This + // tolerance is too loose. We set the default tolerance to 1E-5 for better + // accuracy. + scs_stgs->eps_rel = 1E-5; + // Apply the user's additional custom options (if any). + options->Respell([](const auto& common) { + string_unordered_map respelled; + respelled.emplace("verbose", common.print_to_console ? 1 : 0); + return respelled; + }); + options->CopyToSerializableStruct(scs_stgs); // A_row_count will increment, when we add each constraint. int A_row_count = 0; @@ -600,13 +539,6 @@ void ScsSolver::DoSolve(const MathematicalProgram& prog, SetScsProblemData(A_row_count, num_x, A, b, P_upper_triplets, c, scs_problem_data); - std::unordered_map input_solver_options_int = - merged_options.GetOptionsInt(id()); - std::unordered_map input_solver_options_double = - merged_options.GetOptionsDouble(id()); - SetScsSettings(&input_solver_options_int, - merged_options.get_print_to_console(), scs_stgs); - SetScsSettings(&input_solver_options_double, scs_stgs); ScsInfo scs_info{0}; diff --git a/solvers/scs_solver.h b/solvers/scs_solver.h index 878c295018a6..4a1a11d44401 100644 --- a/solvers/scs_solver.h +++ b/solvers/scs_solver.h @@ -85,8 +85,9 @@ class ScsSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers diff --git a/solvers/snopt_solver.cc b/solvers/snopt_solver.cc index cc47add68982..a17949113388 100644 --- a/solvers/snopt_solver.cc +++ b/solvers/snopt_solver.cc @@ -1114,13 +1114,10 @@ void PruneGradientDuplication(int nx, const std::vector& iGfun_w_duplicate, } } -void SolveWithGivenOptions( - const MathematicalProgram& prog, - const Eigen::Ref& x_init, - const std::unordered_map& snopt_options_string, - const std::unordered_map& snopt_options_int, - const std::unordered_map& snopt_options_double, - const std::string& print_file_common, MathematicalProgramResult* result) { +void SolveWithGivenOptions(const MathematicalProgram& prog, + const Eigen::Ref& x_init, + internal::SpecificOptions* options, + MathematicalProgramResult* result) { SnoptSolverDetails& solver_details = result->SetSolverDetailsType(); @@ -1128,14 +1125,26 @@ void SolveWithGivenOptions( WorkspaceStorage storage(&user_info); const auto& scale_map = prog.GetVariableScaling(); - std::string print_file_name = print_file_common; - const auto print_file_it = snopt_options_string.find("Print file"); - if (print_file_it != snopt_options_string.end()) { - print_file_name = print_file_it->second; - } - Snopt::sninit(print_file_name.c_str(), print_file_name.length(), - 0 /* no summary */, storage.iw(), storage.leniw(), storage.rw(), - storage.lenrw()); + options->Respell([](const auto& common) { + string_unordered_map respelled; + respelled.emplace("Print file", common.print_file_name); + // If "Timing level" is not zero, then snopt periodically calls etime to + // determine it's usage of cpu time (which on Linux calls getrusage in the + // libgfortran implementation). Unfortunately getrusage is called using + // RUSAGE_SELF, which has unfortunate consenquences when using threads, + // namely (1) It returns the total count of CPU usage for all threads, so + // the result is garbage, and (2) on Linux the kernel holds a process wide + // lock inside getrusage when RUSAGE_SELF is specified, so other threads + // using snopt end up blocking on their getrusage calls. Under the theory + // that a user who actually wants this behavior will turn it on + // deliberately, default "Timing level" to zero. + respelled.emplace("Timing level", 0); + return respelled; + }); + const std::string print_file = + options->template Pop("Print file").value_or(""); + Snopt::sninit(print_file.c_str(), print_file.length(), 0 /* no summary */, + storage.iw(), storage.leniw(), storage.rw(), storage.lenrw()); ScopeExit guard([&storage]() { Snopt::snend(storage.iw(), storage.leniw(), storage.rw(), storage.lenrw()); }); @@ -1292,40 +1301,37 @@ void SolveWithGivenOptions( const int lenG = iGfun.size(); user_info.set_lenG(lenG); - for (const auto& it : snopt_options_double) { - int errors = 0; - Snopt::snsetr(it.first.c_str(), it.first.length(), it.second, &errors, - storage.iw(), storage.leniw(), storage.rw(), storage.lenrw()); - if (errors > 0) { - throw std::runtime_error("Error setting Snopt double parameter " + - it.first); - } - } - - for (const auto& it : snopt_options_int) { - int errors = 0; - Snopt::snseti(it.first.c_str(), it.first.length(), it.second, &errors, - storage.iw(), storage.leniw(), storage.rw(), storage.lenrw()); - if (errors > 0) { - throw std::runtime_error("Error setting Snopt integer parameter " + - it.first); - } - } - - for (const auto& it : snopt_options_string) { - int errors = 0; - auto option_string = it.first + " " + it.second; - if (it.first == "Print file") { - // Already handled during sninit, above - continue; - } - Snopt::snset(option_string.c_str(), option_string.length(), &errors, - storage.iw(), storage.leniw(), storage.rw(), storage.lenrw()); - if (errors > 0) { - throw std::runtime_error("Error setting Snopt string parameter " + - it.first); - } - } + // Copy `options` into the snopt program storage. + options->CopyToCallbacks( + [&storage](const std::string& key, double value) { + int errors = 0; + Snopt::snsetr(key.c_str(), key.length(), value, &errors, storage.iw(), + storage.leniw(), storage.rw(), storage.lenrw()); + if (errors > 0) { + throw std::logic_error(fmt::format( + "Error setting Snopt double parameter {}={}", key, value)); + } + }, + [&storage](const std::string& key, int value) { + int errors = 0; + Snopt::snseti(key.c_str(), key.length(), value, &errors, storage.iw(), + storage.leniw(), storage.rw(), storage.lenrw()); + if (errors > 0) { + throw std::logic_error(fmt::format( + "Error setting Snopt integer parameter {}={}", key, value)); + } + }, + [&storage](const std::string& key, const std::string& value) { + int errors = 0; + auto option_string = key + " " + value; + Snopt::snset(option_string.c_str(), option_string.length(), &errors, + storage.iw(), storage.leniw(), storage.rw(), + storage.lenrw()); + if (errors > 0) { + throw std::logic_error(fmt::format( + "Error setting Snopt string parameter {}={}", key, value)); + } + }); int Cold = 0; double objective_constant = linear_cost_constant_term; @@ -1400,33 +1406,11 @@ bool SnoptSolver::is_available() { return true; } -void SnoptSolver::DoSolve(const MathematicalProgram& prog, - const Eigen::VectorXd& initial_guess, - const SolverOptions& merged_options, - MathematicalProgramResult* result) const { - // Call SNOPT. - std::unordered_map int_options = - merged_options.GetOptionsInt(id()); - - // If "Timing level" is not zero, then snopt periodically calls etime to - // determine it's usage of cpu time (which on Linux calls getrusage in the - // libgfortran implementation). Unfortunately getrusage is called using - // RUSAGE_SELF, which has unfortunate consenquences when using threads, - // namely (1) It returns the total count of CPU usage for all threads, so - // the result is garbage, and (2) on Linux the kernel holds a process wide - // lock inside getrusage when RUSAGE_SELF is specified, so other threads - // using snopt end up blocking on their getrusage calls. Under the theory - // that a user who actually wants this behavior will turn it on - // deliberately, set "Timing level" to zero if the user hasn't requested - // another value. - const std::string kTimingLevel = "Timing level"; - if (!int_options.contains(kTimingLevel)) { - int_options[kTimingLevel] = 0; - } - - SolveWithGivenOptions(prog, initial_guess, merged_options.GetOptionsStr(id()), - int_options, merged_options.GetOptionsDouble(id()), - merged_options.get_print_file_name(), result); +void SnoptSolver::DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { + SolveWithGivenOptions(prog, initial_guess, options, result); } bool SnoptSolver::is_bounded_lp_broken() { diff --git a/solvers/snopt_solver.h b/solvers/snopt_solver.h index 0d16ddaa2710..ab324619502f 100644 --- a/solvers/snopt_solver.h +++ b/solvers/snopt_solver.h @@ -79,8 +79,9 @@ class SnoptSolver final : public SolverBase { using SolverBase::Solve; private: - void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&, - const SolverOptions&, MathematicalProgramResult*) const final; + void DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const final; }; } // namespace solvers diff --git a/solvers/solver_base.cc b/solvers/solver_base.cc index cb30735a19b8..ae5818330fd1 100644 --- a/solvers/solver_base.cc +++ b/solvers/solver_base.cc @@ -110,5 +110,20 @@ std::string SolverBase::ExplainUnsatisfiedProgramAttributes( ShortName(*this), to_string(prog.required_capabilities())); } +void SolverBase::DoSolve(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + const SolverOptions& merged_options, + MathematicalProgramResult* result) const { + internal::SpecificOptions options{&solver_id_, &merged_options}; + DoSolve2(prog, initial_guess, &options, result); +} + +void SolverBase::DoSolve2(const MathematicalProgram&, const Eigen::VectorXd&, + internal::SpecificOptions*, + MathematicalProgramResult*) const { + throw std::logic_error(fmt::format("{} failed to override any DoSolve method", + ShortName(*this))); +} + } // namespace solvers } // namespace drake diff --git a/solvers/solver_base.h b/solvers/solver_base.h index 6a1c8a832e3b..6aa8c4d31387 100644 --- a/solvers/solver_base.h +++ b/solvers/solver_base.h @@ -12,6 +12,7 @@ #include "drake/solvers/solver_id.h" #include "drake/solvers/solver_interface.h" #include "drake/solvers/solver_options.h" +#include "drake/solvers/specific_options.h" namespace drake { namespace solvers { @@ -66,7 +67,21 @@ class SolverBase : public SolverInterface { virtual void DoSolve(const MathematicalProgram& prog, const Eigen::VectorXd& initial_guess, const SolverOptions& merged_options, - MathematicalProgramResult* result) const = 0; + MathematicalProgramResult* result) const; + + /** (Internal use only) Like DoSolve() but using SpecificOptions instead of + SolverOptions. The "2" here means "version 2", to help disabiguate various + DoSolve overloads. + + By default, DoSolve() will delegate to DoSolve2() and DoSolve2() will throw. + Subclasses should override exactly one of the various DoSolver methods. + Solvers wrapped inside Drake should override DoSolve2(); solvers wrapped in + downstream projects must override DoSolve() since this function is marked as + internal use only. */ + virtual void DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& initial_guess, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const; private: SolverId solver_id_; diff --git a/solvers/specific_options.cc b/solvers/specific_options.cc new file mode 100644 index 000000000000..922b3eb7612e --- /dev/null +++ b/solvers/specific_options.cc @@ -0,0 +1,276 @@ +#include "drake/solvers/specific_options.h" + +#include +#include +#include + +#include "drake/common/overloaded.h" + +namespace drake { +namespace solvers { +namespace internal { + +using OptionValue = SolverOptions::OptionValue; + +SpecificOptions::SpecificOptions(const SolverId* id, + const SolverOptions* all_options) + : id_{id}, all_options_{all_options} { + DRAKE_DEMAND(id != nullptr); + DRAKE_DEMAND(all_options != nullptr); +} + +SpecificOptions::~SpecificOptions() = default; + +void SpecificOptions::Respell( + const std::function( + const CommonSolverOptionValues&)>& respell) { + DRAKE_DEMAND(respell != nullptr); + respelled_ = respell(CommonSolverOptionValues{ + .print_file_name = all_options_->get_print_file_name(), + .print_to_console = all_options_->get_print_to_console(), + }); +} + +template +std::optional SpecificOptions::Pop(std::string_view key) { + if (popped_.contains(key)) { + return std::nullopt; + } + popped_.emplace(key); + const auto& typed_options = all_options_->template GetOptions(*id_); + if (auto iter = typed_options.find(std::string{key}); + iter != typed_options.end()) { + return iter->second; + } + if (auto iter = respelled_.find(key); iter != respelled_.end()) { + const OptionValue& value = iter->second; + if (std::holds_alternative(value)) { + return std::get(value); + } + throw std::logic_error(fmt::format( + "{}: internal error: option {} was respelled to the wrong type", + id_->name(), key)); + } + return {}; +} + +template std::optional SpecificOptions::Pop(std::string_view); +template std::optional SpecificOptions::Pop(std::string_view); +template std::optional SpecificOptions::Pop(std::string_view); + +void SpecificOptions::CopyToCallbacks( + const std::function& set_double, + const std::function& set_int, + const std::function& + set_string) const { + // Bail out early when we have no options at all for this solver. + const std::unordered_map& options_double = + all_options_->GetOptionsDouble(*id_); + const std::unordered_map& options_int = + all_options_->GetOptionsInt(*id_); + const std::unordered_map& options_str = + all_options_->GetOptionsStr(*id_); + if (options_double.empty() && options_int.empty() && options_str.empty() && + respelled_.empty()) { + return; + } + + // Wrap the solver's set_{type} callbacks with error-reporting sugar, and + // logic to promote integers to doubles. + auto on_double = [this, &set_double](const std::string& key, double value) { + if (popped_.contains(key)) { + return; + } + if (set_double != nullptr) { + set_double(key, value); + return; + } + throw std::logic_error(fmt::format( + "{}: floating-point options are not supported; the option {}={} is " + "invalid", + id_->name(), key, value)); + }; + auto on_int = [this, &set_int, &set_double](const std::string& key, + int value) { + if (popped_.contains(key)) { + return; + } + if (set_int != nullptr) { + set_int(key, value); + return; + } + if (set_double != nullptr) { + set_double(key, value); + return; + } + throw std::logic_error(fmt::format( + "{}: integer and floating-point options are not supported; the option " + "{}={} is invalid", + id_->name(), key, value)); + }; + auto on_string = [this, &set_string](const std::string& key, + const std::string& value) { + if (popped_.contains(key)) { + return; + } + if (set_string != nullptr) { + set_string(key, value); + return; + } + throw std::logic_error(fmt::format( + "{}: string options are not supported; the option {}='{}' is invalid", + id_->name(), key, value)); + }; + + // Handle solver-specific options. + for (const auto& [key, value] : options_double) { + on_double(key, value); + } + for (const auto& [key, value] : options_int) { + on_int(key, value); + } + for (const auto& [key, value] : options_str) { + on_string(key, value); + } + + // Handle any respelled options, being careful not to set anything that has + // already been set. + for (const auto& [respelled_key, boxed_value] : respelled_) { + // Pedantially, lambdas cannot capture a structured binding so we need to + // make a local variable that we can capture. + const auto& key = respelled_key; + std::visit( + overloaded{[&key, &on_double, &options_double](double value) { + if (!options_double.contains(key)) { + on_double(key, value); + } + }, + [&key, &on_int, &options_int](int value) { + if (!options_int.contains(key)) { + on_int(key, value); + } + }, + [&key, &on_string, &options_str](const std::string& value) { + if (!options_str.contains(key)) { + on_string(key, value); + } + }}, + boxed_value); + } +} + +void SpecificOptions::InitializePending() { + pending_keys_.clear(); + for (const auto& [key, _] : all_options_->GetOptionsDouble(*id_)) { + pending_keys_.insert(key); + } + for (const auto& [key, _] : all_options_->GetOptionsInt(*id_)) { + pending_keys_.insert(key); + } + for (const auto& [key, _] : all_options_->GetOptionsStr(*id_)) { + pending_keys_.insert(key); + } + for (const auto& [key, _] : respelled_) { + pending_keys_.insert(key); + } + for (const auto& key : popped_) { + pending_keys_.erase(key); + } +} + +void SpecificOptions::CheckNoPending() const { + // Identify any unsupported names (i.e., leftovers in `pending_`). + if (!pending_keys_.empty()) { + std::vector unknown_names; + for (const auto& name : pending_keys_) { + unknown_names.push_back(name); + } + std::sort(unknown_names.begin(), unknown_names.end()); + throw std::logic_error(fmt::format( + "{}: the following solver option names were not recognized: {}", + id_->name(), fmt::join(unknown_names, ", "))); + } +} + +std::optional SpecificOptions::PrepareToCopy(const char* name) { + DRAKE_DEMAND(name != nullptr); + const std::unordered_map& options_double = + all_options_->GetOptionsDouble(*id_); + if (auto iter = options_double.find(std::string{name}); + iter != options_double.end()) { + pending_keys_.erase(iter->first); + return iter->second; + } + const std::unordered_map& options_int = + all_options_->GetOptionsInt(*id_); + if (auto iter = options_int.find(std::string{name}); + iter != options_int.end()) { + pending_keys_.erase(iter->first); + return iter->second; + } + const std::unordered_map& options_str = + all_options_->GetOptionsStr(*id_); + if (auto iter = options_str.find(std::string{name}); + iter != options_str.end()) { + pending_keys_.erase(iter->first); + return iter->second; + } + if (auto iter = respelled_.find(name); iter != respelled_.end()) { + pending_keys_.erase(iter->first); + return iter->second; + } + return {}; +} + +template +void SpecificOptions::CopyFloatingPointOption(const char* name, T* output) { + DRAKE_DEMAND(output != nullptr); + if (auto value = PrepareToCopy(name)) { + if (std::holds_alternative(*value)) { + *output = std::get(*value); + return; + } + if (std::holds_alternative(*value)) { + *output = std::get(*value); + return; + } + throw std::logic_error( + fmt::format("{}: Expected a floating-point value for option {}", + id_->name(), name)); + } +} +template void SpecificOptions::CopyFloatingPointOption(const char*, double*); +template void SpecificOptions::CopyFloatingPointOption(const char*, float*); + +template +void SpecificOptions::CopyIntegralOption(const char* name, T* output) { + DRAKE_DEMAND(output != nullptr); + if (auto value = PrepareToCopy(name)) { + if (std::holds_alternative(*value)) { + // XXX bool validation / unsigned / etc. + *output = std::get(*value); + return; + } + throw std::logic_error(fmt::format( + "{}: Expected an integer value for option {}", id_->name(), name)); + } +} +template void SpecificOptions::CopyIntegralOption(const char*, int*); +template void SpecificOptions::CopyIntegralOption(const char*, bool*); +template void SpecificOptions::CopyIntegralOption(const char*, uint32_t*); + +void SpecificOptions::CopyStringOption(const char* name, std::string* output) { + DRAKE_DEMAND(output != nullptr); + if (auto value = PrepareToCopy(name)) { + if (std::holds_alternative(*value)) { + *output = std::get(*value); + return; + } + throw std::logic_error(fmt::format( + "{}: Expected a string value for option {}", id_->name(), name)); + } +} + +} // namespace internal +} // namespace solvers +} // namespace drake diff --git a/solvers/specific_options.h b/solvers/specific_options.h new file mode 100644 index 000000000000..d15186e00c26 --- /dev/null +++ b/solvers/specific_options.h @@ -0,0 +1,169 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/drake_copyable.h" +#include "drake/common/name_value.h" +#include "drake/common/string_unordered_map.h" +#include "drake/common/string_unordered_set.h" +#include "drake/solvers/solver_id.h" +#include "drake/solvers/solver_options.h" + +namespace drake { +namespace solvers { +namespace internal { + +/* Helper class that propagates options for a specific solver from Drake's +generic SolverOptions struct into the specific solver's options API. + +This class is intended a short-lived helper. It aliases an immutable list of +overall SolverOptions, and provides convenient tools to map those options into +whatever back-end is necessary. Two different mechanisms are offered, depending +on whether the solver's option names are static or dynamic: CopyToCallbacks or +CopyToSerializableStruct. + +(Method 1 - dynamic) CopyToCallbacks: This method is appropriate for solver +back-ends that offer an API like "set option via string name". The back-end +provides callbacks for each supported data type (i.e., double, int, string), and +this class loops over the stored options and invokes the callbacks. It is up to +the solver back-end to detect unknown or mis-typed options and throw. + +(Method 2 - static) CopyToSerializableStruct: This method is appropriate for +solvers that offer a statically typed `struct MyOptions { ... }` which needs to +be filled in. The caller provides a Serialize() wrapper around that struct, and +this class directly writes the fields and reports errors for unknown names and +mismatched data types. + +With both methods, there are also some ahead-of-time operations that are often +useful: + +- Respell() can project CommonSolverOption values into the back-end vocabulary + so that only the back-end specific names need to be implemented during options + handling. + +- Pop() can yank options that require special handling, so that they will not + participate in the Method 1 or 2 copying / callbacks. + +For examples of use, refer to all of the existing Drake solver wrappers. */ +class SpecificOptions { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SpecificOptions); + + /* Creates a converter that reads the subset of `all_options` destined for the + given `id`. Both arguments are aliased, so must outlive this object. */ + SpecificOptions(const SolverId* id, const SolverOptions* all_options); + + ~SpecificOptions(); + + /* The `respell` callback will be used to respell the CommonSolverOption + values. Any options returned will be handled as if they were solver-specific + options, at a lower priority than any solver-specific options the user already + provided for those names (in our constructor). It is OK for the callback to + have side-effects; it will be invoked exactly once and is not retained. + As such, this can also serve as a way to inject default values (by returning + then from the respell callback here). + @pre This function may be called at most once (per converter object). */ + void Respell( + const std::function( + const CommonSolverOptionValues&)>& respell); + + /* Returns and effectively removes the value of the option named `key`. (To be + specific -- `all_options` remain unchanged; instead, the `key` is memorized + and future calls to either of the `CopyTo...` functions will skip over it.) + If the option was not set, returns nullopt. When checking if an option was + set, this checks `all_options` for our `id` as well as any options added + during a Respell(). + @tparam T must be one of: double, int, std::string. */ + template + std::optional Pop(std::string_view key); + + /* Helper for "Method 1 - dynamic", per our class overview. Converts options + when the solver offers a generic key-value API where options are passed by + string name. */ + void CopyToCallbacks( + const std::function& + set_double, + const std::function& set_int, + const std::function& set_string) const; + + /* Helper for "Method 2 - static", per our class overview. Converts options + when the solver uses an options struct with names known at compile time. The + `Output` struct must obey Drake's Serialize() protocol, for us to interrogate + the option names. + @param [in,out] result The solver's specific options struct; any options not + mentioned in `solver_options` will remain unchanged. */ + template + void CopyToSerializableStruct(Result* result) { + DRAKE_DEMAND(result != nullptr); + InitializePending(); + Serialize(this, *result); + CheckNoPending(); + } + + /* (Internal use only) Helper function for CopyToSerializableStruct(). */ + template + void Visit(const NameValue& x) { + if constexpr (std::is_floating_point_v) { + CopyFloatingPointOption(x.name(), x.value()); + } else if constexpr (std::is_integral_v) { + CopyIntegralOption(x.name(), x.value()); + } else if constexpr (std::is_same_v) { + CopyStringOption(x.name(), x.value()); + } else { + static_assert(std::is_void_v, "Unsupported template argument T"); + } + } + + private: + /* Helper function for CopyToSerializableStruct(). Sets pending_keys_ to the + union of all keys from all_options_ (for our id_) and respelled_, excluding + anything already popped_. */ + void InitializePending(); + + /* Helper function for CopyToSerializableStruct(). If pending_keys_ is + non-empty, throws an error about the unhandled options. */ + void CheckNoPending() const; + + /* Helper function for CopyToSerializableStruct(). Finds the option with the + given name, removes it from pending_keys_, and returns it. */ + std::optional PrepareToCopy(const char* name); + + /* Output helper functions for CopyToSerializableStruct(). + Each one sets its `output` argument to the option value for the given name, + and removes the name from pending_keys_. If the name is not in options, does + nothing (output keeps its prior value). */ + //@{ + template + void CopyFloatingPointOption(const char* name, T* output); + template + void CopyIntegralOption(const char* name, T* output); + void CopyStringOption(const char* name, std::string* output); + //@} + + // The solver we're operating on behalf of (as passed to our constructor). + const SolverId* const id_; + + // The full options for all solvers (as passed to our constructor). + const SolverOptions* const all_options_; + + // The result of the Respell() callback (or empty, if never called). + string_unordered_map respelled_; + + // Items that have already been popped. + string_unordered_set popped_; + + // Temporary storage during CopyToSerializableStruct() of option names that + // are set but haven't yet been processed by Visit(). + std::unordered_set pending_keys_; +}; + +} // namespace internal +} // namespace solvers +} // namespace drake diff --git a/solvers/test/clarabel_solver_test.cc b/solvers/test/clarabel_solver_test.cc index 58e62be44620..9b62f2b85917 100644 --- a/solvers/test/clarabel_solver_test.cc +++ b/solvers/test/clarabel_solver_test.cc @@ -443,7 +443,7 @@ GTEST_TEST(TestOptions, unrecognized) { solver_options.SetOption(solver.id(), "bad_unrecognized", 1); DRAKE_EXPECT_THROWS_MESSAGE( solver.Solve(dut.prog(), std::nullopt, solver_options), - ".*unrecognized solver options bad_unrecognized.*"); + ".*not recognized.*bad_unrecognized.*"); } } diff --git a/solvers/test/equality_constrained_qp_solver_test.cc b/solvers/test/equality_constrained_qp_solver_test.cc index 0c88c06d0aea..385500efd802 100644 --- a/solvers/test/equality_constrained_qp_solver_test.cc +++ b/solvers/test/equality_constrained_qp_solver_test.cc @@ -394,7 +394,7 @@ TEST_F(EqualityConstrainedQPSolverTest, WrongSolverOptions1) { solver_options_.SetOption(solver_.solver_id(), "Foo", 0.1); DRAKE_EXPECT_THROWS_MESSAGE_IF_ARMED( solver_.Solve(prog_, {}, solver_options_, &result_), - "Foo is not allowed in the SolverOptions for EqConstrainedQP."); + ".*not recognized.*Foo.*"); } TEST_F(EqualityConstrainedQPSolverTest, WrongSolverOptions2) { diff --git a/solvers/test/mosek_solver_test.cc b/solvers/test/mosek_solver_test.cc index c97bb50c3200..c57a3dfb0ab8 100644 --- a/solvers/test/mosek_solver_test.cc +++ b/solvers/test/mosek_solver_test.cc @@ -286,7 +286,7 @@ GTEST_TEST(MosekTest, TestLogging) { solver_options.SetOption(CommonSolverOption::kPrintToConsole, 1); DRAKE_EXPECT_THROWS_MESSAGE( solver.Solve(prog, {}, solver_options, &result), - ".* cannot print to both the console and the log file."); + ".*cannot print to both the console and a file.*"); } GTEST_TEST(MosekTest, SolverOptionsTest) { diff --git a/solvers/test/snopt_solver_test.cc b/solvers/test/snopt_solver_test.cc index f7dfb0b3f4fb..324e1a3130ed 100644 --- a/solvers/test/snopt_solver_test.cc +++ b/solvers/test/snopt_solver_test.cc @@ -539,26 +539,28 @@ GTEST_TEST(SnoptSolverTest, EckhardtDualSolution) { GTEST_TEST(SnoptSolverTest, BadIntegerParameter) { SnoptSolver solver; MathematicalProgram prog; - prog.SetSolverOption(solver.solver_id(), "not an option", 15); + prog.SetSolverOption(solver.solver_id(), "not_an_option", 15); DRAKE_EXPECT_THROWS_MESSAGE( solver.Solve(prog), - "Error setting Snopt integer parameter not an option"); + "Error setting Snopt integer parameter not_an_option=15"); } GTEST_TEST(SnoptSolverTest, BadDoubleParameter) { SnoptSolver solver; MathematicalProgram prog; - prog.SetSolverOption(solver.solver_id(), "not an option", 15.0); + prog.SetSolverOption(solver.solver_id(), "not_an_option", 15.1); DRAKE_EXPECT_THROWS_MESSAGE( - solver.Solve(prog), "Error setting Snopt double parameter not an option"); + solver.Solve(prog), + "Error setting Snopt double parameter not_an_option=15.1"); } GTEST_TEST(SnoptSolverTest, BadStringParameter) { SnoptSolver solver; MathematicalProgram prog; - prog.SetSolverOption(solver.solver_id(), "not an option", "test"); + prog.SetSolverOption(solver.solver_id(), "not_an_option", "test"); DRAKE_EXPECT_THROWS_MESSAGE( - solver.Solve(prog), "Error setting Snopt string parameter not an option"); + solver.Solve(prog), + "Error setting Snopt string parameter not_an_option=test"); } GTEST_TEST(SnoptSolverTest, TestNonconvexQP) { diff --git a/solvers/test/solver_base_test.cc b/solvers/test/solver_base_test.cc index 85fde09f5235..237914f50f49 100644 --- a/solvers/test/solver_base_test.cc +++ b/solvers/test/solver_base_test.cc @@ -12,8 +12,9 @@ namespace { using ::testing::HasSubstr; -// A stub subclass of SolverBase, so that we can instantiate and test it. -class StubSolverBase final : public SolverBase { +// A stub subclass of SolverBase, to help instantiate and test it. +// This intermediate class does NOT override DoSolve nor DoSolve2. +class StubSolverBase : public SolverBase { public: DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(StubSolverBase) StubSolverBase() @@ -48,6 +49,24 @@ class StubSolverBase final : public SolverBase { }), unsatisfied_explanation_(std::move(explanation)) {} + // Helper static method for SolverBase ctor. + static SolverId id() { + static const never_destroyed result{"stub"}; + return result.access(); + } + + // The return values for stubbed methods. + bool available_{true}; + bool enabled_{true}; + bool satisfied_{true}; + std::string unsatisfied_explanation_; +}; + +// A concrete SolverBase implementation that overrides DoSolve(). +class StubSolverBase1 final : public StubSolverBase { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(StubSolverBase1) + using StubSolverBase::StubSolverBase; void DoSolve(const MathematicalProgram& prog, const Eigen::VectorXd& x_init, const SolverOptions& options, MathematicalProgramResult* result) const final { @@ -63,22 +82,31 @@ class StubSolverBase final : public SolverBase { } result->set_x_val(x_val); } +}; - // Helper static method for SolverBase ctor. - static SolverId id() { - static const never_destroyed result{"stub"}; - return result.access(); +// A concrete SolverBase implementation that overrides DoSolve2(). +class StubSolverBase2 final : public StubSolverBase { + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(StubSolverBase2) + using StubSolverBase::StubSolverBase; + virtual void DoSolve2(const MathematicalProgram& prog, + const Eigen::VectorXd& x_init, + internal::SpecificOptions* options, + MathematicalProgramResult* result) const { + result->set_solution_result(kSolutionFound); + result->set_optimal_cost(1.0); + Eigen::VectorXd x_val = x_init; + if (const auto x0 = options->Pop("x0_solution")) { + x_val[0] = *x0; + } + if (const auto x1 = options->Pop("x1_solution")) { + x_val[1] = *x1; + } + result->set_x_val(x_val); } - - // The return values for stubbed methods. - bool available_{true}; - bool enabled_{true}; - bool satisfied_{true}; - std::string unsatisfied_explanation_; }; GTEST_TEST(SolverBaseTest, BasicAccessors) { - StubSolverBase dut; + StubSolverBase1 dut; EXPECT_EQ(dut.solver_id(), StubSolverBase::id()); dut.available_ = false; @@ -97,11 +125,11 @@ GTEST_TEST(SolverBaseTest, BasicAccessors) { GTEST_TEST(SolverBaseTest, ProgramAttributesDefault) { const MathematicalProgram prog; - StubSolverBase dut; + StubSolverBase1 dut; dut.satisfied_ = false; EXPECT_FALSE(dut.AreProgramAttributesSatisfied(prog)); EXPECT_THAT(dut.ExplainUnsatisfiedProgramAttributes(prog), - HasSubstr("StubSolverBase is unable to solve")); + HasSubstr("StubSolverBase1 is unable to solve")); dut.satisfied_ = true; EXPECT_TRUE(dut.AreProgramAttributesSatisfied(prog)); @@ -113,7 +141,7 @@ GTEST_TEST(SolverBaseTest, ProgramAttributesDefault) { GTEST_TEST(SolverBaseTest, ProgramAttributesCustom) { const MathematicalProgram prog; - StubSolverBase dut("Do not meddle in the affairs of wizards!"); + StubSolverBase1 dut("Do not meddle in the affairs of wizards!"); dut.satisfied_ = false; EXPECT_FALSE(dut.AreProgramAttributesSatisfied(prog)); EXPECT_THAT(dut.ExplainUnsatisfiedProgramAttributes(prog), @@ -125,65 +153,77 @@ GTEST_TEST(SolverBaseTest, ProgramAttributesCustom) { } GTEST_TEST(SolverBaseTest, SolveAsOutputArgument) { - const StubSolverBase dut; MathematicalProgram mutable_prog; const MathematicalProgram& prog = mutable_prog; auto vars = mutable_prog.NewContinuousVariables(2); - MathematicalProgramResult result; - - // Check that the prog's initial guess and options are used. + // On the program itself, set an initial guess and solver options so that we + // can check that they make it all the way through the solve. mutable_prog.SetSolverOption(StubSolverBase::id(), "x0_solution", 10.0); mutable_prog.SetInitialGuess(vars[1], 11.0); - dut.Solve(prog, {}, {}, &result); - EXPECT_EQ(result.get_solver_id(), StubSolverBase::id()); - EXPECT_TRUE(result.is_success()); - EXPECT_EQ(result.get_solution_result(), kSolutionFound); - EXPECT_EQ(result.get_optimal_cost(), 1.0); - ASSERT_EQ(result.get_x_val().size(), 2); - EXPECT_EQ(result.get_x_val()[0], 10.0); - EXPECT_EQ(result.get_x_val()[1], 11.0); - EXPECT_EQ(result.GetSolution(vars[0]), 10.0); - EXPECT_EQ(result.GetSolution(vars[1]), 11.0); - - // Check that Solve()'s initial guess takes precedence. - dut.Solve(prog, Eigen::VectorXd(Vector2(30.0, 31.0)), {}, &result); - EXPECT_EQ(result.get_x_val()[0], 10.0); - EXPECT_EQ(result.get_x_val()[1], 31.0); - - // Check that Solve's option get merged, but prog's options still apply. - SolverOptions extra_options; - extra_options.SetOption(StubSolverBase::id(), "x1_solution", 41.0); - dut.Solve(prog, {}, extra_options, &result); - EXPECT_EQ(result.get_x_val()[0], 10.0); - EXPECT_EQ(result.get_x_val()[1], 41.0); - - // Check that Solve's options win. - extra_options.SetOption(StubSolverBase::id(), "x0_solution", 40.0); - dut.Solve(prog, {}, extra_options, &result); - EXPECT_EQ(result.get_x_val()[0], 40.0); - EXPECT_EQ(result.get_x_val()[1], 41.0); + + // In SolverBase, we have two flavors of DoSolve to test here. (This is the + // only test case were testing both is relevant / important.) + for (int api = 1; api <= 2; ++api) { + SCOPED_TRACE(fmt::format("api = {}", api)); + std::unique_ptr dut; + if (api == 1) { + dut = std::make_unique(); + } else { + dut = std::make_unique(); + } + + MathematicalProgramResult result; + dut->Solve(prog, {}, {}, &result); + EXPECT_EQ(result.get_solver_id(), StubSolverBase::id()); + EXPECT_TRUE(result.is_success()); + EXPECT_EQ(result.get_solution_result(), kSolutionFound); + EXPECT_EQ(result.get_optimal_cost(), 1.0); + ASSERT_EQ(result.get_x_val().size(), 2); + EXPECT_EQ(result.get_x_val()[0], 10.0); + EXPECT_EQ(result.get_x_val()[1], 11.0); + EXPECT_EQ(result.GetSolution(vars[0]), 10.0); + EXPECT_EQ(result.GetSolution(vars[1]), 11.0); + + // Check that Solve()'s initial guess takes precedence. + dut->Solve(prog, Eigen::VectorXd(Vector2(30.0, 31.0)), {}, &result); + EXPECT_EQ(result.get_x_val()[0], 10.0); + EXPECT_EQ(result.get_x_val()[1], 31.0); + + // Check that Solve's option get merged, but prog's options still apply. + SolverOptions extra_options; + extra_options.SetOption(StubSolverBase::id(), "x1_solution", 41.0); + dut->Solve(prog, {}, extra_options, &result); + EXPECT_EQ(result.get_x_val()[0], 10.0); + EXPECT_EQ(result.get_x_val()[1], 41.0); + + // Check that Solve's options win. + extra_options.SetOption(StubSolverBase::id(), "x0_solution", 40.0); + dut->Solve(prog, {}, extra_options, &result); + EXPECT_EQ(result.get_x_val()[0], 40.0); + EXPECT_EQ(result.get_x_val()[1], 41.0); + } } // Check the error message when the solver is not available. GTEST_TEST(SolverBaseTest, AvailableError) { const MathematicalProgram prog; - StubSolverBase dut; + StubSolverBase1 dut; dut.available_ = false; DRAKE_EXPECT_THROWS_MESSAGE(dut.Solve(prog, {}, {}), - ".*StubSolverBase has not been compiled.*"); + ".*StubSolverBase1 has not been compiled.*"); } // Check the error message when attributes are not satisfied. GTEST_TEST(SolverBaseTest, ProgramAttributesError) { const MathematicalProgram prog; - StubSolverBase dut; + StubSolverBase1 dut; dut.satisfied_ = false; DRAKE_EXPECT_THROWS_MESSAGE(dut.Solve(prog, {}, {}), - ".*StubSolverBase is unable to solve.*"); + ".*StubSolverBase1 is unable to solve.*"); } GTEST_TEST(SolverBaseTest, SolveAndReturn) { - const StubSolverBase dut; + const StubSolverBase1 dut; const MathematicalProgram prog; const MathematicalProgramResult result = dut.Solve(prog, {}, {}); EXPECT_EQ(result.get_solver_id(), StubSolverBase::id()); @@ -200,11 +240,20 @@ GTEST_TEST(SolverBaseTest, SolveAndReturn) { GTEST_TEST(SolverBaseTest, InitialGuessSizeError) { MathematicalProgram prog; prog.NewContinuousVariables<2>(); - StubSolverBase dut; + StubSolverBase1 dut; DRAKE_EXPECT_THROWS_MESSAGE(dut.Solve(prog, Eigen::VectorXd(3), {}), "Solve expects initial guess of size 2, got 3."); } +// Check that nothing absurd happens when a developer forgets to override either +// of the DoSolve/DoSolve2 functions. +GTEST_TEST(SolverBaseTest, NoSolve) { + // N.B. The next line uses StubSolverBase not StubSolverBase{1,2}. + StubSolverBase dut; + const MathematicalProgram prog; + DRAKE_EXPECT_THROWS_MESSAGE(dut.Solve(prog, {}, {}), ".*override.*DoSolve.*"); +} + } // namespace } // namespace test } // namespace solvers diff --git a/tools/workspace/clarabel_cpp_internal/gen_serialize.py b/tools/workspace/clarabel_cpp_internal/gen_serialize.py index d1863c047ac7..041a324765c6 100644 --- a/tools/workspace/clarabel_cpp_internal/gen_serialize.py +++ b/tools/workspace/clarabel_cpp_internal/gen_serialize.py @@ -7,6 +7,11 @@ from python import runfiles +_FIELDS_TO_SKIP = { + # This is an enum, which we don't support yet. + "direct_solve_method", +} + _PROLOGUE = """\ #pragma once @@ -71,7 +76,10 @@ def _settings_names(): def _create_header_text(): result = _PROLOGUE for name in _settings_names(): - result += f" DRAKE_VISIT({name});\n" + if name in _FIELDS_TO_SKIP: + result += f" // skipped: {name}\n" + else: + result += f" DRAKE_VISIT({name});\n" result += _EPILOGUE return result diff --git a/tools/workspace/clarabel_cpp_internal/serialize.h b/tools/workspace/clarabel_cpp_internal/serialize.h index 1c45e4052aca..1a134def5b23 100644 --- a/tools/workspace/clarabel_cpp_internal/serialize.h +++ b/tools/workspace/clarabel_cpp_internal/serialize.h @@ -38,7 +38,7 @@ void Serialize(Archive* a, DefaultSettings& settings) { DRAKE_VISIT(min_switch_step_length); DRAKE_VISIT(min_terminate_step_length); DRAKE_VISIT(direct_kkt_solver); - DRAKE_VISIT(direct_solve_method); + // skipped: direct_solve_method DRAKE_VISIT(static_regularization_enable); DRAKE_VISIT(static_regularization_constant); DRAKE_VISIT(static_regularization_proportional);