qp_solver.hpp

Go to the documentation of this file.

// Copyright (C) 2022 Petter Nilsson, John B. Mains. MIT License.
 
#pragma once
 
#include <chrono>
#include <iomanip>
#include <iostream>
#include <limits>
#include <optional>
 
#include <Eigen/Cholesky>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
 
#include "qp.hpp"
#include "utils/sparse.hpp"
 
namespace smooth::feedback {
 
struct QPSolverParams
{
  bool verbose = false;
 
  float alpha = 1.6f;
  float rho = 0.1f;
  float sigma = 1e-6f;
 
  bool scaling = true;
 
  float eps_abs = 1e-3f;
  float eps_rel = 1e-3f;
  float eps_primal_inf = 1e-4f;
  float eps_dual_inf = 1e-4f;
 
  std::optional<uint32_t> max_iter = {};
 
  std::optional<std::chrono::nanoseconds> max_time = {};
 
  uint32_t stop_check_iter = 25;
 
  bool polish = true;
  uint32_t polish_iter = 5;
  float delta = 1e-6f;
};
 
namespace detail {
 
template<typename Pbm>
using qp_solution_t = QPSolution<
  decltype(Pbm::A)::RowsAtCompileTime,
  decltype(Pbm::A)::ColsAtCompileTime,
  typename decltype(Pbm::A)::Scalar>;
 
template<typename Pbm, typename D1, typename D2>
bool polish_qp(
  const Pbm & pbm,
  qp_solution_t<Pbm> & sol,
  const QPSolverParams & prm,
  const typename decltype(Pbm::A)::Scalar c,
  const Eigen::MatrixBase<D1> & sx,
  const Eigen::MatrixBase<D2> & sy)
{
  using AmatT                  = decltype(Pbm::A);
  using Scalar                 = typename AmatT::Scalar;
  static constexpr bool sparse = std::is_base_of_v<Eigen::SparseMatrixBase<AmatT>, AmatT>;
 
  static constexpr Scalar inf = std::numeric_limits<Scalar>::infinity();
  static constexpr Scalar eps = std::numeric_limits<Scalar>::epsilon();
 
  static constexpr Eigen::Index N = AmatT::ColsAtCompileTime;
  const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows();
 
  // FIND ACTIVE CONSTRAINT SETS
 
  Eigen::Index nl = 0, nu = 0;
  for (auto idx = 0; idx < m; ++idx) {
    if (sol.dual[idx] < -100 * eps && pbm.l[idx] != -inf) { nl++; }
    if (sol.dual[idx] > 100 * eps && pbm.u[idx] != inf) { nu++; }
  }
 
  Eigen::VectorXi LU_idx(nl + nu);
  for (auto idx = 0, lcntr = 0, ucntr = 0; idx < m; ++idx) {
    if (sol.dual[idx] < -100 * eps && pbm.l[idx] != -inf) { LU_idx(lcntr++) = idx; }
    if (sol.dual[idx] > 100 * eps && pbm.u[idx] != inf) { LU_idx(nl + ucntr++) = idx; }
  }
 
  // FORM REDUCED SYSTEMS (27) AND (30)
 
  // square symmetric system matrix
  using HT = std::conditional_t<sparse, Eigen::SparseMatrix<Scalar>, Eigen::Matrix<Scalar, -1, -1>>;
  HT H(n + nl + nu, n + nl + nu), Hp(n + nl + nu, n + nl + nu);
 
  // fill up H
  if constexpr (sparse) {
    // preallocate nonzeros
    Eigen::VectorXi nnz(n + nl + nu);
    for (auto i = 0u; i != n; ++i) { nnz(i) = pbm.P.outerIndexPtr()[i + 1] - pbm.P.outerIndexPtr()[i]; }
    for (auto i = 0u; i != nl + nu; ++i) {
      nnz(n + i) = pbm.A.outerIndexPtr()[LU_idx(i) + 1] - pbm.A.outerIndexPtr()[LU_idx(i)];
    }
    H.reserve(nnz);
    Hp.reserve(nnz + Eigen::VectorXi::Ones(n + nl + nu));
 
    // fill P in top left block
    for (auto k = 0u; k < n; ++k) {
      for (Eigen::InnerIterator it(pbm.P, k); it; ++it) {
        const Scalar pij              = c * sx(it.col()) * sx(it.row()) * it.value();
        H.insert(it.row(), it.col())  = pij;
        Hp.insert(it.row(), it.col()) = pij;
      }
    }
 
    // fill selected rows of A in top right block
    for (auto a_row = 0u; a_row != nl + nu; ++a_row) {
      for (Eigen::InnerIterator it(pbm.A, LU_idx(a_row)); it; ++it) {
        const Scalar Aij               = sy(it.row()) * sx(it.col()) * it.value();
        H.insert(it.col(), n + a_row)  = Aij;
        Hp.insert(it.col(), n + a_row) = Aij;
      }
    }
  } else {
    H.setZero();
    H.topLeftCorner(n, n) = c * sx.asDiagonal() * pbm.P * sx.asDiagonal();
    for (auto i = 0u; i != nl + nu; ++i) {
      H.col(n + i).template head<N>(n) = sy(LU_idx(i)) * pbm.A.row(LU_idx(i)) * sx.asDiagonal();
    }
    Hp = H;
  }
 
  // add perturbing diagonal elements to Hp
  if constexpr (sparse) {
    for (auto i = 0u; i != n; ++i) { Hp.coeffRef(i, i) += prm.delta; }
    for (auto i = 0u; i != nl + nu; ++i) { Hp.coeffRef(n + i, n + i) -= prm.delta; }
    H.makeCompressed();
    Hp.makeCompressed();
  } else {
    Hp.topLeftCorner(n, n) += Eigen::VectorX<Scalar>::Constant(n, prm.delta).asDiagonal();
    Hp.bottomRightCorner(nl + nu, nl + nu) -= Eigen::VectorX<Scalar>::Constant(nl + nu, prm.delta).asDiagonal();
  }
 
  Eigen::VectorX<Scalar> h(n + nl + nu);
  h.head(n) = -c * sx.cwiseProduct(pbm.q);
  for (auto i = 0u; i != nl; ++i) { h(n + i) = sy(LU_idx(i)) * pbm.l(LU_idx(i)); }
  for (auto i = 0u; i != nu; ++i) { h(n + nl + i) = sy(LU_idx(nl + i)) * pbm.u(LU_idx(nl + i)); }
 
  // ITERATIVE REFINEMENT
 
  // factorize Hp
  std::conditional_t<sparse, Eigen::SimplicialLDLT<decltype(H), Eigen::Upper>, Eigen::LDLT<decltype(H), Eigen::Upper>>
    ldlt(Hp);
 
  if (ldlt.info()) { return false; }
 
  Eigen::VectorX<Scalar> t_hat = Eigen::VectorX<Scalar>::Zero(n + nl + nu);
  for (auto i = 0u; i != prm.polish_iter; ++i) {
    t_hat += ldlt.solve(h - H.template selfadjointView<Eigen::Upper>() * t_hat);
  }
 
  // UPDATE SOLUTION
 
  sol.primal = t_hat.template head<N>(n);
  for (auto i = 0u; i < nl; ++i) { sol.dual(LU_idx(i)) = t_hat(n + i); }
  for (auto i = 0u; i < nu; ++i) { sol.dual(LU_idx(nl + i)) = t_hat(n + nl + i); }
 
  return true;
}
 
template<typename LDLTt>
struct LDLTWrapper
{
  bool first{true};  // true if ldlt object needs factorization
  LDLTt ldlt{};
 
