mosek can set initial guess for integer/binary variables. (RobotLocom…

…otion#10426)

mosek can set initial guess for integer/binary variables.

solvers/BUILD.bazel

@@ -1203,6 +1203,7 @@ drake_cc_googletest(
         ":linear_program_examples",
         ":mathematical_program",
         ":mathematical_program_test_util",
+        ":mixed_integer_optimization_util",
         ":quadratic_program_examples",
         ":second_order_cone_program_examples",
         ":semidefinite_program_examples",

solvers/mosek_solver.cc

@@ -665,10 +665,6 @@ void MosekSolver::Solve(const MathematicalProgram& prog,
   SolverOptions merged_solver_options =
       solver_options.value_or(SolverOptions());
   merged_solver_options.Merge(prog.solver_options());
-  // TODO(hongkai.dai): support setting initial guess.
-  if (initial_guess.has_value()) {
-    throw std::runtime_error("Not implemented yet.");
-  }
   const int num_vars = prog.num_vars();
   MSKtask_t task = nullptr;
   MSKrescodee rescode;
@@ -767,6 +763,38 @@ void MosekSolver::Solve(const MathematicalProgram& prog,
     }
   }
 
+  if (with_integer_or_binary_variable && initial_guess.has_value()) {
+    // Set the initial guess for the integer/binary variables.
+    DRAKE_ASSERT(initial_guess->size() == prog.num_vars());
+    MSKint32t num_mosek_vars{0};
+    if (rescode == MSK_RES_OK) {
+      // num_mosek_vars is guaranteed to be no less than prog.num_vars(), as we
+      // can add slack variables when we construct Mosek constraints. For
+      // example, when we call AddSecondOrderConeConstraints().
+      rescode = MSK_getnumvar(task, &num_mosek_vars);
+      DRAKE_DEMAND(num_mosek_vars >= prog.num_vars());
+    }
+    if (rescode == MSK_RES_OK) {
+      rescode = MSK_putintparam(task, MSK_IPAR_MIO_CONSTRUCT_SOL, MSK_ON);
+    }
+    int var_count = 0;
+    for (int i = 0; i < num_mosek_vars; ++i) {
+      if (!is_new_variable[i]) {
+        const auto var_type = prog.decision_variable(var_count).get_type();
+        if (var_type == MathematicalProgram::VarType::INTEGER ||
+            var_type == MathematicalProgram::VarType::BINARY) {
+          if (rescode == MSK_RES_OK) {
+            const MSKrealt initial_guess_i = initial_guess.value()(var_count);
+            rescode =
+                MSK_putxxslice(task, MSK_SOL_ITG, i, i + 1, &initial_guess_i);
+          }
+        }
+        var_count++;
+      }
+    }
+    DRAKE_DEMAND(var_count == prog.num_vars());
+  }
+
   result->set_solution_result(SolutionResult::kUnknownError);
   // Run optimizer.
   if (rescode == MSK_RES_OK) {
@@ -804,7 +832,8 @@ void MosekSolver::Solve(const MathematicalProgram& prog,
         case MSK_SOL_STA_OPTIMAL:
         case MSK_SOL_STA_NEAR_OPTIMAL:
         case MSK_SOL_STA_INTEGER_OPTIMAL:
-        case MSK_SOL_STA_NEAR_INTEGER_OPTIMAL: {
+        case MSK_SOL_STA_NEAR_INTEGER_OPTIMAL:
+        case MSK_SOL_STA_PRIM_FEAS: {
           result->set_solution_result(SolutionResult::kSolutionFound);
           MSKint32t num_mosek_vars;
           rescode = MSK_getnumvar(task, &num_mosek_vars);

solvers/mosek_solver.h

@@ -40,6 +40,17 @@ class MosekSolver : public MathematicalProgramSolverInterface {
 
   static bool is_available();
 
+  /**
+   * @param initial_guess The initial guess of all decision variables.
+   * @note Mosek only cares about the initial guess of integer variables. The
+   * initial guess of continuous variables are not passed to MOSEK. If all the
+   * integer variables are set to some integer values, then MOSEK will be forced
+   * to compute the remaining continuous variable values as the initial guess.
+   * (Mosek might change the values of the integer/binary variables in the
+   * subsequent iterations.) If the specified integer solution is infeasible or
+   * incomplete, MOSEK will simply ignore it. For more details, check
+   * https://docs.mosek.com/8.1/capi/tutorial-mio-shared.html?highlight=initial
+   */
   void Solve(const MathematicalProgram& prog,
              const optional<Eigen::VectorXd>& initial_guess,
              const optional<SolverOptions>& solver_options,

solvers/test/mosek_solver_test.cc

@@ -5,6 +5,7 @@
 
 #include "drake/common/temp_directory.h"
 #include "drake/solvers/mathematical_program.h"
+#include "drake/solvers/mixed_integer_optimization_util.h"
 #include "drake/solvers/test/linear_program_examples.h"
 #include "drake/solvers/test/quadratic_program_examples.h"
 #include "drake/solvers/test/second_order_cone_program_examples.h"
@@ -196,6 +197,50 @@ GTEST_TEST(MosekSolver, SolverOptionsErrorTest) {
 
   EXPECT_EQ(result.get_solution_result(), SolutionResult::kUnknownError);
 }
+
+GTEST_TEST(MosekSolver, TestInitialGuess) {
+  // Mosek allows to set initial guess for integer/binary variables.
+  // Solve the following mixed-integer problem
+  // Find a point C on one of the line segment A1A2, A2A3, A3A4, A4A1 such that
+  // the distance from the point C to the point D = (0, 0) is minimized, where
+  // A1 = (-1, 0), A2 = (0, 1), A3 = (2, 0), A4 = (1, -0.5)
+  MathematicalProgram prog;
+  auto lambda = prog.NewContinuousVariables<5>();
+  auto y = prog.NewBinaryVariables<4>();
+  AddSos2Constraint(&prog, lambda.cast<symbolic::Expression>(),
+                    y.cast<symbolic::Expression>());
+  Eigen::Matrix<double, 2, 5> pts_A;
+  pts_A.col(0) << -1, 0;
+  pts_A.col(1) << 0, 1;
+  pts_A.col(2) << 2, 0;
+  pts_A.col(3) << 1, -0.5;
+  pts_A.col(4) = pts_A.col(0);
+  // point C in the documentation above.
+  auto pt_C = prog.NewContinuousVariables<2>();
+  prog.AddLinearEqualityConstraint(pt_C == pts_A * lambda);
+  prog.AddQuadraticCost(pt_C(0) * pt_C(0) + pt_C(1) * pt_C(1));
+
+  MosekSolver solver;
+  SolverOptions solver_options;
+  // Allow only one solution (the one corresponding to the initial guess on the
+  // integer values.)
+  solver_options.SetOption(solver.id(), "MSK_IPAR_MIO_MAX_NUM_SOLUTIONS", 1);
+  // By setting y = (0, 1, 0, 0), point C is on the line segment A2A3. The
+  // minimal squared distance is 0.8;
+  prog.SetInitialGuess(y, Eigen::Vector4d(0, 1, 0, 0));
+  MathematicalProgramResult result;
+  solver.Solve(prog, prog.initial_guess(), solver_options, &result);
+  const double tol = 1E-8;
+  EXPECT_EQ(result.get_solution_result(), SolutionResult::kSolutionFound);
+  EXPECT_NEAR(result.get_optimal_cost(), 0.8, tol);
+
+  // By setting y = (0, 0, 0, 1), point C is on the line segment A4A1. The
+  // minimal squared distance is  1.0 / 17
+  prog.SetInitialGuess(y, Eigen::Vector4d(0, 0, 0, 1));
+  solver.Solve(prog, prog.initial_guess(), solver_options, &result);
+  EXPECT_EQ(result.get_solution_result(), SolutionResult::kSolutionFound);
+  EXPECT_NEAR(result.get_optimal_cost(), 1.0 / 17, tol);
+}
 }  // namespace test
 }  // namespace solvers
 }  // namespace drake