Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[solvers] Port solver back-ends to use SpecificOptions #21345

Merged
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions solvers/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -729,6 +729,7 @@ drake_cc_library(
deps = [
":mathematical_program",
":solver_interface",
":specific_options",
],
)

Expand Down Expand Up @@ -880,6 +881,7 @@ drake_cc_optional_library(
implementation_deps = [
":aggregate_costs_constraints",
":mathematical_program",
":specific_options",
"//common:parallelism",
"//math:quadratic_form",
"@mosek",
Expand Down
118 changes: 17 additions & 101 deletions solvers/clarabel_solver.cc
Original file line number Diff line number Diff line change
Expand Up @@ -151,96 +151,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().

// Clarabel does not support setting the number of threads so we ignore
// the kMaxNumThreads option.

// 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<std::string> 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<double>& settings() const {
return settings_;
}

void Visit(const NameValue<double>& x) {
this->SetFromDoubleMap(x.name(), x.value());
}
void Visit(const NameValue<bool>& 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<uint32_t>& 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());
}

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 <typename T>
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<std::string, double> pending_options_double_;
std::unordered_map<std::string, int> pending_options_int_;
std::unordered_map<std::string, std::string> pending_options_str_;

clarabel::DefaultSettings<double> settings_ =
clarabel::DefaultSettingsBuilder<double>::default_settings().build();
};

// See ParseBoundingBoxConstraints for the meaning of bbcon_dual_indices.
void SetBoundingBoxDualSolution(
const MathematicalProgram& prog,
Expand Down Expand Up @@ -400,10 +310,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.");
Expand Down Expand Up @@ -557,14 +467,20 @@ void ClarabelSolver::DoSolve(const MathematicalProgram& prog,
A.setFromTriplets(A_triplets.begin(), A_triplets.end());
const Eigen::Map<Eigen::VectorXd> b_vec{b.data(), ssize(b)};

const SettingsConverter settings_converter(merged_options);
clarabel::DefaultSettings<double> settings = settings_converter.settings();
options->Respell([&](const auto& common, auto* respelled) {
respelled->emplace("verbose", common.print_to_console ? 1 : 0);
// TODO(jwnimmer-tri) Handle common.print_file_name.
if (!common.standalone_reproduction_file_name.empty()) {
WriteClarabelReproduction(common.standalone_reproduction_file_name, P,
q_vec, A, b_vec, cones);
}
// Clarabel does not support setting the number of threads so we ignore the
// kMaxThreads option.
});
clarabel::DefaultSettings<double> settings =
clarabel::DefaultSettingsBuilder<double>::default_settings().build();
options->CopyToSerializableStruct(&settings);

std::string repro_file_name =
merged_options.get_standalone_reproduction_file_name();
if (!repro_file_name.empty()) {
WriteClarabelReproduction(repro_file_name, P, q_vec, A, b_vec, cones);
}
clarabel::DefaultSolver<double> solver(P, q_vec, A, b_vec, cones, settings);

solver.solve();
Expand Down
5 changes: 3 additions & 2 deletions solvers/clarabel_solver.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
60 changes: 33 additions & 27 deletions solvers/clp_solver.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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, auto* respelled) {
// '3' is documented as "factorizations plus a bit more" in ClpModel.hpp.
respelled->emplace("log_level", common.print_to_console ? 3 : 0);
// CLP Simplex solver does not support multithreaded solves so we can ignore
// the kMaxThreads option.
});
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());
Expand Down Expand Up @@ -359,13 +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));

// CLP Simplex solver does not support multithreaded solves so we can ignore
// the kMaxNumThreads option.
model.scaling(known_options.scaling);

// Solve
model.primal();
Expand Down
5 changes: 3 additions & 2 deletions solvers/clp_solver.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
90 changes: 55 additions & 35 deletions solvers/csdp_solver.cc
Original file line number Diff line number Diff line change
Expand Up @@ -404,25 +404,55 @@ 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.
{
// The RemoveFreeVariableMethod is `enum class`' so we must cast to `int`s.
constexpr int kTwoSlackVariables =
static_cast<int>(RemoveFreeVariableMethod::kTwoSlackVariables);
constexpr int kNullspace =
static_cast<int>(RemoveFreeVariableMethod::kNullspace);
constexpr int kLorentzConeSlack =
static_cast<int>(RemoveFreeVariableMethod::kLorentzConeSlack);
const int method =
options->template Pop<int>("drake::RemoveFreeVariableMethod")
.value_or(kNullspace);
if (!(method >= kTwoSlackVariables && method <= kLorentzConeSlack)) {
throw std::logic_error(fmt::format(
"CsdpSolver: Bad value ({}) for drake::RemoveFreeVariableMethod",
method));
}
*method_out = static_cast<RemoveFreeVariableMethod>(method);
}
// CSDP does not support setting the number of threads so we ignore
// the kMaxNumThreads option.

// Add the specific options next (so they trump the common options).
for (const auto& [key, value] : options.GetOptionsInt(CsdpSolver::id())) {
all_csdp_params += fmt::format("{}={}\n", key, value);
}
for (const auto& [key, value] : options.GetOptionsDouble(CsdpSolver::id())) {
all_csdp_params += fmt::format("{}={}\n", key, value);
}
// TODO(jwnimmer-tri) Throw an error if there were any string options set.
// 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;

// Process the user-supplied options.
options->Respell([](const auto& common, auto* 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);
}
// CSDP does not support setting the number of threads so we ignore the
// kMaxThreads option.
});
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.
Expand Down Expand Up @@ -454,19 +484,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());
Expand All @@ -479,18 +511,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<RemoveFreeVariableMethod>(it_method->second);
} else {
throw std::runtime_error(
"CsdpSolver::sol(), unknown value for "
"drake::RemoveFreeVariableMethod");
}
}
switch (method) {
case RemoveFreeVariableMethod::kNullspace: {
internal::SolveProgramThroughNullspaceApproach(
Expand Down
5 changes: 3 additions & 2 deletions solvers/csdp_solver.h
Original file line number Diff line number Diff line change
Expand Up @@ -91,8 +91,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
Loading