diff --git a/CHANGELOG.md b/CHANGELOG.md index 30bbb3fe5..06778fe46 100644 --- a/CHANGELOG.md +++ b/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)) diff --git a/include/proxsuite/proxqp/dense/solver.hpp b/include/proxsuite/proxqp/dense/solver.hpp index 6d428ae64..336393dba 100644 --- a/include/proxsuite/proxqp/dense/solver.hpp +++ b/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 &&