QpSolverCollection.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <chrono>
#include <fstream>
#include <memory>
 
#include <Eigen/SparseCore>
 
#include <qp_solver_collection/QpSolverOptions.h>
 
#ifdef QP_SOLVER_COLLECTION_STANDALONE
#  include <iostream>
#  define QSC_ERROR_STREAM(x) std::cerr << x << "\n"
#  define QSC_WARN_STREAM(x) std::cerr << x << "\n"
#  define QSC_INFO_STREAM(x) std::cout << x << "\n"
#else
#  include <rclcpp/rclcpp.hpp>
#  define QSC_ERROR_STREAM(msg) RCLCPP_ERROR_STREAM(rclcpp::get_logger("QpSolverCollection"), msg)
#  define QSC_WARN_STREAM(msg) RCLCPP_WARN_STREAM(rclcpp::get_logger("QpSolverCollection"), msg)
#  define QSC_INFO_STREAM(msg) RCLCPP_INFO_STREAM(rclcpp::get_logger("QpSolverCollection"), msg)
#endif
 
namespace Eigen
{
class QLDDirect;
class QuadProgDense;
class LSSOL_QP;
} // namespace Eigen
 
namespace jrl
{
namespace qp
{
class GoldfarbIdnaniSolver;
} // namespace qp
} // namespace jrl
 
namespace qpOASES
{
class SQProblem;
} // namespace qpOASES
 
namespace OsqpEigen
{
class Solver;
} // namespace OsqpEigen
 
struct d_dense_qp_dim;
struct d_dense_qp;
struct d_dense_qp_sol;
struct d_dense_qp_ipm_arg;
struct d_dense_qp_ipm_ws;
 
namespace proxsuite
{
namespace proxqp
{
namespace dense
{
template<typename T>
class QP;
}
} // namespace proxqp
} // namespace proxsuite
 
namespace qpmad
{
template<typename t_Scalar, int... t_Parameters>
class SolverTemplate;
using Solver = SolverTemplate<double, Eigen::Dynamic, 1, Eigen::Dynamic>;
} // namespace qpmad
 
namespace QpSolverCollection
{
enum class QpSolverType
{
  Any = -2,
  Uninitialized = -1,
  QLD = 0,
  QuadProg,
  LSSOL,
  JRLQP,
  qpOASES,
  OSQP,
  NASOQ,
  HPIPM,
  PROXQP,
  QPMAD
};
 
QpSolverType strToQpSolverType(const std::string & qp_solver_type);
} // namespace QpSolverCollection
 
namespace std
{
using QpSolverType = QpSolverCollection::QpSolverType;
 
inline string to_string(QpSolverType qp_solver_type)
{
  switch(qp_solver_type)
  {
    case QpSolverType::QLD:
      return "QpSolverType::QLD";
    case QpSolverType::QuadProg:
      return "QpSolverType::QuadProg";
    case QpSolverType::LSSOL:
      return "QpSolverType::LSSOL";
    case QpSolverType::JRLQP:
      return "QpSolverType::JRLQP";
    case QpSolverType::qpOASES:
      return "QpSolverType::qpOASES";
    case QpSolverType::OSQP:
      return "QpSolverType::OSQP";
    case QpSolverType::NASOQ:
      return "QpSolverType::NASOQ";
    case QpSolverType::HPIPM:
      return "QpSolverType::HPIPM";
    case QpSolverType::PROXQP:
      return "QpSolverType::PROXQP";
    case QpSolverType::QPMAD:
      return "QpSolverType::QPMAD";
    default:
      QSC_ERROR_STREAM("[QpSolverType] Unsupported value: " << std::to_string(static_cast<int>(qp_solver_type)));
  }
 
  return "";
}
} // namespace std
 
namespace QpSolverCollection
{
class QpCoeff
{
public:
  QpCoeff() {}
 
  void setup(int dim_var, int dim_eq, int dim_ineq);
 
  void printInfo(bool verbose = false, const std::string & header = "") const;
 
  void dump(std::ofstream & ofs) const;
 
public:
  int dim_var_ = 0;
 
  int dim_eq_ = 0;
 
  int dim_ineq_ = 0;
 
  Eigen::MatrixXd obj_mat_;
 
  Eigen::VectorXd obj_vec_;
 
  Eigen::MatrixXd eq_mat_;
 
  Eigen::VectorXd eq_vec_;
 
  Eigen::MatrixXd ineq_mat_;
 
  Eigen::VectorXd ineq_vec_;
 
  Eigen::VectorXd x_min_;
 
  Eigen::VectorXd x_max_;
};
 
class QpSolver
{
public:
  using clock = typename std::conditional<std::chrono::high_resolution_clock::is_steady,
                                          std::chrono::high_resolution_clock,
                                          std::chrono::steady_clock>::type;
 
public:
  QpSolver() {}
 
  void printInfo(bool verbose = false, const std::string & header = "") const;
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) = 0;
 
  virtual Eigen::VectorXd solve(QpCoeff & qp_coeff);
 
  inline QpSolverType type() const
  {
    return type_;
  }
 
