From a0514713336fb923dd467a9d4f788167e8f09496 Mon Sep 17 00:00:00 2001 From: Roland Schwan Date: Thu, 1 Aug 2024 15:59:27 +0200 Subject: [PATCH] handle inf constraints in general inequalities by setting corresponding row in G to zero --- docs/_common/problem_formulation.md | 2 +- include/piqp/dense/data.hpp | 5 ++++ include/piqp/solver.hpp | 28 +++++++++++++++++--- include/piqp/sparse/data.hpp | 6 +++++ tests/src/dense/solver_test.cpp | 32 +++++++++++++++++++++++ tests/src/sparse/solver_test.cpp | 40 +++++++++++++++++++++++++++++ 6 files changed, 109 insertions(+), 4 deletions(-) diff --git a/docs/_common/problem_formulation.md b/docs/_common/problem_formulation.md index f9a37b8..96d173f 100644 --- 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 index e4a3913..0a56f44 100644 --- 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 index 1761dd5..e5710f3 100644 --- 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::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>& x_lb) { isize n_lb = 0; @@ -1021,7 +1041,8 @@ class DenseSolver : public SolverBase, 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, 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 index d7f7e91..93c7367 100644 --- 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>(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 index cd77934..0b68fe1 100644 --- 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 P(4, 4); P << 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1; + Vec c(4); c << 1, 1, 1, 1; + + Mat 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::infinity(); + Vec h(6); h << 1, 1, 1, 1, inf, inf; + + DenseSolver 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 index 9f4f364..c9cda00 100644 --- 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 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 c(4); c << 1, 1, 1, 1; + + SparseMat 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::infinity(); + Vec h(6); h << 1, 1, 1, 1, inf, inf; + + SparseSolver 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); +}