wrapper.hpp

Go to the documentation of this file.

//
// Copyright (c) 2022 INRIA
//
#ifndef PROXSUITE_PROXQP_SPARSE_WRAPPER_HPP
#define PROXSUITE_PROXQP_SPARSE_WRAPPER_HPP
#include <proxsuite/proxqp/results.hpp>
#include <proxsuite/proxqp/settings.hpp>
#include <proxsuite/proxqp/sparse/solver.hpp>
#include <proxsuite/proxqp/sparse/helpers.hpp>
 
namespace proxsuite {
namespace proxqp {
namespace sparse {
 
template<typename T, typename I>
struct QP
{
  Results<T> results;
  Settings<T> settings;
  Model<T, I> model;
  Workspace<T, I> work;
  preconditioner::RuizEquilibration<T, I> ruiz;
  QP(isize dim, isize n_eq, isize n_in)
    : results(dim, n_eq, n_in)
    , settings()
    , model(dim, n_eq, n_in)
    , work()
    , ruiz(dim, n_eq + n_in, 1e-3, 10, preconditioner::Symmetry::UPPER)
  {
    work.timer.stop();
    work.internal.do_symbolic_fact = true;
    work.internal.is_initialized = false;
  }
  QP(const SparseMat<bool, I>& H,
     const SparseMat<bool, I>& A,
     const SparseMat<bool, I>& C)
    : QP(H.rows(), A.rows(), C.rows())
  {
    if (settings.compute_timings) {
      work.timer.stop();
      work.timer.start();
    }
    SparseMat<bool, I> H_triu = H.template triangularView<Eigen::Upper>();
    SparseMat<bool, I> AT = A.transpose();
    SparseMat<bool, I> CT = C.transpose();
    proxsuite::linalg::sparse::MatRef<bool, I> Href = {
      proxsuite::linalg::sparse::from_eigen, H_triu
    };
    proxsuite::linalg::sparse::MatRef<bool, I> ATref = {
      proxsuite::linalg::sparse::from_eigen, AT
    };
    proxsuite::linalg::sparse::MatRef<bool, I> CTref = {
      proxsuite::linalg::sparse::from_eigen, CT
    };
    work.setup_symbolic_factorizaton(
      model, Href.symbolic(), ATref.symbolic(), CTref.symbolic());
    if (settings.compute_timings) {
      results.info.setup_time = work.timer.elapsed().user; // in microseconds
    }
  }
 
