Mathematical program result (RobotLocomotion#9786)

Add MathematicalProgramResult class. In MosekSolver we add a Solve function that accepts a const MathematicalProgram and returns a MathematicalProgramResult.

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 = [

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<SolverId> 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

solvers/mathematical_program_result.h

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <memory>
+#include <typeinfo>
+#include <utility>
+
+// 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 <typename T>
+  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<T>() != nullptr);
+    } else {
+      solver_details_type_ = &typeid(T);
+      solver_details_ = std::make_unique<systems::Value<T>>();
+    }
+    return solver_details_->GetMutableValue<T>();
+  }
+
+  /**
+   * 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<const std::type_info*> solver_details_type_;
+  copyable_unique_ptr<systems::AbstractValue> solver_details_;
+};
+
+}  // namespace solvers
+}  // namespace drake

solvers/mosek_solver.cc

@@ -652,7 +652,8 @@ std::shared_ptr<MosekSolver::License> 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([email protected]): 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([email protected]) : 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<MosekSolverDetails>();
+  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

solvers/mosek_solver.h

@@ -6,10 +6,24 @@
 #include <Eigen/Core>
 
 #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([email protected]): 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<License> 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).

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