Merge pull request #110 from Bambade/add_duality_gap

jcarpent · web-flow · commit 90d81af3d804 · 2022-11-12T08:49:35.000+01:00
Add duality gap measure
diff --git a/include/proxsuite/proxqp/dense/solver.hpp b/include/proxsuite/proxqp/dense/solver.hpp
@@ -985,6 +985,9 @@ qp_solve( //
   T primal_feasibility_in_lhs(0);
   T dual_feasibility_lhs(0);
 
+  T duality_gap(0);
+  T rhs_duality_gap(0);
+
   for (i64 iter = 0; iter < qpsettings.max_iter; ++iter) {
 
     // compute primal residual
@@ -1002,13 +1005,17 @@ qp_solve( //
 
     global_dual_residual(qpresults,
                          qpwork,
+                         qpmodel,
                          ruiz,
                          dual_feasibility_lhs,
                          dual_feasibility_rhs_0,
                          dual_feasibility_rhs_1,
-                         dual_feasibility_rhs_3);
+                         dual_feasibility_rhs_3,
+                         rhs_duality_gap,
+                         duality_gap);
     qpresults.info.pri_res = primal_feasibility_lhs;
     qpresults.info.dua_res = dual_feasibility_lhs;
+    qpresults.info.duality_gap = duality_gap;
 
     T new_bcl_mu_in(qpresults.info.mu_in);
     T new_bcl_mu_eq(qpresults.info.mu_eq);
@@ -1059,8 +1066,9 @@ qp_solve( //
       std::cout << std::scientific << std::setw(2) << std::setprecision(2)
                 << "| primal residual=" << qpresults.info.pri_res
                 << "| dual residual=" << qpresults.info.dua_res
-                << " | mu_in=" << qpresults.info.mu_in
-                << " | rho=" << qpresults.info.rho << std::endl;
+                << "| duality gap=" << qpresults.info.duality_gap
+                << "| mu_in=" << qpresults.info.mu_in
+                << "| rho=" << qpresults.info.rho << std::endl;
       ruiz.scale_primal_in_place(VectorViewMut<T>{ from_eigen, qpresults.x });
       ruiz.scale_dual_in_place_eq(VectorViewMut<T>{ from_eigen, qpresults.y });
       ruiz.scale_dual_in_place_in(VectorViewMut<T>{ from_eigen, qpresults.z });
@@ -1079,8 +1087,12 @@ qp_solve( //
         qpresults.info.rho = rho_new;
       }
       if (is_dual_feasible) {
-        qpresults.info.status = QPSolverOutput::PROXQP_SOLVED;
-        break;
+        if (qpresults.info.duality_gap <=
+            qpsettings.eps_abs +
+              (qpsettings.eps_abs + qpsettings.eps_rel) * rhs_duality_gap) {
+          qpresults.info.status = QPSolverOutput::PROXQP_SOLVED;
+          break;
+        }
       }
     }
     qpresults.info.iter_ext += 1; // We start a new external loop update
@@ -1138,12 +1150,16 @@ qp_solve( //
 
       global_dual_residual(qpresults,
                            qpwork,
+                           qpmodel,
                            ruiz,
                            dual_feasibility_lhs_new,
                            dual_feasibility_rhs_0,
                            dual_feasibility_rhs_1,
-                           dual_feasibility_rhs_3);
+                           dual_feasibility_rhs_3,
+                           rhs_duality_gap,
+                           duality_gap);
       qpresults.info.dua_res = dual_feasibility_lhs_new;
+      qpresults.info.duality_gap = duality_gap;
 
       is_dual_feasible =
         dual_feasibility_lhs_new <=
@@ -1154,8 +1170,12 @@ qp_solve( //
              std::max(dual_feasibility_rhs_1, qpwork.dual_feasibility_rhs_2)));
 
       if (is_dual_feasible) {
-        qpresults.info.status = QPSolverOutput::PROXQP_SOLVED;
-        break;
+        if (qpresults.info.duality_gap <=
+            qpsettings.eps_abs +
+              (qpsettings.eps_abs + qpsettings.eps_rel) * rhs_duality_gap) {
+          qpresults.info.status = QPSolverOutput::PROXQP_SOLVED;
+          break;
+        }
       }
     }
     if (qpsettings.bcl_update) {
@@ -1190,12 +1210,16 @@ qp_solve( //
 
     global_dual_residual(qpresults,
                          qpwork,
+                         qpmodel,
                          ruiz,
                          dual_feasibility_lhs_new,
                          dual_feasibility_rhs_0,
                          dual_feasibility_rhs_1,
-                         dual_feasibility_rhs_3);
+                         dual_feasibility_rhs_3,
+                         rhs_duality_gap,
+                         duality_gap);
     qpresults.info.dua_res = dual_feasibility_lhs_new;
+    qpresults.info.duality_gap = duality_gap;
 
     if (primal_feasibility_lhs_new >= primal_feasibility_lhs &&
         dual_feasibility_lhs_new >= dual_feasibility_lhs &&
diff --git a/include/proxsuite/proxqp/dense/utils.hpp b/include/proxsuite/proxqp/dense/utils.hpp
@@ -343,27 +343,44 @@ global_dual_residual_infeasibility(
  */
 template<typename T>
 void
-global_dual_residual(const Results<T>& qpresults,
+global_dual_residual(Results<T>& qpresults,
                      Workspace<T>& qpwork,
+                     const Model<T>& qpmodel,
                      const preconditioner::RuizEquilibration<T>& ruiz,
                      T& dual_feasibility_lhs,
                      T& dual_feasibility_rhs_0,
                      T& dual_feasibility_rhs_1,
-                     T& dual_feasibility_rhs_3)
+                     T& dual_feasibility_rhs_3,
+                     T& rhs_duality_gap,
+                     T& duality_gap)
 {
   // dual_feasibility_lhs = norm(dual_residual_scaled)
   // dual_feasibility_rhs_0 = norm(unscaled(Hx))
   // dual_feasibility_rhs_1 = norm(unscaled(ATy))
   // dual_feasibility_rhs_3 = norm(unscaled(CTz))
   //
   // dual_residual_scaled = scaled(Hx + g + ATy + CTz)
+  const isize max_dim =
+    std::max(qpmodel.dim, std::max(qpmodel.n_eq, qpmodel.n_in));
+  const T sqrt_max_dim(std::sqrt(max_dim)); // for normalizing scalar products
+
   qpwork.dual_residual_scaled = qpwork.g_scaled;
   qpwork.CTz.noalias() =
     qpwork.H_scaled.template selfadjointView<Eigen::Lower>() * qpresults.x;
   qpwork.dual_residual_scaled += qpwork.CTz;
   ruiz.unscale_dual_residual_in_place(
-    VectorViewMut<T>{ from_eigen, qpwork.CTz });
+    VectorViewMut<T>{ from_eigen, qpwork.CTz }); // contains unscaled Hx
   dual_feasibility_rhs_0 = infty_norm(qpwork.CTz);
+
+  ruiz.unscale_primal_in_place(VectorViewMut<T>{ from_eigen, qpresults.x });
+  duality_gap = (qpmodel.g).dot(qpresults.x);
+  rhs_duality_gap = std::abs(duality_gap);
+  T xHx = (qpwork.CTz).dot(qpresults.x);
+  duality_gap += xHx; // contains now xHx+g.Tx
+  rhs_duality_gap = std::max(rhs_duality_gap, std::abs(xHx));
+
+  ruiz.scale_primal_in_place(VectorViewMut<T>{ from_eigen, qpresults.x });
+
   qpwork.CTz.noalias() = qpwork.A_scaled.transpose() * qpresults.y;
   qpwork.dual_residual_scaled += qpwork.CTz;
   ruiz.unscale_dual_residual_in_place(
@@ -383,6 +400,21 @@ global_dual_residual(const Results<T>& qpresults,
 
   ruiz.scale_dual_residual_in_place(
     VectorViewMut<T>{ from_eigen, qpwork.dual_residual_scaled });
+
+  ruiz.unscale_dual_in_place_eq(VectorViewMut<T>{ from_eigen, qpresults.y });
+  T by = (qpmodel.b).dot(qpresults.y);
+  rhs_duality_gap = std::max(rhs_duality_gap, std::abs(by));
+  duality_gap += by;
+  ruiz.scale_dual_in_place_eq(VectorViewMut<T>{ from_eigen, qpresults.y });
+  ruiz.unscale_dual_in_place_in(VectorViewMut<T>{ from_eigen, qpresults.z });
+  T zu = positive_part(qpresults.z).dot(qpmodel.u);
+  rhs_duality_gap = std::max(rhs_duality_gap, std::abs(zu));
+  duality_gap += zu;
+  T zl = negative_part(qpresults.z).dot(qpmodel.l);
+  rhs_duality_gap = std::max(rhs_duality_gap, std::abs(zl));
+  duality_gap += zl;
+  ruiz.scale_dual_in_place_in(VectorViewMut<T>{ from_eigen, qpresults.z });
+  duality_gap /= sqrt_max_dim; // in order to get an a-dimensional duality gap
 }
 
 } // namespace dense
diff --git a/include/proxsuite/proxqp/results.hpp b/include/proxsuite/proxqp/results.hpp
@@ -49,7 +49,7 @@ struct Info
   T objValue;
   T pri_res;
   T dua_res;
-
+  T duality_gap;
   //// sparse backend used by solver, either CholeskySparse or MatrixFree
   SparseBackend sparse_backend;
 };
@@ -106,6 +106,7 @@ struct Results
     info.objValue = 0.;
     info.pri_res = 0.;
     info.dua_res = 0.;
+    info.duality_gap = 0.;
     info.status = QPSolverOutput::PROXQP_NOT_RUN;
     info.sparse_backend = SparseBackend::Automatic;
   }
@@ -132,6 +133,7 @@ struct Results
     info.rho_updates = 0;
     info.pri_res = 0.;
     info.dua_res = 0.;
+    info.duality_gap = 0.;
     info.status = QPSolverOutput::PROXQP_MAX_ITER_REACHED;
     info.sparse_backend = SparseBackend::Automatic;
   }
