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,19 @@ 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:
+   * The interpretation for the dual variable to conic constraint x ∈ K can be
+   * different. Here K is a convex cone, including (rotated) Lorentz cone,
+   * 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. The only
+   * exception is when the problem is solved through Gurobi, that its dual
+   * variable still has the "shadow price" interpretation.
+   *
+   * 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 +310,12 @@ 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.0/capi/prob-def-conic.html#duality-for-conic-optimization
+   *    as an explanation.
    */
   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/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;
@@ -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/second_order_cone_program_examples.cc

@@ -473,6 +473,62 @@ void SolveAndCheckSmallestEllipsoidCoveringProblems(
     prob_4d.CheckSolution(result, tol);
   }
 }
+
+void TestSocpDualSolution1(const SolverInterface& solver,
+                           const SolverOptions& solver_options, double tol) {
+  MathematicalProgram prog;
+  auto x = prog.NewContinuousVariables<2>();
+  auto constraint1 = prog.AddLorentzConeConstraint(
+      Vector3<symbolic::Expression>(2., 2 * x(0), 3 * x(1) + 1));
+  prog.AddLinearCost(x(1));
+  if (solver.available()) {
+    // By default the dual solution for second order cone is not computed.
+    MathematicalProgramResult result;
+    solver.Solve(prog, {} /* empty initial guess */, solver_options, &result);
+    // The dual solution for lorentz cone constraint are the values of the dual
+    // variables, lies in the dual cone of a Lorentz cone (which is also a
+    // Lorentz cone). Notice that this is NOT the shadow price as in the linear
+    // constraints.
+    EXPECT_TRUE(CompareMatrices(result.GetDualSolution(constraint1),
+                                Eigen::Vector3d(1. / 3, 0, 1. / 3), tol));
+
+    auto bb_con = prog.AddBoundingBoxConstraint(0.1, kInf, x(1));
+    solver.Solve(prog, {}, solver_options, &result);
+    ASSERT_TRUE(result.is_success());
+    EXPECT_NEAR(result.GetSolution(x(1)), 0.1, tol);
+    // The cost is x(1), hence the shadow price for the constraint x(1) >= 0
+    // should be 1.
+    EXPECT_TRUE(
+        CompareMatrices(result.GetDualSolution(bb_con), Vector1d(1.), tol));
+  }
+}
+
+void TestSocpDualSolution2(const SolverInterface& solver,
+                           const SolverOptions& solver_options, double tol,
+                           bool rotated_lorentz_cone_with_coefficient_two) {
+  MathematicalProgram prog;
+  auto x = prog.NewContinuousVariables<1>()(0);
+  auto constraint1 = prog.AddRotatedLorentzConeConstraint(
+      Vector3<symbolic::Expression>(2., x + 1.5, x));
+  auto constraint2 =
+      prog.AddLorentzConeConstraint(Vector2<symbolic::Expression>(1, x + 1));
+  prog.AddLinearCost(x);
+  if (solver.available()) {
+    SolverOptions options;
+    MathematicalProgramResult result;
+    solver.Solve(prog, {}, solver_options, &result);
+    ASSERT_TRUE(result.is_success());
+    const Eigen::Vector3d constraint1_dual =
+        rotated_lorentz_cone_with_coefficient_two
+            ? Eigen::Vector3d(0.25, 0.5, 0.5)
+            : Eigen::Vector3d(0.5, 0.5, 0.5);
+    EXPECT_TRUE(CompareMatrices(result.GetDualSolution(constraint1),
+                                constraint1_dual, tol));
+    // This Lorentz cone is not activated, hence its dual should be zero.
+    EXPECT_TRUE(CompareMatrices(result.GetDualSolution(constraint2),
+                                Eigen::Vector2d(0, 0), tol));
+  }
+}
 }  // namespace test
 }  // namespace solvers
 }  // namespace drake

solvers/test/second_order_cone_program_examples.h

@@ -320,6 +320,17 @@ class MinimalDistanceFromSphereProblem {
   double radius_;
 };
 
+void TestSocpDualSolution1(const SolverInterface& solver,
+                           const SolverOptions& solver_options, double tol);
+
+// @param rotated_lorentz_cone_with_coeffcieint_two. Set this to true if this
+// solver has a coefficient 2 on the rotated Lorentz cone constraint as 2*x₁x₂
+// >= x₃² + ... + xₙ² (like in Mosek). Set this to false if this solver doesn't
+// have a coefficient 2 on the rotated Lorentz cone constraint, as x₁x₂
+// >= x₃² + ... + xₙ²
+void TestSocpDualSolution2(const SolverInterface& solver,
+                           const SolverOptions& solver_options, double tol,
+                           bool rotated_lorentz_cone_with_coefficient_two);
 }  // namespace test
 }  // namespace solvers
 }  // namespace drake