  LDLTWrapper() = default;
  LDLTWrapper(const LDLTWrapper &) : first{true}, ldlt{} {}
  LDLTWrapper(LDLTWrapper &&) noexcept : first{true}, ldlt{} {}
  LDLTWrapper & operator=(const LDLTWrapper & rhs)
  {
    if (this == &rhs) { return *this; }
 
    first = true;
    return *this;
  }
  LDLTWrapper & operator=(LDLTWrapper &&) noexcept
  {
    first = true;
    return *this;
  }
  ~LDLTWrapper() = default;
};
 
}  // namespace detail
 
template<typename Pbm>
class QPSolver
{
  using AmatT                  = decltype(Pbm::A);
  using Scalar                 = typename AmatT::Scalar;
  static constexpr bool sparse = std::is_base_of_v<Eigen::SparseMatrixBase<AmatT>, AmatT>;
 
  // static sizes
  static constexpr Eigen::Index M = AmatT::RowsAtCompileTime;
  static constexpr Eigen::Index N = AmatT::ColsAtCompileTime;
  static constexpr Eigen::Index K = (N == -1 || M == -1) ? Eigen::Index(-1) : N + M;
 
  // typedefs
  using Rn    = Eigen::Vector<Scalar, N>;
  using Rm    = Eigen::Vector<Scalar, M>;
  using Rk    = Eigen::Vector<Scalar, K>;
  using Ht    = std::conditional_t<sparse, Eigen::SparseMatrix<Scalar>, Eigen::Matrix<Scalar, K, K>>;
  using LDLTt = std::conditional_t<sparse, Eigen::SimplicialLDLT<Ht, Eigen::Upper>, Eigen::LDLT<Ht, Eigen::Upper>>;
 