  void init(optional<SparseMat<T, I>> H,
            optional<VecRef<T>> g,
            optional<SparseMat<T, I>> A,
            optional<VecRef<T>> b,
            optional<SparseMat<T, I>> C,
            optional<VecRef<T>> l,
            optional<VecRef<T>> u,
            bool compute_preconditioner_ = true,
            optional<T> rho = nullopt,
            optional<T> mu_eq = nullopt,
            optional<T> mu_in = nullopt,
            optional<T> manual_minimal_H_eigenvalue = nullopt)
  {
    if (settings.compute_timings) {
      work.timer.stop();
      work.timer.start();
    }
    if (g != nullopt && g.value().size() != 0) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        g.value().size(),
        model.dim,
        "the dimension wrt the primal variable x variable for initializing g "
        "is not valid.");
    } else {
      g.reset();
    }
    if (b != nullopt && b.value().size() != 0) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        b.value().size(),
        model.n_eq,
        "the dimension wrt equality constrained variables for initializing b "
        "is not valid.");
    } else {
      b.reset();
    }
    if (u != nullopt && u.value().size() != 0) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        u.value().size(),
        model.n_in,
        "the dimension wrt inequality constrained variables for initializing u "
        "is not valid.");
    } else {
      u.reset();
    }
    if (l != nullopt && l.value().size() != 0) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        l.value().size(),
        model.n_in,
        "the dimension wrt inequality constrained variables for initializing l "
        "is not valid.");
    } else {
      l.reset();
    }
    if (H != nullopt && H.value().size() != 0) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        H.value().rows(),
        model.dim,
        "the row dimension for initializing H is not valid.");
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        H.value().cols(),
        model.dim,
        "the column dimension for initializing H is not valid.");
    } else {
      H.reset();
    }
    if (A != nullopt && A.value().size() != 0) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        A.value().rows(),
        model.n_eq,
        "the row dimension for initializing A is not valid.");
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        A.value().cols(),
        model.dim,
        "the column dimension for initializing A is not valid.");
    } else {
      A.reset();
    }
    if (C != nullopt && C.value().size() != 0) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        C.value().rows(),
        model.n_in,
        "the row dimension for initializing C is not valid.");
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        C.value().cols(),
        model.dim,
        "the column dimension for initializing C is not valid.");
    } else {
      C.reset();
    }
    work.internal.proximal_parameter_update = false;
    PreconditionerStatus preconditioner_status;
    if (compute_preconditioner_) {
      preconditioner_status = proxsuite::proxqp::PreconditionerStatus::EXECUTE;
    } else {
      preconditioner_status = proxsuite::proxqp::PreconditionerStatus::IDENTITY;
    }
    proxsuite::proxqp::sparse::update_proximal_parameters(
      settings, results, work, rho, mu_eq, mu_in);
 
    if (g != nullopt) {
      model.g = g.value();
    } // else qpmodel.g remains initialzed to a matrix with zero elements or
      // zero shape
    if (b != nullopt) {
      model.b = b.value();
    } // else qpmodel.b remains initialzed to a matrix with zero elements or
      // zero shape
    if (u != nullopt) {
      model.u = u.value();
    } // else qpmodel.u remains initialzed to a matrix with zero elements or
      // zero shape
    if (l != nullopt) {
      model.l = l.value();
    } // else qpmodel.l remains initialzed to a matrix with zero elements or
      // zero shape
 
    // avoid allocations when H is not nullopt
    SparseMat<T, I> AT(model.dim, model.n_eq);
    if (A != nullopt) {
      AT = (A.value()).transpose();
    } else {
      AT.setZero();
    }
    SparseMat<T, I> CT(model.dim, model.n_in);
    if (C != nullopt) {
      CT = (C.value()).transpose();
    } else {
      CT.setZero();
    }
    if (H != nullopt) {
      SparseMat<T, I> H_triu =
        (H.value()).template triangularView<Eigen::Upper>();
 
      sparse::QpView<T, I> qp = {
        { proxsuite::linalg::sparse::from_eigen, H_triu },
        { proxsuite::linalg::sparse::from_eigen, model.g },
        { proxsuite::linalg::sparse::from_eigen, AT },
        { proxsuite::linalg::sparse::from_eigen, model.b },
        { proxsuite::linalg::sparse::from_eigen, CT },
        { proxsuite::linalg::sparse::from_eigen, model.l },
        { proxsuite::linalg::sparse::from_eigen, model.u }
      };
      qp_setup(qp, results, model, work, settings, ruiz, preconditioner_status);
      proxsuite::proxqp::sparse::
        update_default_rho_with_minimal_Hessian_eigen_value(
          manual_minimal_H_eigenvalue, results, settings);
    } else {
      SparseMat<T, I> H_triu(model.dim, model.dim);
      H_triu.setZero();
      H_triu = (H.value()).template triangularView<Eigen::Upper>();
      sparse::QpView<T, I> qp = {
        { proxsuite::linalg::sparse::from_eigen, H_triu },
        { proxsuite::linalg::sparse::from_eigen, model.g },
        { proxsuite::linalg::sparse::from_eigen, AT },
        { proxsuite::linalg::sparse::from_eigen, model.b },
        { proxsuite::linalg::sparse::from_eigen, CT },
        { proxsuite::linalg::sparse::from_eigen, model.l },
        { proxsuite::linalg::sparse::from_eigen, model.u }
      };
      qp_setup(qp, results, model, work, settings, ruiz, preconditioner_status);
    }
    work.internal.is_initialized = true;
 
    if (settings.compute_timings) {
      results.info.setup_time += work.timer.elapsed().user; // in microseconds
    }
  };
  void update(const optional<SparseMat<T, I>> H,
              optional<VecRef<T>> g,
              const optional<SparseMat<T, I>> A,
              optional<VecRef<T>> b,
              const optional<SparseMat<T, I>> C,
              optional<VecRef<T>> l,
              optional<VecRef<T>> u,
              bool update_preconditioner = false,
              optional<T> rho = nullopt,
              optional<T> mu_eq = nullopt,
              optional<T> mu_in = nullopt,
              optional<T> manual_minimal_H_eigenvalue = nullopt)
  {
    if (!work.internal.is_initialized) {
      init(H, g, A, b, C, l, u, update_preconditioner, rho, mu_eq, mu_in);
      return;
    }
    if (settings.compute_timings) {
      work.timer.stop();
      work.timer.start();
    }
    work.internal.dirty = false;
    work.internal.proximal_parameter_update = false;
    PreconditionerStatus preconditioner_status;
    if (update_preconditioner) {
      preconditioner_status = proxsuite::proxqp::PreconditionerStatus::EXECUTE;
    } else {
      preconditioner_status = proxsuite::proxqp::PreconditionerStatus::KEEP;
    }
    isize n = model.dim;
    isize n_eq = model.n_eq;
    isize n_in = model.n_in;
    proxsuite::linalg::sparse::MatMut<T, I> kkt_unscaled =
      model.kkt_mut_unscaled();
 
    auto kkt_top_n_rows = detail::top_rows_mut_unchecked(
      proxsuite::linalg::veg::unsafe, kkt_unscaled, n);
 
    proxsuite::linalg::sparse::MatMut<T, I> H_unscaled =
      detail::middle_cols_mut(kkt_top_n_rows, 0, n, model.H_nnz);
 
    proxsuite::linalg::sparse::MatMut<T, I> AT_unscaled =
      detail::middle_cols_mut(kkt_top_n_rows, n, n_eq, model.A_nnz);
 
    proxsuite::linalg::sparse::MatMut<T, I> CT_unscaled =
      detail::middle_cols_mut(kkt_top_n_rows, n + n_eq, n_in, model.C_nnz);
 
    // check the model is valid
    if (g != nullopt) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(g.value().size(),
                                    model.dim,
                                    "the dimension wrt the primal variable x "
                                    "variable for updating g is not valid.");
    }
    if (b != nullopt) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(b.value().size(),
                                    model.n_eq,
                                    "the dimension wrt equality constrained "
                                    "variables for updating b is not valid.");
    }
    if (u != nullopt) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(u.value().size(),
                                    model.n_in,
                                    "the dimension wrt inequality constrained "
                                    "variables for updating u is not valid.");
    }
    if (l != nullopt) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(l.value().size(),
                                    model.n_in,
                                    "the dimension wrt inequality constrained "
                                    "variables for updating l is not valid.");
    }
    if (H != nullopt) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        H.value().rows(),
        model.dim,
        "the row dimension for updating H is not valid.");
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        H.value().cols(),
        model.dim,
        "the column dimension for updating H is not valid.");
    }
    if (A != nullopt) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        A.value().rows(),
        model.n_eq,
        "the row dimension for updating A is not valid.");
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        A.value().cols(),
        model.dim,
        "the column dimension for updating A is not valid.");
    }
    if (C != nullopt) {
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        C.value().rows(),
        model.n_in,
        "the row dimension for updating C is not valid.");
      PROXSUITE_CHECK_ARGUMENT_SIZE(
        C.value().cols(),
        model.dim,
        "the column dimension for updating C is not valid.");
    }
 
    // update the model
 
    if (g != nullopt) {
      model.g = g.value();
    }
    if (b != nullopt) {
      model.b = b.value();
    }
    if (u != nullopt) {
      model.u = u.value();
    }
    if (l != nullopt) {
      model.l = l.value();
    }
    if (H != nullopt) {
      SparseMat<T, I> H_triu =
        H.value().template triangularView<Eigen::Upper>();
      if (A != nullopt) {
        if (C != nullopt) {
          bool res =
            have_same_structure(
              H_unscaled.as_const(),
              { proxsuite::linalg::sparse::from_eigen, H_triu }) &&
            have_same_structure(AT_unscaled.as_const(),
                                { proxsuite::linalg::sparse::from_eigen,
                                  SparseMat<T, I>(A.value().transpose()) }) &&
            have_same_structure(CT_unscaled.as_const(),
                                { proxsuite::linalg::sparse::from_eigen,
                                  SparseMat<T, I>(C.value().transpose()) });
          /* TO PUT IN DEBUG MODE
          std::cout << "have same structure = " << res << std::endl;
          */
          if (res) {
            copy(H_unscaled,
                 { proxsuite::linalg::sparse::from_eigen,
                   H_triu }); // copy rhs into lhs
            copy(
              AT_unscaled,
              { proxsuite::linalg::sparse::from_eigen,
                SparseMat<T, I>(A.value().transpose()) }); // copy rhs into lhs
            copy(
              CT_unscaled,
              { proxsuite::linalg::sparse::from_eigen,
                SparseMat<T, I>(C.value().transpose()) }); // copy rhs into lhs
          }
        } else {
          bool res =
            have_same_structure(
              H_unscaled.as_const(),
              { proxsuite::linalg::sparse::from_eigen, H_triu }) &&
            have_same_structure(AT_unscaled.as_const(),
                                { proxsuite::linalg::sparse::from_eigen,
                                  SparseMat<T, I>(A.value().transpose()) });
          /* TO PUT IN DEBUG MODE
          std::cout << "have same structure = " << res << std::endl;
          */
          if (res) {
            copy(H_unscaled,
                 { proxsuite::linalg::sparse::from_eigen,
                   H_triu }); // copy rhs into lhs
            copy(
              AT_unscaled,
              { proxsuite::linalg::sparse::from_eigen,
                SparseMat<T, I>(A.value().transpose()) }); // copy rhs into lhs
          }
        }
      } else if (C != nullopt) {
        bool res =
          have_same_structure(
            H_unscaled.as_const(),
            { proxsuite::linalg::sparse::from_eigen, H_triu }) &&
          have_same_structure(CT_unscaled.as_const(),
                              { proxsuite::linalg::sparse::from_eigen,
                                SparseMat<T, I>(C.value().transpose()) });
        /* TO PUT IN DEBUG MODE
        std::cout << "have same structure = " << res << std::endl;
        */
        if (res) {
          copy(H_unscaled,
               { proxsuite::linalg::sparse::from_eigen,
                 H_triu }); // copy rhs into lhs
          copy(CT_unscaled,
               { proxsuite::linalg::sparse::from_eigen,
                 SparseMat<T, I>(C.value().transpose()) }); // copy rhs into lhs
        }
      } else {
 
        bool res = have_same_structure(
          H_unscaled.as_const(),
          { proxsuite::linalg::sparse::from_eigen, H_triu });
        /* TO PUT IN DEBUG MODE
        std::cout << "have same structure = " << res << std::endl;
        */
        if (res) {
          copy(H_unscaled,
               { proxsuite::linalg::sparse::from_eigen,
                 H.value() }); // copy rhs into lhs
        }
      }
    } else if (A != nullopt) {
      if (C != nullopt) {
        bool res =
          have_same_structure(AT_unscaled.as_const(),
                              { proxsuite::linalg::sparse::from_eigen,
                                SparseMat<T, I>(A.value().transpose()) }) &&
          have_same_structure(CT_unscaled.as_const(),
                              { proxsuite::linalg::sparse::from_eigen,
                                SparseMat<T, I>(C.value().transpose()) });
        /* TO PUT IN DEBUG MODE
        std::cout << "have same structure = " << res << std::endl;
        */
        if (res) {
          copy(AT_unscaled,
               { proxsuite::linalg::sparse::from_eigen,
                 SparseMat<T, I>(A.value().transpose()) }); // copy rhs into lhs
          copy(CT_unscaled,
               { proxsuite::linalg::sparse::from_eigen,
                 SparseMat<T, I>(C.value().transpose()) }); // copy rhs into lhs
        }
      } else {
        bool res =
          have_same_structure(AT_unscaled.as_const(),
                              { proxsuite::linalg::sparse::from_eigen,
                                SparseMat<T, I>(A.value().transpose()) });
        /* TO PUT IN DEBUG MODE
        std::cout << "have same structure = " << res << std::endl;
        */
        if (res) {
          copy(AT_unscaled,
               { proxsuite::linalg::sparse::from_eigen,
                 SparseMat<T, I>(A.value().transpose()) }); // copy rhs into lhs
        }
      }
    } else if (C != nullopt) {
      bool res =
        have_same_structure(CT_unscaled.as_const(),
                            { proxsuite::linalg::sparse::from_eigen,
                              SparseMat<T, I>(C.value().transpose()) });
      /* TO PUT IN DEBUG MODE
      std::cout << "have same structure = " << res << std::endl;
      */
      if (res) {
        copy(CT_unscaled,
             { proxsuite::linalg::sparse::from_eigen,
               SparseMat<T, I>(C.value().transpose()) }); // copy rhs into lhs
      }
    }
 
    SparseMat<T, I> H_triu =
      H_unscaled.to_eigen().template triangularView<Eigen::Upper>();
    sparse::QpView<T, I> qp = {
      { proxsuite::linalg::sparse::from_eigen, H_triu },
      { proxsuite::linalg::sparse::from_eigen, model.g },
      { proxsuite::linalg::sparse::from_eigen, AT_unscaled.to_eigen() },
      { proxsuite::linalg::sparse::from_eigen, model.b },
      { proxsuite::linalg::sparse::from_eigen, CT_unscaled.to_eigen() },
      { proxsuite::linalg::sparse::from_eigen, model.l },
      { proxsuite::linalg::sparse::from_eigen, model.u }
    };
    proxsuite::proxqp::sparse::update_proximal_parameters(
      settings, results, work, rho, mu_eq, mu_in);
    qp_setup(qp,
             results,
             model,
             work,
             settings,
             ruiz,
             preconditioner_status); // store model value + performs scaling
                                     // according to chosen options
    if (H != nullopt) {
      proxsuite::proxqp::sparse::
        update_default_rho_with_minimal_Hessian_eigen_value(
          manual_minimal_H_eigenvalue, results, settings);
    }
    if (settings.compute_timings) {
      results.info.setup_time = work.timer.elapsed().user; // in microseconds
    }
  };
 
  void solve()
  {
    qp_solve( //
      results,
      model,
      settings,
      work,
      ruiz);
  };
  void solve(optional<VecRef<T>> x,
             optional<VecRef<T>> y,
             optional<VecRef<T>> z)
  {
    proxsuite::proxqp::sparse::warm_start(x, y, z, results, settings, model);
    qp_solve( //
      results,
      model,
      settings,
      work,
      ruiz);
  };
  void cleanup() { results.cleanup(settings); }
};
template<typename T, typename I>
proxqp::Results<T>
solve(
  optional<SparseMat<T, I>> H,
  optional<VecRef<T>> g,
  optional<SparseMat<T, I>> A,
  optional<VecRef<T>> b,
  optional<SparseMat<T, I>> C,
  optional<VecRef<T>> l,
  optional<VecRef<T>> u,
  optional<VecRef<T>> x = nullopt,
  optional<VecRef<T>> y = nullopt,
  optional<VecRef<T>> z = nullopt,
  optional<T> eps_abs = nullopt,
  optional<T> eps_rel = nullopt,
  optional<T> rho = nullopt,
  optional<T> mu_eq = nullopt,
  optional<T> mu_in = nullopt,
  optional<bool> verbose = nullopt,
  bool compute_preconditioner = true,
  bool compute_timings = false,
  optional<isize> max_iter = nullopt,
  proxsuite::proxqp::InitialGuessStatus initial_guess =
    proxsuite::proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS,
  proxsuite::proxqp::SparseBackend sparse_backend =
    proxsuite::proxqp::SparseBackend::Automatic,
  bool check_duality_gap = false,
  optional<T> eps_duality_gap_abs = nullopt,
  optional<T> eps_duality_gap_rel = nullopt,
  bool primal_infeasibility_solving = false,
  optional<T> manual_minimal_H_eigenvalue = nullopt)
{
 
  isize n(0);
  isize n_eq(0);
  isize n_in(0);
  if (H != nullopt) {
    n = H.value().rows();
  }
  if (A != nullopt) {
    n_eq = A.value().rows();
  }
  if (C != nullopt) {
    n_in = C.value().rows();
  }
 
  proxqp::sparse::QP<T, I> Qp(n, n_eq, n_in);
  Qp.settings.initial_guess = initial_guess;
  Qp.settings.check_duality_gap = check_duality_gap;
 
  if (eps_abs != nullopt) {
    Qp.settings.eps_abs = eps_abs.value();
  }
  if (eps_rel != nullopt) {
    Qp.settings.eps_rel = eps_rel.value();
  }
  if (verbose != nullopt) {
    Qp.settings.verbose = verbose.value();
  }
  if (max_iter != nullopt) {
    Qp.settings.max_iter = max_iter.value();
  }
  if (eps_duality_gap_abs != nullopt) {
    Qp.settings.eps_duality_gap_abs = eps_duality_gap_abs.value();
  }
  if (eps_duality_gap_rel != nullopt) {
    Qp.settings.eps_duality_gap_rel = eps_duality_gap_rel.value();
  }
  Qp.settings.compute_timings = compute_timings;
  Qp.settings.sparse_backend = sparse_backend;
  Qp.settings.primal_infeasibility_solving = primal_infeasibility_solving;
  if (manual_minimal_H_eigenvalue != nullopt) {
    Qp.init(H,
            g,
            A,
            b,
            C,
            l,
            u,
            compute_preconditioner,
            rho,
            mu_eq,
            mu_in,
            manual_minimal_H_eigenvalue.value());
  } else {
    Qp.init(
      H, g, A, b, C, l, u, compute_preconditioner, rho, mu_eq, mu_in, nullopt);
  }
  Qp.solve(x, y, z);
 
  return Qp.results;
}
 