diff --git a/include/proxsuite/proxqp/sparse/solver.hpp b/include/proxsuite/proxqp/sparse/solver.hpp
@@ -724,6 +724,8 @@ qp_solve(Results<T>& results,
       break;
     }
   }
+  T rhs_duality_gap(0);
+
   for (isize iter = 0; iter < settings.max_iter; ++iter) {
 
     results.info.iter_ext += 1;
@@ -777,7 +779,8 @@ qp_solve(Results<T>& results,
       VEG_BIND( // ?
         auto,
         (primal_feasibility_lhs, dual_feasibility_lhs),
-        detail::unscaled_primal_dual_residual(primal_residual_eq_scaled,
+        detail::unscaled_primal_dual_residual(results,
+                                              primal_residual_eq_scaled,
                                               primal_residual_in_scaled_lo,
                                               primal_residual_in_scaled_up,
                                               dual_residual_scaled,
@@ -786,12 +789,13 @@ qp_solve(Results<T>& results,
                                               dual_feasibility_rhs_0,
                                               dual_feasibility_rhs_1,
                                               dual_feasibility_rhs_3,
+                                              rhs_duality_gap,
                                               precond,
                                               data,
                                               qp_scaled.as_const(),
-                                              detail::vec(x_e),
-                                              detail::vec(y_e),
-                                              detail::vec(z_e),
+                                              detail::vec_mut(x_e),
+                                              detail::vec_mut(y_e),
+                                              detail::vec_mut(z_e),
                                               stack));
       /*put in debug mode
       if (settings.verbose) {
@@ -824,8 +828,9 @@ qp_solve(Results<T>& results,
         std::cout << std::scientific << std::setw(2) << std::setprecision(2)
                   << "| primal residual=" << primal_feasibility_lhs
                   << "| dual residual=" << dual_feasibility_lhs
-                  << " | mu_in=" << results.info.mu_in
-                  << " | rho=" << results.info.rho << std::endl;
+                  << "| dualality gap=" << results.info.duality_gap
+                  << "| mu_in=" << results.info.mu_in
+                  << "| rho=" << results.info.rho << std::endl;
         results.info.pri_res = primal_feasibility_lhs;
         results.info.dua_res = dual_feasibility_lhs;
         precond.scale_primal_in_place(VectorViewMut<T>{ from_eigen, x_e });
@@ -834,10 +839,14 @@ qp_solve(Results<T>& results,
       }
       if (is_primal_feasible(primal_feasibility_lhs) &&
           is_dual_feasible(dual_feasibility_lhs)) {
-        results.info.pri_res = primal_feasibility_lhs;
-        results.info.dua_res = dual_feasibility_lhs;
-        results.info.status = QPSolverOutput::PROXQP_SOLVED;
-        break;
+        if (results.info.duality_gap <=
+            settings.eps_abs +
+              (settings.eps_rel + settings.eps_abs) * rhs_duality_gap) {
+          results.info.pri_res = primal_feasibility_lhs;
+          results.info.dua_res = dual_feasibility_lhs;
+          results.info.status = QPSolverOutput::PROXQP_SOLVED;
+          break;
+        }
       }
 
       LDLT_TEMP_VEC_UNINIT(T, x_prev_e, n, stack);
@@ -1202,7 +1211,8 @@ qp_solve(Results<T>& results,
       VEG_BIND(
         auto,
         (primal_feasibility_lhs_new, dual_feasibility_lhs_new),
-        detail::unscaled_primal_dual_residual(primal_residual_eq_scaled,
+        detail::unscaled_primal_dual_residual(results,
+                                              primal_residual_eq_scaled,
                                               primal_residual_in_scaled_lo,
                                               primal_residual_in_scaled_up,
                                               dual_residual_scaled,
@@ -1211,20 +1221,25 @@ qp_solve(Results<T>& results,
                                               dual_feasibility_rhs_0,
                                               dual_feasibility_rhs_1,
                                               dual_feasibility_rhs_3,
+                                              rhs_duality_gap,
                                               precond,
                                               data,
                                               qp_scaled.as_const(),
-                                              detail::vec(x_e),
-                                              detail::vec(y_e),
-                                              detail::vec(z_e),
+                                              detail::vec_mut(x_e),
+                                              detail::vec_mut(y_e),
+                                              detail::vec_mut(z_e),
                                               stack));
 
       if (is_primal_feasible(primal_feasibility_lhs_new) &&
           is_dual_feasible(dual_feasibility_lhs_new)) {
-        results.info.pri_res = primal_feasibility_lhs_new;
-        results.info.dua_res = dual_feasibility_lhs_new;
-        results.info.status = QPSolverOutput::PROXQP_SOLVED;
-        break;
+        if (results.info.duality_gap <=
+            settings.eps_abs +
+              (settings.eps_abs + settings.eps_rel) * rhs_duality_gap) {
+          results.info.pri_res = primal_feasibility_lhs_new;
+          results.info.dua_res = dual_feasibility_lhs_new;
+          results.info.status = QPSolverOutput::PROXQP_SOLVED;
+          break;
+        }
       }
 
       // vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv
@@ -1259,7 +1274,8 @@ qp_solve(Results<T>& results,
       VEG_BIND(
         auto,
         (_, dual_feasibility_lhs_new_2),
-        detail::unscaled_primal_dual_residual(primal_residual_eq_scaled,
+        detail::unscaled_primal_dual_residual(results,
+                                              primal_residual_eq_scaled,
                                               primal_residual_in_scaled_lo,
                                               primal_residual_in_scaled_up,
                                               dual_residual_scaled,
@@ -1268,12 +1284,13 @@ qp_solve(Results<T>& results,
                                               dual_feasibility_rhs_0,
                                               dual_feasibility_rhs_1,
                                               dual_feasibility_rhs_3,
+                                              rhs_duality_gap,
                                               precond,
                                               data,
                                               qp_scaled.as_const(),
-                                              detail::vec(x_e),
-                                              detail::vec(y_e),
-                                              detail::vec(z_e),
+                                              detail::vec_mut(x_e),
+                                              detail::vec_mut(y_e),
+                                              detail::vec_mut(z_e),
                                               stack));
       proxsuite::linalg::veg::unused(_);
 
diff --git a/include/proxsuite/proxqp/sparse/utils.hpp b/include/proxsuite/proxqp/sparse/utils.hpp
