handle inf constraints in general inequalities by setting correspondi…

…ng row in G to zero
PREDICT-EPFL · Aug 1, 2024 · a051471 · a051471
1 parent 9a5e62a
commit a051471
Show file tree

Hide file tree

Showing 6 changed files with 109 additions and 4 deletions.
diff --git a/docs/_common/problem_formulation.md b/docs/_common/problem_formulation.md
@@ -14,7 +14,7 @@ $$
 with primal decision variables $$x \in \mathbb{R}^n$$, matrices $$P\in \mathbb{S}_+^n$$, $$A \in \mathbb{R}^{p \times n}$$,  $$G \in \mathbb{R}^{m \times n}$$, and vectors $$c \in \mathbb{R}^n$$, $$b \in \mathbb{R}^p$$, $$h \in \mathbb{R}^m$$, $$x_{lb} \in \mathbb{R}^n$$, and $$x_{ub} \in \mathbb{R}^n$$.
 
 {: .note }
-PIQP can handle infinite box constraints well, i.e. when elements of $$x_{lb}$$ or $$x_{ub}$$ are $$-\infty$$ or $$\infty$$, respectively. On the contrary, infinite values in the general inequalities $$Gx \leq h = \pm \infty$$ can cause problems, i.e., they are converted internally to `-1e30` and `1e30`, respectively.
+PIQP can handle infinite box constraints well, i.e. when elements of $$x_{lb}$$ or $$x_{ub}$$ are $$-\infty$$ or $$\infty$$, respectively. On the contrary, infinite values in the general inequalities $$Gx \leq h = \pm \infty$$ can cause problems. PIQP internally disables them by setting the corresponding rows in $$G$$ to zero (sparsity structure is preserved). For best performance, consider removing the corresponding constraints from the problem formulation directly.
 
 ### Example QP
 

diff --git a/include/piqp/dense/data.hpp b/include/piqp/dense/data.hpp
@@ -85,6 +85,11 @@ struct Data
 
     ~Data() {};
 
+    void set_G_row_zero(Eigen::Index row)
+    {
+        GT.col(row).setZero();
+    }
+
     Eigen::Index non_zeros_P_utri() { return P_utri.rows() * (P_utri.rows() - 1) / 2; }
     Eigen::Index non_zeros_A() { return AT.rows() * AT.cols(); }
     Eigen::Index non_zeros_G() { return GT.rows() * GT.cols(); }

diff --git a/include/piqp/solver.hpp b/include/piqp/solver.hpp
@@ -196,7 +196,8 @@ class SolverBase
         m_data.c = c;
         m_data.b = b.has_value() ? *b : Vec<T>::Zero(0);
         if (h.has_value()) {
-            m_data.h = (*h).cwiseMin(PIQP_INF).cwiseMax(-PIQP_INF);
+            m_data.h = *h;
+            disable_inf_constraints();
         } else {
             m_data.h.resize(0);
         }
@@ -234,6 +235,25 @@ class SolverBase
         }
     }
 
+    void disable_inf_constraints()
+    {
+        if (m_data.m > 0 && (m_data.h.maxCoeff() > PIQP_INF || m_data.h.minCoeff() < -PIQP_INF))
+        {
+            piqp_eprint("h contains values which are close to +/- infinity.\n");
+            piqp_eprint("PIQP is setting the corresponding rows in G to zero (sparsity preserving).\n");
+            piqp_eprint("Consider removing the corresponding constraints for faster solves.\n");
+
+            for (int i = 0; i < m_data.h.rows(); i++)
+            {
+                if (m_data.h(i) > PIQP_INF || m_data.h(i) < -PIQP_INF)
+                {
+                    m_data.set_G_row_zero(i);
+                    m_data.h(i) = T(1);
+                }
+            }
+        }
+    }
+
     void setup_lb_data(const optional<CVecRef<T>>& x_lb)
     {
         isize n_lb = 0;
@@ -1021,7 +1041,8 @@ class DenseSolver : public SolverBase<DenseSolver<T, Preconditioner>, T, int, Pr
         if (h.has_value())
         {
             if (h->size() != this->m_data.m) { piqp_eprint("h has wrong dimensions\n"); return; }
-            this->m_data.h = (*h).cwiseMin(PIQP_INF).cwiseMax(-PIQP_INF);
+            this->m_data.h = *h;
+            this->disable_inf_constraints();
         }
 
         if (x_lb.has_value() && x_lb->size() != this->m_data.n) { piqp_eprint("x_lb has wrong dimensions\n"); return; }
@@ -1134,7 +1155,8 @@ class SparseSolver : public SolverBase<SparseSolver<T, I, Mode, Preconditioner>,
         if (h.has_value())
         {
             if (h->size() != this->m_data.m) { piqp_eprint("h has wrong dimensions\n"); return; }
-            this->m_data.h = (*h).cwiseMin(PIQP_INF).cwiseMax(-PIQP_INF);
+            this->m_data.h = *h;
+            this->disable_inf_constraints();
         }
 
         if (x_lb.has_value() && x_lb->size() != this->m_data.n) { piqp_eprint("x_lb has wrong dimensions\n"); return; }

diff --git a/include/piqp/sparse/data.hpp b/include/piqp/sparse/data.hpp
@@ -88,6 +88,12 @@ struct Data
 
     ~Data() {};
 
+    void set_G_row_zero(Eigen::Index row)
+    {
+        Eigen::Index row_nnz = GT.outerIndexPtr()[row + 1] - GT.outerIndexPtr()[row];
+        Eigen::Map<Vec<T>>(GT.valuePtr() + GT.outerIndexPtr()[row], row_nnz).setZero();
+    }
+
     Eigen::Index non_zeros_P_utri() { return P_utri.nonZeros(); }
     Eigen::Index non_zeros_A() { return AT.nonZeros(); }
     Eigen::Index non_zeros_G() { return GT.nonZeros(); }

diff --git a/tests/src/dense/solver_test.cpp b/tests/src/dense/solver_test.cpp
@@ -300,3 +300,35 @@ TEST(DenseSolverTest, StronglyConvexNoConstraints)
 
     ASSERT_EQ(status, Status::PIQP_SOLVED);
 }