  static inline const Scalar inf = std::numeric_limits<Scalar>::infinity();
 
public:
  QPSolver(const QPSolverParams & prm = {}) : prm_(prm) {}
 
  QPSolver(const Pbm & pbm, const QPSolverParams & prm = {}) : prm_(prm) { analyze(pbm); }
 
  QPSolver(const QPSolver &) = default;
  QPSolver(QPSolver &&) noexcept = default;
  QPSolver & operator=(const QPSolver &) = default;
  QPSolver & operator=(QPSolver &&) noexcept = default;
  ~QPSolver() = default;
 
  const QPSolution<M, N, Scalar> & sol() const { return sol_; }
 
  void analyze(const Pbm & pbm)
  {
    const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows(), k = n + m;
 
    // solution
    sol_.primal.setZero(n);
    sol_.dual.setZero(m);
 
    // scaling variables and working memory
    c_ = 1;
    sx_.setOnes(n);
    sy_.setOnes(m);
    sx_inc_.setZero(n);
    sy_inc_.setZero(m);
 
    // solve working memory
    z_.resize(m);
    z_next_.resize(m);
    rho_.resize(m);
    p_.resize(k);
    pt_.resize(k);
 
    // stopping working memory
    x_us_.resize(n);
    dx_us_.resize(n);
    y_us_.resize(m);
    dy_us_.resize(m);
    z_us_.resize(m);
    Px_.resize(n);
    Aty_.resize(n);
    Ax_.resize(m);
 
    // preallocate nonzeros in H
    H_.setZero();
    H_.resize(k, k);
    if constexpr (sparse) {
      Eigen::VectorXi nnz(k);
      for (auto i = 0u; i < n; ++i) { nnz(i) = pbm.P.outerIndexPtr()[i + 1] - pbm.P.outerIndexPtr()[i] + 1; }
      for (auto i = 0u; i < m; ++i) { nnz(n + i) = pbm.A.outerIndexPtr()[i + 1] - pbm.A.outerIndexPtr()[i] + 1; }
      H_.reserve(nnz);
    }
  }
 
