Skip to content

Commit

Permalink
solvers: Port most back-ends to use SolverBase (RobotLocomotion#10672)
Browse files Browse the repository at this point in the history
  • Loading branch information
jwnimmer-tri authored Feb 13, 2019
1 parent 4b1009b commit 9549a01
Show file tree
Hide file tree
Showing 34 changed files with 406 additions and 703 deletions.
16 changes: 16 additions & 0 deletions solvers/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -614,6 +614,7 @@ drake_cc_library(
hdrs = ["linear_system_solver.h"],
deps = [
":mathematical_program_lite",
":solver_base",
"//common:essential",
],
)
Expand All @@ -624,6 +625,7 @@ drake_cc_library(
hdrs = ["unrevised_lemke_solver.h"],
deps = [
":mathematical_program_lite",
":solver_base",
"//common:default_scalars",
"//common:essential",
],
Expand All @@ -635,6 +637,7 @@ drake_cc_library(
hdrs = ["moby_lcp_solver.h"],
deps = [
":mathematical_program_lite",
":solver_base",
"//common:essential",
],
)
Expand All @@ -648,6 +651,7 @@ drake_cc_library(
hdrs = ["dreal_solver.h"],
deps = [
":mathematical_program_lite",
":solver_base",
"//common:essential",
"@dreal",
],
Expand Down Expand Up @@ -753,24 +757,28 @@ drake_cc_library(
# This is Drake's default flavor; currently f2c.
"//tools:with_snopt": [
":mathematical_program_lite",
":solver_base",
"//math:autodiff",
"@snopt//:snopt_c",
],
# This is always the Fortran flavor.
"//tools:with_snopt_fortran": [
":mathematical_program_lite",
":solver_base",
"//math:autodiff",
"@snopt//:snopt_cwrap",
],
# This is always the f2c flavor.
"//tools:with_snopt_f2c": [
":mathematical_program_lite",
":solver_base",
"//math:autodiff",
"@snopt//:snopt_c",
],
# This is when SNOPT is disabled / unavailable.
"//conditions:default": [
":mathematical_program_lite",
":solver_base",
],
}),
)
Expand All @@ -792,11 +800,13 @@ drake_cc_library(
"//conditions:default": [
"@ipopt",
":mathematical_program_lite",
":solver_base",
"//common:unused",
"//math:autodiff",
],
"//tools:no_ipopt": [
":mathematical_program_lite",
":solver_base",
],
}),
)
Expand All @@ -818,10 +828,12 @@ drake_cc_library(
"//conditions:default": [
":mathematical_program_lite",
"//math:autodiff",
":solver_base",
"@nlopt",
],
"//tools:no_nlopt": [
":mathematical_program_lite",
":solver_base",
],
}),
)
Expand All @@ -842,11 +854,13 @@ drake_cc_library(
deps = select({
"//conditions:default": [
":mathematical_program_lite",
":solver_base",
"//math:eigen_sparse_triplet",
"@osqp",
],
"//tools:no_osqp": [
":mathematical_program_lite",
":solver_base",
],
}),
)
Expand All @@ -867,12 +881,14 @@ drake_cc_library(
deps = select({
"//conditions:default": [
":mathematical_program_lite",
":solver_base",
"//common:essential",
"//math:eigen_sparse_triplet",
"@scs//:scsdir",
],
"//tools:no_scs": [
":mathematical_program_lite",
":solver_base",
],
}),
)
Expand Down
31 changes: 7 additions & 24 deletions solvers/dreal_solver.cc
Original file line number Diff line number Diff line change
Expand Up @@ -523,17 +523,12 @@ void ExtractQuadraticCosts(const MathematicalProgram& prog,

} // namespace