+
+TEST(DenseSolverTest, InfinityBounds)
+{
+    Mat<T> P(4, 4); P << 1, 0, 0, 0,
+                         0, 1, 0, 0,
+                         0, 0, 1, 0,
+                         0, 0, 0, 1;
+    Vec<T> c(4); c << 1, 1, 1, 1;
+
+    Mat<T> G(6, 4); G << 1,  0, 0,  0,
+                         1,  0, -1, 0,
+                         -1, 0, -1, 0,
+                         -1, 0, 0,  0,
+                         -1, 0, 1,  0,
+                         1,  0, 1,  0;
+    T inf = std::numeric_limits<T>::infinity();
+    Vec<T> h(6); h << 1, 1, 1, 1, inf, inf;
+
+    DenseSolver<T> solver;
+    solver.settings().verbose = true;
+    solver.setup(P, c, piqp::nullopt, piqp::nullopt, G, h);
+
+    PIQP_EIGEN_MALLOC_NOT_ALLOWED();
+    Status status = solver.solve();
+    PIQP_EIGEN_MALLOC_ALLOWED();
+
+    ASSERT_EQ(status, Status::PIQP_SOLVED);
+    ASSERT_NEAR(solver.result().x(0), -0.5, 1e-6);
+    ASSERT_NEAR(solver.result().x(1), -1.0, 1e-6);
+    ASSERT_NEAR(solver.result().x(2), -0.5, 1e-6);
+    ASSERT_NEAR(solver.result().x(3), -1.0, 1e-6);
+}
diff --git a/tests/src/sparse/solver_test.cpp b/tests/src/sparse/solver_test.cpp
@@ -345,3 +345,43 @@ TYPED_TEST(SparseSolverTest, StronglyConvexNoConstraints)
 
     ASSERT_EQ(status, Status::PIQP_SOLVED);
 }
+
+TYPED_TEST(SparseSolverTest, InfinityBounds)
+{
+    SparseMat<T, I> P(4, 4);
+    P.insert(0, 0) = 1;
+    P.insert(1, 1) = 1;
+    P.insert(2, 2) = 1;
+    P.insert(3, 3) = 1;
+    P.makeCompressed();
+    Vec<T> c(4); c << 1, 1, 1, 1;
+
+    SparseMat<T, I> G(6, 4);
+    G.insert(0, 0) = 1;
+    G.insert(1, 0) = 1;
+    G.insert(1, 2) = -1;
+    G.insert(2, 0) = -1;
+    G.insert(2, 2) = -1;
+    G.insert(3, 0) = -1;
+    G.insert(4, 0) = -1;
+    G.insert(4, 2) = 1;
+    G.insert(5, 0) = 1;
+    G.insert(5, 2) = 1;
+    G.makeCompressed();
+    T inf = std::numeric_limits<T>::infinity();
+    Vec<T> h(6); h << 1, 1, 1, 1, inf, inf;
+
+    SparseSolver<T, I, TypeParam::Mode> solver;
+    solver.settings().verbose = true;
+    solver.setup(P, c, piqp::nullopt, piqp::nullopt, G, h);
+
+    PIQP_EIGEN_MALLOC_NOT_ALLOWED();
+    Status status = solver.solve();
+    PIQP_EIGEN_MALLOC_ALLOWED();
+
+    ASSERT_EQ(status, Status::PIQP_SOLVED);
+    ASSERT_NEAR(solver.result().x(0), -0.5, 1e-6);
+    ASSERT_NEAR(solver.result().x(1), -1.0, 1e-6);
+    ASSERT_NEAR(solver.result().x(2), -0.5, 1e-6);
+    ASSERT_NEAR(solver.result().x(3), -1.0, 1e-6);
+}