  inline bool solveFailed() const
  {
    return solve_failed_;
  }
 
protected:
  QpSolverType type_ = QpSolverType::Uninitialized;
 
  bool solve_failed_ = false;
};
 
#if ENABLE_QLD
class QpSolverQld : public QpSolver
{
public:
  QpSolverQld();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
protected:
  std::unique_ptr<Eigen::QLDDirect> qld_;
};
#endif
 
#if ENABLE_QUADPROG
class QpSolverQuadprog : public QpSolver
{
public:
  QpSolverQuadprog();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
protected:
  std::unique_ptr<Eigen::QuadProgDense> quadprog_;
};
#endif
 
#if ENABLE_LSSOL
class QpSolverLssol : public QpSolver
{
public:
  QpSolverLssol();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
protected:
  std::unique_ptr<Eigen::LSSOL_QP> lssol_;
};
#endif
 
#if ENABLE_JRLQP
class QpSolverJrlqp : public QpSolver
{
public:
  QpSolverJrlqp();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
protected:
  std::unique_ptr<jrl::qp::GoldfarbIdnaniSolver> jrlqp_;
};
#endif
 
#if ENABLE_QPOASES
class QpSolverQpoases : public QpSolver
{
public:
  QpSolverQpoases();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
public:
  int n_wsr_ = 10000;
 
  bool force_initialize_ = true;
 
protected:
  std::unique_ptr<qpOASES::SQProblem> qpoases_;
};
#endif
 
#if ENABLE_OSQP
class QpSolverOsqp : public QpSolver
{
public:
  QpSolverOsqp();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
public:
  bool force_initialize_ = true;
 
protected:
  std::unique_ptr<OsqpEigen::Solver> osqp_;
 
  Eigen::SparseMatrix<double> Q_sparse_;
  Eigen::VectorXd c_;
  Eigen::SparseMatrix<double> AC_with_bound_sparse_;
  Eigen::VectorXd bd_with_bound_min_;
  Eigen::VectorXd bd_with_bound_max_;
 
  double sparse_duration_ = 0; // [ms]
};
#endif
 
#if ENABLE_NASOQ
class QpSolverNasoq : public QpSolver
{
public:
  QpSolverNasoq();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
protected:
  Eigen::SparseMatrix<double, Eigen::ColMajor, int> Q_sparse_;
  Eigen::SparseMatrix<double, Eigen::ColMajor, int> A_sparse_;
  Eigen::SparseMatrix<double, Eigen::ColMajor, int> C_with_bound_sparse_;
 
  double sparse_duration_ = 0; // [ms]
};
#endif
 
#if ENABLE_HPIPM
class QpSolverHpipm : public QpSolver
{
public:
  QpSolverHpipm();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
public:
  double bound_limit_ = 1e10;
 
protected:
  std::unique_ptr<struct d_dense_qp_dim> qp_dim_;
  std::unique_ptr<struct d_dense_qp> qp_;
  std::unique_ptr<struct d_dense_qp_sol> qp_sol_;
  std::unique_ptr<struct d_dense_qp_ipm_arg> ipm_arg_;
  std::unique_ptr<struct d_dense_qp_ipm_ws> ipm_ws_;
 
  std::unique_ptr<uint8_t[]> qp_dim_mem_ = nullptr;
  std::unique_ptr<uint8_t[]> qp_mem_ = nullptr;
  std::unique_ptr<uint8_t[]> qp_sol_mem_ = nullptr;
  std::unique_ptr<uint8_t[]> ipm_arg_mem_ = nullptr;
  std::unique_ptr<uint8_t[]> ipm_ws_mem_ = nullptr;
 
  std::unique_ptr<double[]> opt_x_mem_ = nullptr;
};
#endif
 
#if ENABLE_PROXQP
class QpSolverProxqp : public QpSolver
{
public:
  QpSolverProxqp();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
protected:
  std::unique_ptr<proxsuite::proxqp::dense::QP<double>> proxqp_;
};
#endif
 
#if ENABLE_QPMAD
class QpSolverQpmad : public QpSolver
{
public:
  QpSolverQpmad();
 
  virtual Eigen::VectorXd solve(int dim_var,
                                int dim_eq,
                                int dim_ineq,
                                Eigen::Ref<Eigen::MatrixXd> Q,
                                const Eigen::Ref<const Eigen::VectorXd> & c,
                                const Eigen::Ref<const Eigen::MatrixXd> & A,
                                const Eigen::Ref<const Eigen::VectorXd> & b,
                                const Eigen::Ref<const Eigen::MatrixXd> & C,
                                const Eigen::Ref<const Eigen::VectorXd> & d,
                                const Eigen::Ref<const Eigen::VectorXd> & x_min,
                                const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
 
protected:
  std::unique_ptr<qpmad::Solver> qpmad_;
};
#endif
 
QpSolverType getAnyQpSolverType();
 
bool isQpSolverEnabled(const QpSolverType & qp_solver_type);
 
std::shared_ptr<QpSolver> allocateQpSolver(const QpSolverType & qp_solver_type);
} // namespace QpSolverCollection