Merge pull request #432 from Lucas-Haubert/fix/proxqp_solved_closest_primal_feasible

Correction of update status PROXQP_SOLVED_CLOSEST_FEASIBLE in dense backend

Author: jorisv

CHANGELOG.md

@@ -17,6 +17,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Better dynamic module handling ([#419](https://github.com/Simple-Robotics/proxsuite/pull/419))
 
 ### Fixed
+- Correction of the status update for `PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE` ([#432])(https://github.com/Simple-Robotics/proxsuite/pull/432)
 - Use the right table to store `configure-args` cmeel argument ([#403](https://github.com/Simple-Robotics/proxsuite/pull/403))
 - Allow project to be used as an external CMake project (FetchContent) ([#408](https://github.com/Simple-Robotics/proxsuite/pull/408))
 - Fix -Wdeprecated-literal-operator warning ([#420](https://github.com/Simple-Robotics/proxsuite/pull/420))

include/proxsuite/proxqp/dense/solver.hpp

@@ -1508,7 +1508,13 @@ qp_solve( //
           break;
         }
       } else {
-        qpresults.info.status = QPSolverOutput::PROXQP_SOLVED;
+        if (qpsettings.primal_infeasibility_solving &&
+            qpresults.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) {
+          qpresults.info.status =
+            QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE;
+        } else {
+          qpresults.info.status = QPSolverOutput::PROXQP_SOLVED;
+        }
         break;
       }
     }
@@ -1593,8 +1599,8 @@ qp_solve( //
       scaled_eps =
         infty_norm(qpwork.rhs.head(qpmodel.dim)) * qpsettings.eps_abs;
     }
-    T primal_feasibility_lhs_new(primal_feasibility_lhs);
 
+    T primal_feasibility_lhs_new(primal_feasibility_lhs);
     global_primal_residual(qpmodel,
                            qpresults,
                            qpsettings,
@@ -1607,37 +1613,38 @@ qp_solve( //
                            primal_feasibility_eq_lhs,
                            primal_feasibility_in_lhs);
 
+    T dual_feasibility_lhs_new(dual_feasibility_lhs);
+    global_dual_residual(qpresults,
+                         qpwork,
+                         qpmodel,
+                         box_constraints,
+                         ruiz,
+                         dual_feasibility_lhs_new,
+                         dual_feasibility_rhs_0,
+                         dual_feasibility_rhs_1,
+                         dual_feasibility_rhs_3,
+                         rhs_duality_gap,
+                         duality_gap,
+                         hessian_type);
+
+    qpresults.info.pri_res = primal_feasibility_lhs_new;
+    qpresults.info.dua_res = dual_feasibility_lhs_new;
+    qpresults.info.duality_gap = duality_gap;
+
     is_primal_feasible =
       primal_feasibility_lhs_new <=
       (scaled_eps + qpsettings.eps_rel * std::max(primal_feasibility_eq_rhs_0,
                                                   primal_feasibility_in_rhs_0));
-    qpresults.info.pri_res = primal_feasibility_lhs_new;
-    if (is_primal_feasible) {
-      T dual_feasibility_lhs_new(dual_feasibility_lhs);
 
-      global_dual_residual(qpresults,
-                           qpwork,
-                           qpmodel,
-                           box_constraints,
-                           ruiz,
-                           dual_feasibility_lhs_new,
-                           dual_feasibility_rhs_0,
-                           dual_feasibility_rhs_1,
-                           dual_feasibility_rhs_3,
-                           rhs_duality_gap,
-                           duality_gap,
-                           hessian_type);
-      qpresults.info.dua_res = dual_feasibility_lhs_new;
-      qpresults.info.duality_gap = duality_gap;
-
-      is_dual_feasible =
-        dual_feasibility_lhs_new <=
-        (qpsettings.eps_abs +
-         qpsettings.eps_rel *
-           std::max(
-             std::max(dual_feasibility_rhs_3, dual_feasibility_rhs_0),
-             std::max(dual_feasibility_rhs_1, qpwork.dual_feasibility_rhs_2)));
+    is_dual_feasible =
+      dual_feasibility_lhs_new <=
+      (qpsettings.eps_abs +
+       qpsettings.eps_rel *
+         std::max(
+           std::max(dual_feasibility_rhs_3, dual_feasibility_rhs_0),
+           std::max(dual_feasibility_rhs_1, qpwork.dual_feasibility_rhs_2)));
 
+    if (is_primal_feasible) {
       if (is_dual_feasible) {
         if (qpsettings.check_duality_gap) {
           if (std::fabs(qpresults.info.duality_gap) <=
@@ -1651,6 +1658,7 @@ qp_solve( //
             } else {
               qpresults.info.status = QPSolverOutput::PROXQP_SOLVED;
             }
+            break;
           }
         } else {
           if (qpsettings.primal_infeasibility_solving &&
@@ -1661,9 +1669,11 @@ qp_solve( //
           } else {
             qpresults.info.status = QPSolverOutput::PROXQP_SOLVED;
           }
+          break;
         }
       }
     }
+
     if (qpsettings.bcl_update) {
       bcl_update(qpsettings,
                  qpresults,
@@ -1690,24 +1700,8 @@ qp_solve( //
                       new_bcl_mu_in_inv,
                       new_bcl_mu_eq_inv);
     }
-    // COLD RESTART
-
-    T dual_feasibility_lhs_new(dual_feasibility_lhs);
 
-    global_dual_residual(qpresults,
-                         qpwork,
-                         qpmodel,
-                         box_constraints,
-                         ruiz,
-                         dual_feasibility_lhs_new,
-                         dual_feasibility_rhs_0,
-                         dual_feasibility_rhs_1,
-                         dual_feasibility_rhs_3,
-                         rhs_duality_gap,
-                         duality_gap,
-                         hessian_type);
-    qpresults.info.dua_res = dual_feasibility_lhs_new;
-    qpresults.info.duality_gap = duality_gap;
+    // COLD RESTART
 
     if (primal_feasibility_lhs_new >= primal_feasibility_lhs &&
         dual_feasibility_lhs_new >= dual_feasibility_lhs &&