void DrealSolver::Solve(const MathematicalProgram& prog,
const optional<Eigen::VectorXd>& initial_guess,
const optional<SolverOptions>& solver_options,
MathematicalProgramResult* result) const {
*result = {};
result->set_decision_variable_index(prog.decision_variable_index());
void DrealSolver::DoSolve(
const MathematicalProgram& prog,
const Eigen::VectorXd& initial_guess,
const SolverOptions& merged_options,
MathematicalProgramResult* result) const {
unused(initial_guess);
if (!AreProgramAttributesSatisfied(prog)) {
throw std::invalid_argument(
"dReal's capability doesn't satisfy the requirement of the program.");
}

// 1. Extracts the constraints from @p prog and constructs an equivalent
// symbolic formula.
Expand All @@ -556,15 +551,11 @@ void DrealSolver::Solve(const MathematicalProgram& prog,
// TODO(soonho): Support other dReal options. For now, we only support
// "--preicision" and "--local-optimization".

SolverOptions merged_solver_options =
solver_options.has_value() ? solver_options.value() : SolverOptions();
merged_solver_options.Merge(prog.solver_options());

const double precision{GetOptionWithDefaultValue(
merged_solver_options, "precision", 0.001 /* default */)};
merged_options, "precision", 0.001 /* default */)};
const LocalOptimization local_optimization{
GetOptionWithDefaultValue<int>(
merged_solver_options, "use_local_optimization", 1 /* default */) > 0
merged_options, "use_local_optimization", 1 /* default */) > 0
? LocalOptimization::kUse
: LocalOptimization::kNotUse};
optional<IntervalBox> dreal_result;
Expand All @@ -581,7 +572,6 @@ void DrealSolver::Solve(const MathematicalProgram& prog,

// 4. Sets up SolverResult and SolutionResult.
result->set_solution_result(SolutionResult::kUnknownError);
result->set_solver_id(id());
if (dreal_result) {
// 4.1. delta-SAT case.
const int num_vars{prog.num_vars()};
Expand All @@ -601,12 +591,5 @@ void DrealSolver::Solve(const MathematicalProgram& prog,
}
}

SolutionResult DrealSolver::Solve(MathematicalProgram& prog) const {
MathematicalProgramResult result;
Solve(prog, {}, {}, &result);
const SolverResult solver_result = result.ConvertToSolverResult();
prog.SetSolverResult(solver_result);
return result.get_solution_result();
}
} // namespace solvers
} // namespace drake
45 changes: 18 additions & 27 deletions solvers/dreal_solver.h
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,12 @@
#include "drake/common/drake_optional.h"
#include "drake/common/hash.h"
#include "drake/common/symbolic.h"
#include "drake/solvers/solver_interface.h"
#include "drake/solvers/solver_base.h"

