Retrieve dual solution for conic constraint in Mosek.

solvers/mathematical_program_result.h

@@ -248,11 +248,13 @@ class MathematicalProgramResult final {
   /**
    * Gets the dual solution associated with a constraint.
    *
-   * We interpret the dual variable value as the "shadow price" of the original
-   * problem. Namely if we change the constraint bound by one unit (each unit is
-   * infinitesimally small), the change of the optimal cost is the value of the
-   * dual solution times the unit. Mathematically dual_solution = ∂optimal_cost
-   * / ∂bound.
+   * For constraints in the form lower <= f(x) <= upper (including linear
+   * inequality, linear equality, bounding box constraints, and general
+   * nonlinear constraints), we interpret the dual variable value as the "shadow
+   * price" of the original problem. Namely if we change the constraint bound by
+   * one unit (each unit is infinitesimally small), the change of the optimal
+   * cost is the value of the dual solution times the unit. Mathematically
+   * dual_solution = ∂optimal_cost / ∂bound.
    *
    * For a linear equality constraint Ax = b where b ∈ ℝⁿ, the vector of dual
    * variables has n rows, and dual_solution(i) is the value of the dual
@@ -273,8 +275,9 @@ class MathematicalProgramResult final {
    * For a bounding box constraint lower <= x <= upper, the interpretation of
    * the dual solution is the same as the linear inequality constraint.
    *
-   * For a Lorentz cone or rotated Lorentz cone constraint that Ax + b is in the
-   * cone, depending on the solver, the dual solution has different meanings:
+   * For a Lorentz cone or rotated Lorentz cone constraint that Ax + b is in
+   * the cone, depending on the solver, the dual solution has different
+   * meanings:
    * 1. If the solver is Gurobi, then the user can only obtain the dual solution
    *    by explicitly setting the options for computing dual solution.
    *    @code
@@ -297,6 +300,20 @@ class MathematicalProgramResult final {
    * 2. For nonlinear solvers like IPOPT, the dual solution for Lorentz cone
    *    constraint (with EvalType::kConvex) is the shadow price for
    *    z₀ - sqrt(z₁² + ... +zₙ²) ≥ 0, where z = Ax+b.
+   * 3. For other convex conic solver such as SCS, Mosek, CSDP, etc, the dual
+   *    solution to the (rotated) Lorentz cone constraint doesn't have the
+   *    "shadow price" interpretation, but should lie in the dual cone, and
+   *    satisfy the KKT condition. For more information, refer to
+   *    https://docs.mosek.com/9.2/capi/prob-def-conic.html#duality-for-conic-optimization
+   *    as an explanation.
+   *
+   * The interpretation for the dual variable to conic constraint x ∈ K can be
+   * different. Here K is a convex cone, including exponential cone, power
+   * cone, PSD cone, etc. When the problem is solved by a convex solver (like
+   * SCS, Mosek, CSDP, etc), often it has a dual variable z ∈ K*, where K* is
+   * the dual cone. Here the dual variable DOESN'T have the interpretation of
+   * "shadow price", but should satisfy the KKT condition, while the dual
+   * variable stays inside the dual cone.
    */
   template <typename C>
   Eigen::VectorXd GetDualSolution(const Binding<C>& constraint) const {

solvers/mosek_solver.cc

@@ -659,13 +659,14 @@ MSKrescodee AddBoundingBoxConstraints(
 template <typename C>
 MSKrescodee AddConeConstraints(
     const MathematicalProgram& prog,
-    const std::vector<Binding<C>>& second_order_cone_constraints,
+    const std::vector<Binding<C>>& cone_constraints,
     const std::unordered_map<int, MatrixVariableEntry>&
         decision_variable_index_to_mosek_matrix_variable,
     const std::unordered_map<int, int>&
         decision_variable_index_to_mosek_nonmatrix_variable,
     std::unordered_map<MatrixVariableEntry::Id, MSKint64t>*
         matrix_variable_entry_to_selection_matrix_id,
+    std::unordered_map<Binding<C>, ConstraintDualIndices>* dual_indices,
     MSKtask_t* task) {
   static_assert(std::is_same<C, LorentzConeConstraint>::value ||
                     std::is_same<C, RotatedLorentzConeConstraint>::value ||
@@ -675,7 +676,7 @@ MSKrescodee AddConeConstraints(
   const bool is_rotated_cone =
       std::is_same<C, RotatedLorentzConeConstraint>::value;
   MSKrescodee rescode = MSK_RES_OK;
-  for (auto const& binding : second_order_cone_constraints) {
+  for (auto const& binding : cone_constraints) {
     const auto& A = binding.evaluator()->A();
     const auto& b = binding.evaluator()->b();
     const int num_z = A.rows();
@@ -707,6 +708,14 @@ MSKrescodee AddConeConstraints(
     } else {
       DRAKE_UNREACHABLE();
     }
+    // The dual cone has the same size as the nonlinear cones (Lorentz cone,
+    // rotated Lorentz cone, exponential cone, etc).
+    ConstraintDualIndices duals(num_z);
+    for (int i = 0; i < num_z; ++i) {
+      duals[i].type = DualVarType::kNonlinearConic;
+      duals[i].index = num_mosek_vars + i;
+    }
+    dual_indices->emplace(binding, duals);
     rescode =
         MSK_appendcone(*task, cone_type, 0.0, num_z, new_var_indices.data());
     if (rescode != MSK_RES_OK) {
@@ -1484,6 +1493,22 @@ void SetLinearConstraintDualSolution(
   }
 }
 
+template <typename C>
+void SetNonlinearConstraintDualSolution(
+    const std::vector<Binding<C>>& bindings, const std::vector<MSKrealt>& snx,
+    const std::unordered_map<Binding<C>, ConstraintDualIndices>& dual_indices,
+    MathematicalProgramResult* result) {
+  for (const auto& binding : bindings) {
+    const ConstraintDualIndices duals = dual_indices.at(binding);
+    Eigen::VectorXd dual_sol = Eigen::VectorXd::Zero(duals.size());
+    for (int i = 0; i < dual_sol.rows(); ++i) {
+      DRAKE_DEMAND(duals[i].type == DualVarType::kNonlinearConic);
+      dual_sol[i] = snx[duals[i].index];
+    }
+    result->set_dual_solution(binding, dual_sol);
+  }
+}
+
 MSKrescodee SetDualSolution(
     const MSKtask_t& task, MSKsoltypee which_sol,
     const MathematicalProgram& prog,
@@ -1495,6 +1520,13 @@ MSKrescodee SetDualSolution(
         linear_con_dual_indices,
     const std::unordered_map<Binding<LinearEqualityConstraint>,
                              ConstraintDualIndices>& lin_eq_con_dual_indices,
+    const std::unordered_map<Binding<LorentzConeConstraint>,
+                             ConstraintDualIndices>& lorentz_cone_dual_indices,
+    const std::unordered_map<Binding<RotatedLorentzConeConstraint>,
+                             ConstraintDualIndices>&
+        rotated_lorentz_cone_dual_indices,
+    const std::unordered_map<Binding<ExponentialConeConstraint>,
+                             ConstraintDualIndices>& exp_cone_dual_indices,
     MathematicalProgramResult* result) {
   // TODO(hongkai.dai): support other types of constraints, like linear
   // constraint, second order cone constraint, etc.
@@ -1550,6 +1582,23 @@ MSKrescodee SetDualSolution(
     // Set the duals for the linear equality constraint.
     SetLinearConstraintDualSolution(prog.linear_equality_constraints(), slc,
                                     suc, lin_eq_con_dual_indices, result);
+    // Set the duals for the nonlinear conic constraint.
+    // Mosek provides the dual solution for nonlinear conic constraints only if
+    // the program is solved through interior point approach.
+    if (which_sol == MSK_SOL_ITR) {
+      std::vector<MSKrealt> snx(num_mosek_vars);
+      rescode = MSK_getsnx(task, which_sol, snx.data());
+      if (rescode != MSK_RES_OK) {
+        return rescode;
+      }
+      SetNonlinearConstraintDualSolution(prog.lorentz_cone_constraints(), snx,
+                                         lorentz_cone_dual_indices, result);
+      SetNonlinearConstraintDualSolution(
+          prog.rotated_lorentz_cone_constraints(), snx,
+          rotated_lorentz_cone_dual_indices, result);
+      SetNonlinearConstraintDualSolution(prog.exponential_cone_constraints(),
+                                         snx, exp_cone_dual_indices, result);
+    }
   }
   return rescode;
 }
@@ -1760,21 +1809,28 @@ void MosekSolver::DoSolve(const MathematicalProgram& prog,
   }
 
   // Add Lorentz cone constraints.
+  std::unordered_map<Binding<LorentzConeConstraint>, ConstraintDualIndices>
+      lorentz_cone_dual_indices;
   if (rescode == MSK_RES_OK) {
-    rescode = AddConeConstraints(
-        prog, prog.lorentz_cone_constraints(),
-        decision_variable_index_to_mosek_matrix_variable,
-        decision_variable_index_to_mosek_nonmatrix_variable,
-        &matrix_variable_entry_to_selection_matrix_id, &task);
+    rescode =
+        AddConeConstraints(prog, prog.lorentz_cone_constraints(),
+                           decision_variable_index_to_mosek_matrix_variable,
+                           decision_variable_index_to_mosek_nonmatrix_variable,
+                           &matrix_variable_entry_to_selection_matrix_id,
+                           &lorentz_cone_dual_indices, &task);
   }
 
   // Add rotated Lorentz cone constraints.
+  std::unordered_map<Binding<RotatedLorentzConeConstraint>,
+                     ConstraintDualIndices>
+      rotated_lorentz_cone_dual_indices;
   if (rescode == MSK_RES_OK) {
-    rescode = AddConeConstraints(
-        prog, prog.rotated_lorentz_cone_constraints(),
-        decision_variable_index_to_mosek_matrix_variable,
-        decision_variable_index_to_mosek_nonmatrix_variable,
-        &matrix_variable_entry_to_selection_matrix_id, &task);
+    rescode =
+        AddConeConstraints(prog, prog.rotated_lorentz_cone_constraints(),
+                           decision_variable_index_to_mosek_matrix_variable,
+                           decision_variable_index_to_mosek_nonmatrix_variable,
+                           &matrix_variable_entry_to_selection_matrix_id,
+                           &rotated_lorentz_cone_dual_indices, &task);
   }
 
   // Add linear matrix inequality constraints.
@@ -1786,12 +1842,15 @@ void MosekSolver::DoSolve(const MathematicalProgram& prog,
   }
 
   // Add exponential cone constraints.
+  std::unordered_map<Binding<ExponentialConeConstraint>, ConstraintDualIndices>
+      exp_cone_dual_indices;
   if (rescode == MSK_RES_OK) {
-    rescode = AddConeConstraints(
-        prog, prog.exponential_cone_constraints(),
-        decision_variable_index_to_mosek_matrix_variable,
-        decision_variable_index_to_mosek_nonmatrix_variable,
-        &matrix_variable_entry_to_selection_matrix_id, &task);
+    rescode =
+        AddConeConstraints(prog, prog.exponential_cone_constraints(),
+                           decision_variable_index_to_mosek_matrix_variable,
+                           decision_variable_index_to_mosek_nonmatrix_variable,
+                           &matrix_variable_entry_to_selection_matrix_id,
+                           &exp_cone_dual_indices, &task);
   }
 
   // log file.
@@ -1911,7 +1970,9 @@ void MosekSolver::DoSolve(const MathematicalProgram& prog,
           }
           rescode = SetDualSolution(
               task, solution_type, prog, bb_con_dual_indices,
-              linear_con_dual_indices, lin_eq_con_dual_indices, result);
+              linear_con_dual_indices, lin_eq_con_dual_indices,
+              lorentz_cone_dual_indices, rotated_lorentz_cone_dual_indices,
+              exp_cone_dual_indices, result);
           DRAKE_ASSERT(rescode == MSK_RES_OK);
           break;
         }

solvers/test/exponential_cone_program_examples.cc

@@ -12,13 +12,15 @@ namespace solvers {
 namespace test {
 const double kInf = std::numeric_limits<double>::infinity();
 
-void ExponentialConeTrivialExample(const SolverInterface& solver, double tol) {
+void ExponentialConeTrivialExample(const SolverInterface& solver, double tol,
+                                   bool check_dual) {
   MathematicalProgram prog;
   auto x = prog.NewContinuousVariables<1>()(0);
   auto y = prog.NewContinuousVariables<1>()(0);
   auto z = prog.NewContinuousVariables<1>()(0);
-  prog.AddExponentialConeConstraint(Vector3<symbolic::Expression>(x, y, z));
-  prog.AddLinearEqualityConstraint(y + z == 1);
+  auto exp_constr =
+      prog.AddExponentialConeConstraint(Vector3<symbolic::Expression>(x, y, z));
+  auto lin_eq_constr = prog.AddLinearEqualityConstraint(y + z == 1);
   prog.AddLinearCost(x);
 
   MathematicalProgramResult result;
@@ -30,6 +32,14 @@ void ExponentialConeTrivialExample(const SolverInterface& solver, double tol) {
   // Namely y =1
   EXPECT_NEAR(result.GetSolution(y), 1, tol);
   EXPECT_NEAR(result.GetSolution(z), 0, tol);
+
+  // Check the dual solution.
+  if (check_dual) {
+    EXPECT_TRUE(CompareMatrices(result.GetDualSolution(exp_constr),
+                                Eigen::Vector3d(1, -1, -1), tol));
+    EXPECT_TRUE(CompareMatrices(result.GetDualSolution(lin_eq_constr),
+                                Vector1d(1), tol));
+  }
 }
 
 void MinimizeKLDivergence(const SolverInterface& solver, double tol) {

solvers/test/exponential_cone_program_examples.h

@@ -11,8 +11,10 @@ namespace test {
  * min y*exp(z/y)
  * s.t y + z = 1
  *     y > 0
+ * @param check_dual If set to true, we will also check the dual solution.
  */
-void ExponentialConeTrivialExample(const SolverInterface& solver, double tol);
+void ExponentialConeTrivialExample(const SolverInterface& solver, double tol,
+                                   bool check_dual);
 
 /**
  * For a random variable x (assuming that the sample space of x is  {0, 1, 2,

solvers/test/mosek_solver_test.cc

@@ -82,8 +82,9 @@ TEST_P(TestEllipsoidsSeparation, TestSOCP) {
   }
 }
 
-INSTANTIATE_TEST_SUITE_P(MosekTest, TestEllipsoidsSeparation,
-                        ::testing::ValuesIn(GetEllipsoidsSeparationProblems()));
+INSTANTIATE_TEST_SUITE_P(
+    MosekTest, TestEllipsoidsSeparation,
+    ::testing::ValuesIn(GetEllipsoidsSeparationProblems()));
 
 TEST_P(TestQPasSOCP, TestSOCP) {
   MosekSolver mosek_solver;
@@ -93,7 +94,7 @@ TEST_P(TestQPasSOCP, TestSOCP) {
 }
 
 INSTANTIATE_TEST_SUITE_P(MosekTest, TestQPasSOCP,
-                        ::testing::ValuesIn(GetQPasSOCPProblems()));
+                         ::testing::ValuesIn(GetQPasSOCPProblems()));
 
 TEST_P(TestFindSpringEquilibrium, TestSOCP) {
   MosekSolver mosek_solver;
@@ -183,7 +184,7 @@ GTEST_TEST(TestSemidefiniteProgram, SolveSDPwithOverlappingVariables) {
 GTEST_TEST(TestExponentialConeProgram, ExponentialConeTrivialExample) {
   MosekSolver solver;
   if (solver.available()) {
-    ExponentialConeTrivialExample(solver, 1E-5);
+    ExponentialConeTrivialExample(solver, 1E-5, true);
   }
 }
 
@@ -232,7 +233,7 @@ GTEST_TEST(MosekTest, SolverOptionsTest) {
   prog.AddLinearConstraint(100 * x(0) + 100 * x(1) <= 1);
   prog.AddConstraint(x(0) >= 0);
   prog.AddConstraint(x(1) >= 0);
-  prog.AddLinearCost(1E5* x(0) + x(1));
+  prog.AddLinearCost(1E5 * x(0) + x(1));
 
   SolverOptions solver_options;
   solver_options.SetOption(MosekSolver::id(), "MSK_DPAR_DATA_TOL_C_HUGE", 1E3);
@@ -464,6 +465,22 @@ GTEST_TEST(MosekTest, EqualityConstrainedQPDualSolution2) {
   }
 }
 
+GTEST_TEST(MosekSolver, SocpDualSolution1) {
+  MosekSolver solver;
+  if (solver.available()) {
+    SolverOptions solver_options{};
+    TestSocpDualSolution1(solver, solver_options, 1E-7);
+  }
+}
+
+GTEST_TEST(MosekSolver, SocpDualSolution2) {
+  MosekSolver solver;
+  if (solver.available()) {
+    SolverOptions solver_options{};
+    TestSocpDualSolution2(solver, solver_options, 1E-6, true);
+  }
+}
+
 GTEST_TEST(MosekTest, SDPDualSolution1) {
   MosekSolver solver;
   if (solver.available()) {

solvers/test/scs_solver_test.cc

@@ -307,7 +307,8 @@ GTEST_TEST(TestSemidefiniteProgram, SolveSDPwithOverlappingVariables) {
 GTEST_TEST(TestExponentialConeProgram, ExponentialConeTrivialExample) {
   ScsSolver solver;
   if (solver.available()) {
-    ExponentialConeTrivialExample(solver, kTol);
+    // Currently we don't support retrieving dual solution from SCS yet.
+    ExponentialConeTrivialExample(solver, kTol, false);
   }
 }