  const QPSolution<M, N, Scalar> &
  solve(const Pbm & pbm, std::optional<std::reference_wrapper<const QPSolution<M, N, Scalar>>> warmstart = {})
  {
    // update problem scaling
    if (prm_.scaling) { scale(pbm); }
 
    // dynamic sizes
    const Eigen::Index n = pbm.A.cols(), m = pbm.A.rows();
 
    // cast parameters to scalar type
    const Scalar rho_bar    = static_cast<Scalar>(prm_.rho);
    const Scalar alpha      = static_cast<Scalar>(prm_.alpha);
    const Scalar alpha_comp = Scalar(1) - alpha;
    const Scalar sigma      = static_cast<Scalar>(prm_.sigma);
 
    // return code: when set algorithm is finished
    std::optional<QPSolutionStatus> ret_code = std::nullopt;
 
    for (auto i = 0u; i != m; ++i) {
      if (pbm.l(i) == inf || pbm.u(i) == -inf || pbm.u(i) - pbm.l(i) < Scalar(0.)) {
        ret_code = QPSolutionStatus::PrimalInfeasible;  // feasible set trivially empty
      }
 
      // set rho depending on constraint type
      if (pbm.l(i) == -inf && pbm.u(i) == inf) {
        rho_(i) = Scalar(1e-6);  // unbounded
      } else if (sy_(i) * std::fabs(pbm.l(i) - pbm.u(i)) < 1e-5) {
        rho_(i) = Scalar(1e3) * rho_bar;  // equality
      } else {
        rho_(i) = rho_bar;  // inequality
      }
    }
 
    const auto t0 = std::chrono::high_resolution_clock::now();
 
    // fill square symmetric system matrix H = [P A'; A 0]
    if constexpr (sparse) {
      if (H_.isCompressed()) { H_.coeffs().setZero(); }
 
      for (auto i = 0u; i < pbm.P.outerSize(); ++i) {
        for (Eigen::InnerIterator it(pbm.P, i); it; ++it) {
          if (it.col() >= it.row()) {
            H_.coeffRef(it.row(), it.col()) = c_ * sx_(it.row()) * sx_(it.col()) * it.value();
          }
        }
      }
      block_add_identity(H_, 0, 0, n, sigma);
      for (auto i = 0u; i < pbm.A.outerSize(); ++i) {
        for (Eigen::InnerIterator it(pbm.A, i); it; ++it) {
          H_.coeffRef(it.col(), n + it.row()) = sy_(it.row()) * sx_(it.col()) * it.value();
        }
      }
      for (auto row = 0u; row < m; ++row) { H_.coeffRef(n + row, n + row) = Scalar(-1) / rho_(row); }
 
      if (!H_.isCompressed()) { H_.makeCompressed(); }
    } else {
      H_.setZero();
 
      H_.template topLeftCorner<N, N>(n, n) = c_ * sx_.asDiagonal() * pbm.P * sx_.asDiagonal();
      H_.template topLeftCorner<N, N>(n, n) += Eigen::Vector<Scalar, N>::Constant(n, sigma).asDiagonal();
      H_.template topRightCorner<N, M>(n, m)    = (sy_.asDiagonal() * pbm.A * sx_.asDiagonal()).transpose();
      H_.template bottomRightCorner<M, M>(m, m) = (-rho_).cwiseInverse().asDiagonal();
    }
 
    const auto t_fill = std::chrono::high_resolution_clock::now();
 
    if (prm_.verbose) {
      using std::cout, std::left, std::setw, std::right;
      // clang-format off
      cout << "========================= QP Solver =========================" << '\n';
      cout << "Solving " << (sparse ? "sparse" : "dense") << " QP with n=" << n << ", m=" << m << '\n';
      cout << setw(8)  << right << "ITER"
           << setw(14) << right << "OBJ"
           << setw(14) << right << "PRI_RES"
           << setw(14) << right << "DUA_RES"
           << setw(10) << right << "TIME" << '\n';
      // clang-format on
    }
 
    // factorize H
    if constexpr (sparse) {
      if (ldlt_.first) { ldlt_.ldlt.analyzePattern(H_); }
      ldlt_.ldlt.factorize(H_);  // makes copy for permuted matrix..
      ldlt_.first = false;
    } else {
      ldlt_.ldlt.compute(H_);
    }
 
    const auto t_factor = std::chrono::high_resolution_clock::now();
 
    if (ldlt_.ldlt.info()) { ret_code = QPSolutionStatus::Unknown; }
 
    // initialize solver variables
    if (warmstart.has_value()) {
      // warmstart variables must be scaled
      sol_.primal  = sx_.cwiseInverse().cwiseProduct(warmstart.value().get().primal);
      sol_.dual    = c_ * sy_.cwiseInverse().cwiseProduct(warmstart.value().get().dual);
      z_.noalias() = sy_.asDiagonal() * pbm.A * warmstart.value().get().primal;
    } else {
      sol_.primal.setZero();
      sol_.dual.setZero();
      z_.setZero();
    }
 
    // main optimization loop
    auto iter = 0u;
    for (; (!prm_.max_iter || iter != prm_.max_iter.value()) && !ret_code; ++iter) {
      p_.template segment<N>(0, n) = sigma * sol_.primal - c_ * sx_.asDiagonal() * pbm.q;
      p_.template segment<M>(n, m) = z_ - rho_.cwiseInverse().cwiseProduct(sol_.dual);
      if constexpr (sparse) {
        // p_ = ldlt_.solve(p_);
        // manual solve because ldlt_.solve() uses temporaries...
        // Pinv L D L' P x = b  <==> x = Pinv * (L' \ Dinv(L \ (P * b)))
        pt_.noalias() = ldlt_.ldlt.permutationP() * p_;
        ldlt_.ldlt.matrixL().solveInPlace(pt_);
        pt_.applyOnTheLeft(ldlt_.ldlt.vectorD().cwiseInverse().asDiagonal());
        ldlt_.ldlt.matrixU().solveInPlace(pt_);
        p_.noalias() = ldlt_.ldlt.permutationPinv() * pt_;
      } else {
        ldlt_.ldlt.solveInPlace(p_);
      }
 
      if (iter % prm_.stop_check_iter == 1) {
        // termination checking requires difference, store old scaled values
        dx_us_ = sol_.primal, dy_us_ = sol_.dual;
      }
 
      sol_.primal = alpha * p_.template segment<N>(0, n) + alpha_comp * sol_.primal;
      z_next_     = (alpha * rho_.cwiseInverse().cwiseProduct(p_.template segment<M>(n, m))
                 + alpha_comp * rho_.cwiseInverse().cwiseProduct(sol_.dual) + z_)
                  .cwiseMax(sy_.cwiseProduct(pbm.l))
                  .cwiseMin(sy_.cwiseProduct(pbm.u));
      sol_.dual = alpha_comp * sol_.dual + alpha * p_.template segment<M>(n, m) + rho_.cwiseProduct(z_)
                - rho_.cwiseProduct(z_next_);
      std::swap(z_, z_next_);
 
      if (iter % prm_.stop_check_iter == 1) {
        // unscale solution
        x_us_  = sx_.cwiseProduct(sol_.primal);
        y_us_  = sy_.cwiseProduct(sol_.dual) / c_;
        z_us_  = sy_.cwiseInverse().cwiseProduct(z_);
        dx_us_ = sx_.cwiseProduct(sol_.primal - dx_us_);
        dy_us_ = sy_.cwiseProduct(sol_.dual - dy_us_) / c_;
 
        // check stopping criteria for unscaled problem and unscaled variables
        ret_code = check_stopping(pbm);
 
        if (prm_.verbose) {
          using std::cout, std::setw, std::right, std::chrono::microseconds;
          // clang-format off
          cout << setw(7) << right << iter << ":"
            << std::scientific
            << setw(14) << right << (0.5 * pbm.P * x_us_ + pbm.q).dot(x_us_)
            << setw(14) << right << (pbm.A * x_us_ - z_us_).template lpNorm<Eigen::Infinity>()
            << setw(14) << right << (pbm.P * x_us_ + pbm.q + pbm.A.transpose() * y_us_).template lpNorm<Eigen::Infinity>()
            << setw(10) << right << duration_cast<microseconds>(std::chrono::high_resolution_clock::now() - t0).count()
            << '\n';
          // clang-format on
        }
 
        // check for timeout
        if (!ret_code) {
          if (prm_.max_time && std::chrono::high_resolution_clock::now() > t0 + prm_.max_time.value()) {
            ret_code = QPSolutionStatus::MaxTime;
          }
        }
      }
    }
 
    const auto t_iter = std::chrono::high_resolution_clock::now();
 
    // polish solution if optimal
    if (ret_code.has_value() && ret_code.value() == QPSolutionStatus::Optimal && prm_.polish) {
      if (detail::polish_qp(pbm, sol_, prm_, c_, sx_, sy_)) {
        if (prm_.verbose) {
          // unscale solution
          x_us_ = sx_.cwiseProduct(sol_.primal);
          y_us_ = sy_.cwiseProduct(sol_.dual) / c_;
          z_us_ = sy_.cwiseInverse().cwiseProduct(z_);
 
          using std::cout, std::setw, std::right, std::chrono::microseconds;
          // clang-format off
          cout << setw(8) << right << "polish:"
            << std::scientific
            << setw(14) << right << (0.5 * pbm.P * x_us_ + pbm.q).dot(x_us_)
            << setw(14) << right << (pbm.A * x_us_ - z_us_).template lpNorm<Eigen::Infinity>()
            << setw(14) << right << (pbm.P * x_us_ + pbm.q + pbm.A.transpose() * y_us_).template lpNorm<Eigen::Infinity>()
            << setw(10) << right << duration_cast<microseconds>(std::chrono::high_resolution_clock::now() - t0).count()
            << '\n';
          // clang-format on
        }
 
      } else {
        if (prm_.verbose) { std::cout << "Polish failed" << '\n'; }
        sol_.code = QPSolutionStatus::PolishFailed;
      }
    }
 
    const auto t_polish = std::chrono::high_resolution_clock::now();
 
    // unscale solution
    sol_.code      = ret_code.value_or(QPSolutionStatus::MaxIterations);
    sol_.primal    = sx_.cwiseProduct(sol_.primal);
    sol_.dual      = sy_.cwiseProduct(sol_.dual) / c_;
    sol_.objective = sol_.primal.dot(0.5 * pbm.P * sol_.primal + pbm.q);
    sol_.iter      = iter;
 
    if (prm_.verbose) {
      using std::cout, std::left, std::right, std::setw, std::chrono::microseconds;
 
      // clang-format off
      cout << "QP solver summary:" << '\n';
      cout << "Result " << static_cast<int>(sol_.code) << '\n';
 
      cout << setw(25) << left << "Iterations"        << setw(10) << right << iter - 1                                               << '\n';
      cout << setw(26) << left << "Total time (µs)"   << setw(10) << right << duration_cast<microseconds>(t_polish - t0).count()     << '\n';
      cout << setw(25) << left << "  Matrix filling"  << setw(10) << right << duration_cast<microseconds>(t_fill - t0).count()       << '\n';
      cout << setw(25) << left << "  Factorization"   << setw(10) << right << duration_cast<microseconds>(t_factor - t_fill).count() << '\n';
      cout << setw(25) << left << "  Iteration"       << setw(10) << right << duration_cast<microseconds>(t_iter - t_factor).count() << '\n';
      cout << setw(25) << left << "  Polish"          << setw(10) << right << duration_cast<microseconds>(t_polish - t_iter).count() << '\n';
      cout << "=============================================================" << '\n';
      // clang-format on
    }
 
    return sol_;
  }
 
protected:
  std::optional<QPSolutionStatus> check_stopping(const Pbm & pbm)
  {
    const Eigen::Index m = pbm.A.rows();
 
    // norm function
    static const auto norm = [](auto && t) -> Scalar { return t.template lpNorm<Eigen::Infinity>(); };
 
    // OPTIMALITY
 
    // check primal
    Ax_.noalias()        = pbm.A * x_us_;
    const Scalar Ax_norm = norm(Ax_);
    Ax_ -= z_us_;
    if (norm(Ax_) <= prm_.eps_abs + prm_.eps_rel * std::max<Scalar>(Ax_norm, norm(z_us_))) {
      // primal succeeded, check dual
      Px_.noalias()           = pbm.P * x_us_;
      Aty_.noalias()          = pbm.A.transpose() * y_us_;
      const Scalar dual_scale = std::max<Scalar>({norm(Px_), norm(pbm.q), norm(Aty_)});
      Px_ += pbm.q + Aty_;
      if (norm(Px_) <= prm_.eps_abs + prm_.eps_rel * dual_scale) { return QPSolutionStatus::Optimal; }
    }
 
    // PRIMAL INFEASIBILITY
 
    Aty_.noalias()        = pbm.A.transpose() * dy_us_;  // NOTE new value A' * dy
    const Scalar Edy_norm = norm(dy_us_);
 
    Scalar u_dyp_plus_l_dyn = Scalar(0);
    for (auto i = 0u; i != m; ++i) {
      if (pbm.u(i) != inf) {
        u_dyp_plus_l_dyn += pbm.u(i) * std::max<Scalar>(Scalar(0), dy_us_(i));
      } else if (dy_us_(i) > prm_.eps_primal_inf * Edy_norm) {
        // contributes +inf to sum --> no certificate
        u_dyp_plus_l_dyn = inf;
        break;
      }
      if (pbm.l(i) != -inf) {
        u_dyp_plus_l_dyn += pbm.l(i) * std::min<Scalar>(Scalar(0), dy_us_(i));
      } else if (dy_us_(i) < -prm_.eps_primal_inf * Edy_norm) {
        // contributes +inf to sum --> no certificate
        u_dyp_plus_l_dyn = inf;
        break;
      }
    }
 
    if (std::max<Scalar>(norm(Aty_), u_dyp_plus_l_dyn) < prm_.eps_primal_inf * Edy_norm) {
      return QPSolutionStatus::PrimalInfeasible;
    }
 
    // DUAL INFEASIBILITY
 
    Ax_.noalias()        = pbm.A * dx_us_;  // note new value A * dx
    const Scalar dx_norm = norm(dx_us_);
    Px_.noalias()        = pbm.P * dx_us_;
 
    bool dual_infeasible =
      (norm(Px_) <= prm_.eps_dual_inf * dx_norm) && (pbm.q.dot(dx_us_) <= prm_.eps_dual_inf * dx_norm);
    for (auto i = 0u; i != m && dual_infeasible; ++i) {
      if (pbm.u(i) == inf) {
        dual_infeasible &= (Ax_(i) >= -prm_.eps_dual_inf * dx_norm);
      } else if (pbm.l(i) == -inf) {
        dual_infeasible &= (Ax_(i) <= prm_.eps_dual_inf * dx_norm);
      } else {
        dual_infeasible &= std::fabs(Ax_(i)) < prm_.eps_dual_inf * dx_norm;
      }
    }
 
    if (dual_infeasible) { return QPSolutionStatus::DualInfeasible; }
 
    return std::nullopt;
  }
 