namespace drake {
namespace solvers {

class DrealSolver : public SolverInterface {
class DrealSolver final : public SolverBase {
public:
using Interval = dreal::Box::Interval;
using IntervalBox = std::unordered_map<symbolic::Variable, Interval>;
Expand All @@ -27,31 +27,8 @@ class DrealSolver : public SolverInterface {

DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DrealSolver)

DrealSolver() = default;
~DrealSolver() override = default;

// This solver is implemented in various pieces depending on if
// Dreal was available during compilation.
bool available() const override { return is_available(); };

static bool is_available();

SolutionResult Solve(MathematicalProgram& prog) const override;

void Solve(const MathematicalProgram& prog,
const optional<Eigen::VectorXd>& initial_guess,
const optional<SolverOptions>& solver_options,
MathematicalProgramResult* result) const override;

SolverId solver_id() const override;

/// @return same as SolverInterface::solver_id()
static SolverId id();

bool AreProgramAttributesSatisfied(
const MathematicalProgram& prog) const override;

static bool ProgramAttributesSatisfied(const MathematicalProgram& prog);
DrealSolver();
~DrealSolver() final;

/// Checks the satisfiability of a given formula @p f with a given precision
/// @p delta.
Expand All @@ -75,6 +52,20 @@ class DrealSolver : public SolverInterface {
const symbolic::Formula& constraint,
double delta,
LocalOptimization local_optimization);

/// @name Static versions of the instance methods with similar names.
//@{
static SolverId id();
static bool is_available();
static bool ProgramAttributesSatisfied(const MathematicalProgram&);
//@}

// A using-declaration adds these methods into our class's Doxygen.
using SolverBase::Solve;

private:
void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&,
const SolverOptions&, MathematicalProgramResult*) const final;
};

} // namespace solvers
Expand Down
12 changes: 4 additions & 8 deletions solvers/dreal_solver_common.cc
Original file line number Diff line number Diff line change
Expand Up @@ -11,20 +11,16 @@
namespace drake {
namespace solvers {

SolverId DrealSolver::solver_id() const {
return id();
}
DrealSolver::DrealSolver()
: SolverBase(&id, &is_available, &ProgramAttributesSatisfied) {}

DrealSolver::~DrealSolver() = default;

SolverId DrealSolver::id() {
static const never_destroyed<SolverId> singleton{"dReal"};
return singleton.access();
}

bool DrealSolver::AreProgramAttributesSatisfied(
const MathematicalProgram& prog) const {
return DrealSolver::ProgramAttributesSatisfied(prog);
}

bool DrealSolver::ProgramAttributesSatisfied(const MathematicalProgram& prog) {
static const never_destroyed<ProgramAttributes> solver_capabilities(
std::initializer_list<ProgramAttribute>{
Expand Down
11 changes: 6 additions & 5 deletions solvers/equality_constrained_qp_solver.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,14 +29,15 @@ class EqualityConstrainedQPSolver final : public SolverBase {
/// @returns string key for SolverOptions to set the feasibility tolerance.
static std::string FeasibilityTolOptionName();

/// A static flavor of the base class method solver_id().
/// @name Static versions of the instance methods with similar names.
//@{
static SolverId id();

/// A static flavor of the base class method available().
static bool is_available();

/// A static flavor of the base class method AreProgramAttributesSatisfied().
static bool ProgramAttributesSatisfied(const MathematicalProgram&);
//@}

// A using-declaration adds these methods into our class's Doxygen.
using SolverBase::Solve;

private:
void DoSolve(const MathematicalProgram&, const Eigen::VectorXd&,
Expand Down
2 changes: 1 addition & 1 deletion solvers/gurobi_solver.h
Original file line number Diff line number Diff line change
Expand Up @@ -141,7 +141,7 @@ class GurobiSolver final : public SolverBase {
*/
static std::shared_ptr<License> AcquireLicense();

/// @name Static versions of the instance methods of the same name.
/// @name Static versions of the instance methods with similar names.
//@{
static SolverId id();
static bool is_available();
Expand Down
33 changes: 7 additions & 26 deletions solvers/ipopt_solver.cc
Original file line number Diff line number Diff line change
Expand Up @@ -396,8 +396,6 @@ class IpoptSolver_NLP : public Ipopt::TNLP {
solver_details.g = Eigen::Map<const Eigen::VectorXd>(g, m);
solver_details.lambda = Eigen::Map<const Eigen::VectorXd>(lambda, m);

result_->set_solver_id(IpoptSolver::id());

result_->set_solution_result(SolutionResult::kUnknownError);
switch (status) {
case Ipopt::SUCCESS: {
Expand Down Expand Up @@ -639,25 +637,16 @@ const char* IpoptSolverDetails::ConvertStatusToString() const {

bool IpoptSolver::is_available() { return true; }

void IpoptSolver::Solve(const MathematicalProgram& prog,
const optional<Eigen::VectorXd>& initial_guess,
const optional<SolverOptions>& solver_options,
MathematicalProgramResult* result) const {
*result = {};
result->set_decision_variable_index(prog.decision_variable_index());
if (!AreProgramAttributesSatisfied(prog)) {
throw std::invalid_argument(
"Ipopt doesn't satisfy the capabilities required by the program");
}

// TODO(hongkai.dai): do not retrieve initial guess from prog.
const Eigen::VectorXd x_init =
initial_guess.has_value() ? initial_guess.value() : prog.initial_guess();
void IpoptSolver::DoSolve(
const MathematicalProgram& prog,
const Eigen::VectorXd& initial_guess,
const SolverOptions& merged_options,
MathematicalProgramResult* result) const {

Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
app->RethrowNonIpoptException(true);

SetIpoptOptions(prog, solver_options, &(*app));
SetIpoptOptions(prog, merged_options, &(*app));

Ipopt::ApplicationReturnStatus status = app->Initialize();
if (status != Ipopt::Solve_Succeeded) {
Expand All @@ -666,17 +655,9 @@ void IpoptSolver::Solve(const MathematicalProgram& prog,
}

Ipopt::SmartPtr<IpoptSolver_NLP> nlp =
new IpoptSolver_NLP(prog, x_init, result);
new IpoptSolver_NLP(prog, initial_guess, result);
status = app->OptimizeTNLP(nlp);
}

SolutionResult IpoptSolver::Solve(MathematicalProgram& prog) const {
MathematicalProgramResult result;
Solve(prog, {}, {}, &result);
const SolverResult solver_result = result.ConvertToSolverResult();
prog.SetSolverResult(solver_result);
return result.get_solution_result();
}

} // namespace solvers
} // namespace drake
Loading

0 comments on commit 9549a01

Please sign in to comment.