diff --git a/solvers/BUILD.bazel b/solvers/BUILD.bazel index a1fb99cf1a24..9af2376e8886 100644 --- a/solvers/BUILD.bazel +++ b/solvers/BUILD.bazel @@ -37,6 +37,7 @@ drake_cc_package_library( ":linear_system_solver", ":mathematical_program", ":mathematical_program_api", + ":mathematical_program_result", ":mixed_integer_optimization_util", ":mixed_integer_rotation_constraint", ":moby_lcp_solver", @@ -272,6 +273,16 @@ drake_cc_library( ], ) +drake_cc_library( + name = "mathematical_program_result", + srcs = ["mathematical_program_result.cc"], + hdrs = ["mathematical_program_result.h"], + deps = [ + ":mathematical_program_api", + "//systems/framework:value", + ], +) + drake_cc_library( name = "branch_and_bound", srcs = ["branch_and_bound.cc"], @@ -615,11 +626,13 @@ drake_cc_library( deps = select({ "//tools:with_mosek": [ ":mathematical_program_api", + ":mathematical_program_result", "//common:scoped_singleton", "@mosek", ], "//conditions:default": [ ":mathematical_program_api", + ":mathematical_program_result", ], }), ) @@ -1364,6 +1377,15 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "mathematical_program_result_test", + deps = [ + ":mathematical_program_result", + "//common/test_utilities:eigen_matrix_compare", + "//common/test_utilities:expect_throws_message", + ], +) + # The extra_srcs are required here because add_lint_tests() doesn't understand # how to extract labels from select() functions yet. add_lint_tests(cpplint_extra_srcs = [ diff --git a/solvers/mathematical_program_result.cc b/solvers/mathematical_program_result.cc new file mode 100644 index 000000000000..68200c1c14b8 --- /dev/null +++ b/solvers/mathematical_program_result.cc @@ -0,0 +1,37 @@ +#include "drake/solvers/mathematical_program_result.h" + +namespace drake { +namespace solvers { +namespace { +SolverId UnknownId() { + static const never_destroyed result(SolverId({})); + return result.access(); +} +} // namespace + +MathematicalProgramResult::MathematicalProgramResult() + : solution_result_{SolutionResult::kUnknownError}, + x_val_{0}, + optimal_cost_{NAN}, + solver_id_{UnknownId()}, + solver_details_type_{nullptr}, + solver_details_{nullptr} {} + +const systems::AbstractValue& MathematicalProgramResult::get_solver_details() + const { + if (!solver_details_) { + throw std::logic_error("The solver_details has not been set yet."); + } + return *solver_details_; +} + +SolverResult MathematicalProgramResult::ConvertToSolverResult() const { + SolverResult solver_result(solver_id_); + if (x_val_.size() != 0) { + solver_result.set_decision_variable_values(x_val_); + } + solver_result.set_optimal_cost(optimal_cost_); + return solver_result; +} +} // namespace solvers +} // namespace drake diff --git a/solvers/mathematical_program_result.h b/solvers/mathematical_program_result.h new file mode 100644 index 000000000000..ad8aff7096fa --- /dev/null +++ b/solvers/mathematical_program_result.h @@ -0,0 +1,99 @@ +#pragma once + +#include +#include +#include + +// TODO(hongkai.dai): separate SolutionResult and SolverResult from +// mathematical_program_solver_interface, so that MathematicalProgramResult +// won't link to mathematical_program_api. +#include "drake/solvers/mathematical_program_solver_interface.h" +#include "drake/systems/framework/value.h" + +namespace drake { +namespace solvers { +/** + * The result returned by MathematicalProgram::Solve(). It stores the + * solvers::SolutionResult (whether the program is solved to optimality, + * detected infeasibility, etc), the optimal * value for the decision variables, + * the optimal cost, and solver specific details. + */ +class MathematicalProgramResult final { + public: + DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(MathematicalProgramResult) + /** + * Constructs the result. + * @note The solver_details is set to nullptr. + */ + MathematicalProgramResult(); + + /** Gets SolutionResult. */ + SolutionResult get_solution_result() const { return solution_result_; } + + /** Sets SolutionResult. */ + void set_solution_result(SolutionResult solution_result) { + solution_result_ = solution_result; + } + + /** Gets the decision variable values. */ + const Eigen::VectorXd& get_x_val() const { return x_val_; } + + /** Sets the decision variable values. */ + void set_x_val(const Eigen::VectorXd& x_val) { x_val_ = x_val; } + + /** Gets the optimal cost. */ + double get_optimal_cost() const { return optimal_cost_; } + + /** Sets the optimal cost. */ + void set_optimal_cost(double optimal_cost) { optimal_cost_ = optimal_cost; } + + /** Gets the solver ID. */ + const SolverId& get_solver_id() const { return solver_id_; } + + /** Sets the solver ID. */ + void set_solver_id(const SolverId& solver_id) { solver_id_ = solver_id; } + + /** Gets the solver details. Throws an error if the solver_details has not + * been set.*/ + const systems::AbstractValue& get_solver_details() const; + + /** Forces the solver_details to be stored using the given type T. + * If the storage was already typed as T, this is a no-op. + * If there were not any solver_details previously, or if it was of a + * different type, initializes the storage to a default-constructed T. + * Returns a reference to the mutable solver_details object. + * The reference remains valid until the next call to this method, or + * until this MathematicalProgramResult is destroyed. */ + template + T& SetSolverDetailsType() { + // Leave the storage alone if it already has the correct type. + if (solver_details_type_ && (*solver_details_type_ == typeid(T))) { + DRAKE_ASSERT(solver_details_ != nullptr); + DRAKE_ASSERT(solver_details_->MaybeGetValue() != nullptr); + } else { + solver_details_type_ = &typeid(T); + solver_details_ = std::make_unique>(); + } + return solver_details_->GetMutableValue(); + } + + /** + * Convert MathematicalProgramResult to SolverResult. + * @note This function doesn't set optimal_cost_lower_bound. If + * SolverResult.optimal_cost_lower_bound needs to be set (like in + * GurobiSolver), then the user will have to set it after calling + * ConvertToSolverResult. + */ + SolverResult ConvertToSolverResult() const; + + private: + SolutionResult solution_result_{}; + Eigen::VectorXd x_val_; + double optimal_cost_{}; + SolverId solver_id_; + reset_after_move solver_details_type_; + copyable_unique_ptr solver_details_; +}; + +} // namespace solvers +} // namespace drake diff --git a/solvers/mosek_solver.cc b/solvers/mosek_solver.cc index 09b555f9df05..ebe3d038150b 100644 --- a/solvers/mosek_solver.cc +++ b/solvers/mosek_solver.cc @@ -652,7 +652,8 @@ std::shared_ptr MosekSolver::AcquireLicense() { bool MosekSolver::available() const { return true; } -SolutionResult MosekSolver::Solve(MathematicalProgram& prog) const { +MathematicalProgramResult MosekSolver::SolveConstProg( + const MathematicalProgram& prog) const { const int num_vars = prog.num_vars(); MSKtask_t task = nullptr; MSKrescodee rescode; @@ -727,7 +728,8 @@ SolutionResult MosekSolver::Solve(MathematicalProgram& prog) const { } } - SolutionResult result = SolutionResult::kUnknownError; + MathematicalProgramResult result; + result.set_solution_result(SolutionResult::kUnknownError); // Run optimizer. if (rescode == MSK_RES_OK) { // TODO(hongkai.dai@tri.global): add trmcode to the returned struct. @@ -751,11 +753,11 @@ SolutionResult MosekSolver::Solve(MathematicalProgram& prog) const { solution_type = MSK_SOL_ITR; } - SolverResult solver_result(id()); + result.set_solver_id(id()); // TODO(hongkai.dai@tri.global) : Add MOSEK parameters. // Mosek parameter are added by enum, not by string. + MSKsolstae solution_status{MSK_SOL_STA_UNKNOWN}; if (rescode == MSK_RES_OK) { - MSKsolstae solution_status; if (rescode == MSK_RES_OK) { rescode = MSK_getsolsta(task, solution_type, &solution_status); } @@ -765,7 +767,7 @@ SolutionResult MosekSolver::Solve(MathematicalProgram& prog) const { case MSK_SOL_STA_NEAR_OPTIMAL: case MSK_SOL_STA_INTEGER_OPTIMAL: case MSK_SOL_STA_NEAR_INTEGER_OPTIMAL: { - result = SolutionResult::kSolutionFound; + result.set_solution_result(SolutionResult::kSolutionFound); MSKint32t num_mosek_vars; rescode = MSK_getnumvar(task, &num_mosek_vars); DRAKE_ASSERT(rescode == MSK_RES_OK); @@ -781,41 +783,62 @@ SolutionResult MosekSolver::Solve(MathematicalProgram& prog) const { } } if (rescode == MSK_RES_OK) { - solver_result.set_decision_variable_values(sol_vector); + result.set_x_val(sol_vector); } MSKrealt optimal_cost; rescode = MSK_getprimalobj(task, solution_type, &optimal_cost); DRAKE_ASSERT(rescode == MSK_RES_OK); if (rescode == MSK_RES_OK) { - solver_result.set_optimal_cost(optimal_cost); + result.set_optimal_cost(optimal_cost); } break; } case MSK_SOL_STA_DUAL_INFEAS_CER: case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER: - result = SolutionResult::kDualInfeasible; + result.set_solution_result(SolutionResult::kDualInfeasible); break; case MSK_SOL_STA_PRIM_INFEAS_CER: case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER: { - result = SolutionResult::kInfeasibleConstraints; + result.set_solution_result(SolutionResult::kInfeasibleConstraints); break; } default: { - result = SolutionResult::kUnknownError; + result.set_solution_result(SolutionResult::kUnknownError); break; } } } } - prog.SetSolverResult(solver_result); - if (rescode != MSK_RES_OK) { - result = SolutionResult::kUnknownError; + MosekSolverDetails& solver_details = + result.SetSolverDetailsType(); + solver_details.rescode = rescode; + solver_details.solution_status = solution_status; + if (rescode == MSK_RES_OK) { + rescode = MSK_getdouinf(task, MSK_DINF_OPTIMIZER_TIME, + &(solver_details.optimizer_time)); } + // rescode is not used after this. If in the future, the user wants to call + // more MSK functions after this line, then he/she needs to check if rescode + // is OK. But do not modify result.solution_result_ if rescode is not OK after + // this line. + unused(rescode); MSK_deletetask(&task); return result; } +MathematicalProgramResult MosekSolver::Solve( + const MathematicalProgram& prog) const { + return SolveConstProg(prog); +} + +SolutionResult MosekSolver::Solve(MathematicalProgram& prog) const { + const MathematicalProgramResult result = SolveConstProg(prog); + const SolverResult solver_result = result.ConvertToSolverResult(); + prog.SetSolverResult(solver_result); + return result.get_solution_result(); +} + } // namespace solvers } // namespace drake diff --git a/solvers/mosek_solver.h b/solvers/mosek_solver.h index 768e8195eaaf..f1471cafd716 100644 --- a/solvers/mosek_solver.h +++ b/solvers/mosek_solver.h @@ -6,10 +6,24 @@ #include #include "drake/common/drake_copyable.h" +#include "drake/solvers/mathematical_program_result.h" #include "drake/solvers/mathematical_program_solver_interface.h" namespace drake { namespace solvers { +struct MosekSolverDetails { + // The mosek optimization time. Please refer to MSK_DINF_OPTIMIZER_TIME in + // https://docs.mosek.com/8.1/capi/constants.html?highlight=msk_dinf_optimizer_time + double optimizer_time{}; + // The response code returned from mosek solver. Check + // https://docs.mosek.com/8.1/capi/response-codes.html for the meaning on the + // response code. + int rescode{}; + // The solution status after solving the problem. Check + // https://docs.mosek.com/8.1/capi/accessing-solution.html for the meaning on + // the solution status. + int solution_status{}; +}; class MosekSolver : public MathematicalProgramSolverInterface { public: @@ -23,6 +37,10 @@ class MosekSolver : public MathematicalProgramSolverInterface { */ bool available() const override; + MathematicalProgramResult Solve(const MathematicalProgram& prog) const; + + // Todo(hongkai.dai@tri.global): deprecate Solve with a non-const + // MathematicalProgram. SolutionResult Solve(MathematicalProgram& prog) const override; SolverId solver_id() const override; @@ -65,6 +83,10 @@ class MosekSolver : public MathematicalProgramSolverInterface { static std::shared_ptr AcquireLicense(); private: + // TODO(hongkai.dai) remove this function when we remove Solve() with a + // non-cost MathematicalProgram. + MathematicalProgramResult SolveConstProg( + const MathematicalProgram& prog) const; // 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 // before we know that we actually want one). diff --git a/solvers/no_mosek.cc b/solvers/no_mosek.cc index a9689651e2f8..ddebb3076f08 100644 --- a/solvers/no_mosek.cc +++ b/solvers/no_mosek.cc @@ -26,5 +26,18 @@ SolutionResult MosekSolver::Solve(MathematicalProgram&) const { "solver."); } +MathematicalProgramResult MosekSolver::Solve(const MathematicalProgram&) const { + throw runtime_error( + "Mosek is not installed in your build. You'll need to use a different " + "solver."); +} + +MathematicalProgramResult MosekSolver::SolveConstProg( + const MathematicalProgram&) const { + throw runtime_error( + "Mosek is not installed in your build. You'll need to use a different " + "solver."); +} + } // namespace solvers } // namespace drake diff --git a/solvers/test/mathematical_program_result_test.cc b/solvers/test/mathematical_program_result_test.cc new file mode 100644 index 000000000000..6e29701f789a --- /dev/null +++ b/solvers/test/mathematical_program_result_test.cc @@ -0,0 +1,79 @@ +#include "drake/solvers/mathematical_program_result.h" + +#include + +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/common/test_utilities/expect_throws_message.h" + +namespace drake { +namespace solvers { +namespace { +GTEST_TEST(MathematicalProgramResultTest, DefaultConstructor) { + MathematicalProgramResult result; + EXPECT_EQ(result.get_solution_result(), SolutionResult::kUnknownError); + EXPECT_EQ(result.get_x_val().size(), 0); + EXPECT_TRUE(std::isnan(result.get_optimal_cost())); + DRAKE_EXPECT_THROWS_MESSAGE(result.get_solver_details(), std::logic_error, + "The solver_details has not been set yet."); +} + +GTEST_TEST(MathematicalProgramResultTest, Setters) { + MathematicalProgramResult result; + result.set_solution_result(SolutionResult::kSolutionFound); + const Eigen::Vector2d x_val(0, 0); + result.set_x_val(x_val); + const double cost = 1; + result.set_optimal_cost(cost); + result.set_solver_id(SolverId("foo")); + EXPECT_EQ(result.get_solution_result(), SolutionResult::kSolutionFound); + EXPECT_TRUE(CompareMatrices(result.get_x_val(), x_val)); + EXPECT_EQ(result.get_optimal_cost(), cost); + EXPECT_EQ(result.get_solver_id().name(), "foo"); +} + +struct DummySolverDetails { + int data{0}; +}; + +GTEST_TEST(MathematicalProgramResultTest, SetSolverDetails) { + MathematicalProgramResult result; + const int data = 1; + DummySolverDetails& dummy_solver_details = + result.SetSolverDetailsType(); + dummy_solver_details.data = data; + EXPECT_EQ(result.get_solver_details().GetValue().data, + data); + // Now we test if we call SetSolverDetailsType again, it doesn't allocate new + // memory. + // First we check the address of (result.get_solver_details()) is unchanged. + const systems::AbstractValue* details = &(result.get_solver_details()); + dummy_solver_details = result.SetSolverDetailsType(); + EXPECT_EQ(details, &(result.get_solver_details())); + // Now we check that the value in the solver details are unchanged, note that + // the default value for data is 0, as in the constructor of + // DummySolverDetails, so if the constructor were called, + // dummy_solver_details.data won't be equal to 1. + dummy_solver_details = result.SetSolverDetailsType(); + EXPECT_EQ(result.get_solver_details().GetValue().data, + data); +} + +GTEST_TEST(MathematicalProgramResultTest, ConvertToSolverResult) { + MathematicalProgramResult result; + result.set_solver_id(SolverId("foo")); + result.set_optimal_cost(2); + // The x_val is not set. So solver_result.decision_variable_values should be + // empty. + SolverResult solver_result = result.ConvertToSolverResult(); + EXPECT_FALSE(solver_result.decision_variable_values()); + // Now set x_val. + result.set_x_val(Eigen::Vector2d::Ones()); + solver_result = result.ConvertToSolverResult(); + EXPECT_EQ(result.get_solver_id(), solver_result.solver_id()); + EXPECT_TRUE(CompareMatrices( + result.get_x_val(), solver_result.decision_variable_values().value())); + EXPECT_EQ(result.get_optimal_cost(), solver_result.optimal_cost()); +} +} // namespace +} // namespace solvers +} // namespace drake diff --git a/solvers/test/mosek_solver_test.cc b/solvers/test/mosek_solver_test.cc index 6ef03c0d9b08..55fdcb65241a 100644 --- a/solvers/test/mosek_solver_test.cc +++ b/solvers/test/mosek_solver_test.cc @@ -27,9 +27,12 @@ INSTANTIATE_TEST_CASE_P( TEST_F(UnboundedLinearProgramTest0, Test) { MosekSolver solver; if (solver.available()) { - const SolutionResult result = solver.Solve(*prog_); + const MathematicalProgram& const_prog = *prog_; + const MathematicalProgramResult result = solver.Solve(const_prog); // Mosek can only detect dual infeasibility, not primal unboundedness. - EXPECT_EQ(result, SolutionResult::kDualInfeasible); + EXPECT_EQ(result.get_solution_result(), SolutionResult::kDualInfeasible); + EXPECT_EQ( + result.get_solver_details().GetValue().rescode, 0); } }