template<typename T, typename I>
struct BatchQP
{
  std::vector<QP<T, I>> qp_vector;
  sparse::isize m_size;
 
  BatchQP(long unsigned int batchSize)
  {
    if (qp_vector.max_size() != batchSize) {
      qp_vector.clear();
      qp_vector.reserve(batchSize);
    }
    m_size = 0;
  }
 
  QP<T, I>& init_qp_in_place(sparse::isize dim,
                             sparse::isize n_eq,
                             sparse::isize n_in)
  {
    qp_vector.emplace_back(dim, n_eq, n_in);
    auto& qp = qp_vector.back();
    m_size++;
    return qp;
  };
 
  // /*!
  //  * Init a QP in place and return a reference to it
  //  */
  // QP<T, I>& init_qp_in_place(const sparse::SparseMat<bool, I>& H,
  //                            const sparse::SparseMat<bool, I>& A,
  //                            const sparse::SparseMat<bool, I>& C)
  // {
  //   qp_vector.emplace_back(H.rows(), A.rows(), C.rows());
  //   auto& qp = qp_vector.back();
  //   m_size++;
  //   return qp;
  // };
 
  void insert(QP<T, I>& qp) { qp_vector.emplace_back(qp); };
 
  QP<T, I>& get(isize i) { return qp_vector.at(size_t(i)); };
 
  const QP<T, I>& get(isize i) const { return qp_vector.at(size_t(i)); };
 
  QP<T, I>& operator[](isize i) { return get(i); };
 
  const QP<T, I>& operator[](isize i) const { return get(i); };
 
  sparse::isize size() { return m_size; };
};
 
} // namespace sparse
} // namespace proxqp
} // namespace proxsuite
 
#endif /* end of include guard PROXSUITE_PROXQP_SPARSE_WRAPPER_HPP */