Merge pull request #328 from fabinsch/fix-infeas-detection

Fix infeasibility detection logic
Simple-Robotics · Jun 15, 2024 · fe3a7ec · fe3a7ec
2 parents f5ae9f1 + b7adeb8
commit fe3a7ec
Show file tree

Hide file tree

Showing 5 changed files with 52 additions and 6 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,6 +6,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+### Fixed
+* Fix infeasibility detection and add a unit test ([#328](https://github.com/Simple-Robotics/proxsuite/pull/328))
+
 ## [0.6.5] - 2024-05-31
 
 ### Added

diff --git a/include/proxsuite/proxqp/dense/utils.hpp b/include/proxsuite/proxqp/dense/utils.hpp
@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2022 INRIA
+// Copyright (c) 2022-2024 INRIA
 //
 /**
  * @file utils.hpp
@@ -289,7 +289,7 @@ global_primal_residual_infeasibility(
   //
   // the variables in entry are changed in place
 
-  bool res = infty_norm(dy.to_eigen()) != 0 && infty_norm(dz.to_eigen()) != 0;
+  bool res = infty_norm(dy.to_eigen()) != 0 || infty_norm(dz.to_eigen()) != 0;
   if (!res) {
     return res;
   }

diff --git a/include/proxsuite/proxqp/sparse/utils.hpp b/include/proxsuite/proxqp/sparse/utils.hpp
@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2022-2023 INRIA
+// Copyright (c) 2022-2024 INRIA
 //
 /** \file */
 
@@ -488,7 +488,7 @@ global_primal_residual_infeasibility(VectorViewMut<T> ATdy,
   //
   // the variables in entry are changed in place
 
-  bool res = infty_norm(dy.to_eigen()) != 0 && infty_norm(dz.to_eigen()) != 0;
+  bool res = infty_norm(dy.to_eigen()) != 0 || infty_norm(dz.to_eigen()) != 0;
   if (!res) {
     return res;
   }

diff --git a/test/src/dense_qp_eq.cpp b/test/src/dense_qp_eq.cpp
@@ -1,5 +1,5 @@
 //
-// Copyright (c) 2022 INRIA
+// Copyright (c) 2022 - 2024 INRIA
 //
 #include <iostream>
 #include <doctest.hpp>
@@ -212,4 +212,45 @@ DOCTEST_TEST_CASE("linear problem with equality with equality constraints and "
     std::cout << "total number of iteration: " << qp.results.info.iter
               << std::endl;
   }
+}
+
+DOCTEST_TEST_CASE("infeasible qp")
+{
+  // (x1- 9)^2 + (x2-6)^2
+  // s.t.
+  // x1 <= 10
+  // x2 <= 10
+  // x1 >= 20
+  Eigen::Matrix<T, 2, 2> H;
+  H << 1.0, 0.0, 0.0, 1.0;
+  H = 2 * H;
+
+  Eigen::Matrix<T, 2, 1> g;
+  g << -18.0, -12.0;
+
+  Eigen::Matrix<T, 3, 2> C;
+  C << 1, 0, // x1 <= 10
+    0, 1,    // x2 <= 10
+    -1, 0;   // x1 >= 20
+
+  Eigen::Matrix<T, 3, 1> u;
+  u << 10, 10, -20;
+
+  int n = H.rows();
+  int n_in = C.rows();
+  int n_eq = 0;
+
+  Eigen::Matrix<T, Eigen::Dynamic, 1> l =
+    Eigen::Matrix<T, Eigen::Dynamic, 1>::Constant(
+      n_in, -std::numeric_limits<double>::infinity());
+
+  proxsuite::proxqp::dense::QP<T> qp(n, n_eq, n_in);
+  qp.init(H, g, nullopt, nullopt, C, l, u);
+  qp.settings.eps_rel = 0.;
+  qp.settings.eps_abs = 1e-9;
+
+  qp.solve();
+
+  DOCTEST_CHECK(qp.results.info.status ==
+                proxsuite::proxqp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE);
 }
diff --git a/test/src/dense_qp_with_eq_and_in.cpp b/test/src/dense_qp_with_eq_and_in.cpp
@@ -172,7 +172,7 @@ DOCTEST_TEST_CASE("sparse random strongly convex qp with degenerate inequality "
     << "---testing sparse random strongly convex qp with degenerate "
        "inequality constraints and increasing dimension using the API---"
     << std::endl;
-  T sparsity_factor = 0.15;
+  T sparsity_factor = 0.45;
   T eps_abs = T(1e-9);
   T strong_convexity_factor(1e-2);
   proxqp::utils::rand::set_seed(1);
@@ -197,6 +197,8 @@ DOCTEST_TEST_CASE("sparse random strongly convex qp with degenerate inequality "
             qp_random.l,
             qp_random.u);
     qp.solve();
+    DOCTEST_CHECK(qp.results.info.status ==
+                  proxqp::QPSolverOutput::PROXQP_SOLVED);
     T pri_res = std::max(
       (qp_random.A * qp.results.x - qp_random.b).lpNorm<Eigen::Infinity>(),
       (helpers::positive_part(qp_random.C * qp.results.x - qp_random.u) +