  void scale(const Pbm & pbm)
  {
    sx_.setOnes();
    sy_.setOnes();
 
    sx_inc_.setZero();
 
    // find "norm" of cost function
    for (auto i = 0u; i < pbm.P.outerSize(); ++i) {
      for (Eigen::InnerIterator it(pbm.P, i); it; ++it) {
        sx_inc_(it.col()) = std::max(sx_inc_(it.col()), std::fabs(it.value()));
      }
    }
 
    // if there are "zero cols"
    for (auto i = 0u; i != sx_.size(); ++i) {
      if (sx_inc_(i) == 0) { sx_inc_(i) = 1; }
    }
 
    // scale cost function
    c_ = Scalar(1) / std::max({1e-6, sx_inc_.mean(), pbm.q.template lpNorm<Eigen::Infinity>()});
 
    int iter = 0;
 
    // calculate inf-norm for every column of [Ps As' ; As 0]
    do {
      sx_inc_.setZero();
      sy_inc_.setZero();
      for (auto k = 0u; k < pbm.P.outerSize(); ++k) {
        for (Eigen::InnerIterator it(pbm.P, k); it; ++it) {
          // upper left block of H
          sx_inc_(it.col()) = std::max({
            sx_inc_(it.col()),
            std::fabs(c_ * sx_(it.row()) * sx_(it.col()) * it.value()),
          });
        }
      }
      for (auto k = 0u; k < pbm.A.outerSize(); ++k) {
        for (Eigen::InnerIterator it(pbm.A, k); it; ++it) {
          const Scalar Aij  = std::fabs(sy_(it.row()) * sx_(it.col()) * it.value());
          sx_inc_(it.col()) = std::max(sx_inc_(it.col()), Aij);  // bottom left block of H
          sy_inc_(it.row()) = std::max(sy_inc_(it.row()), Aij);  // upper right block of H
        }
      }
 
      // if there are "zero cols" we don't scale
      for (auto i = 0u; i < sx_.size(); ++i) {
        if (sx_inc_(i) == 0) { sx_inc_(i) = 1; }
      }
      for (auto i = 0u; i < sy_.size(); ++i) {
        if (sy_inc_(i) == 0) { sy_inc_(i) = 1; }
      }
 
      sx_.applyOnTheLeft(sx_inc_.cwiseMax(1e-8).cwiseInverse().cwiseSqrt().asDiagonal());
      sy_.applyOnTheLeft(sy_inc_.cwiseMax(1e-8).cwiseInverse().cwiseSqrt().asDiagonal());
    } while (iter++ < 10
             && std::max((sx_inc_.array() - 1).abs().maxCoeff(), (sy_inc_.array() - 1).abs().maxCoeff()) > 0.1);
  }
 
private:
  // solver parameters
  QPSolverParams prm_{};
 
  // solution
  QPSolution<M, N, Scalar> sol_{};
 
  // scaling variables and working memory
  Scalar c_{0};
  Rn sx_{}, sx_inc_{};
  Rm sy_{}, sy_inc_{};
 
  // solve working memory
  Rm z_{}, z_next_{}, rho_{};
  Rk p_{}, pt_{};
 
  // stopping working memory
  Rn x_us_{}, dx_us_{};
  Rn Px_{}, Aty_{};
  Rm Ax_{};
  Rm y_us_{}, z_us_{}, dy_us_{};
 
  // system matrix and decomposition
  Ht H_{};
  detail::LDLTWrapper<LDLTt> ldlt_{};
};
 
template<typename Pbm>
detail::qp_solution_t<Pbm> solve_qp(
  const Pbm & pbm,
  const QPSolverParams & prm,
  std::optional<std::reference_wrapper<const detail::qp_solution_t<Pbm>>> warmstart = {})
{
  QPSolver solver(pbm, prm);
  return solver.solve(pbm, warmstart);
}
 
}  // namespace smooth::feedback