qp_solver_collection
QpSolverCollection.h
Go to the documentation of this file.
1 /* Author: Masaki Murooka */
2 
3 #pragma once
4 
5 #include <chrono>
6 #include <fstream>
7 #include <memory>
8 
9 #include <Eigen/SparseCore>
10 
12 
13 #ifdef QP_SOLVER_COLLECTION_STANDALONE
14 # include <iostream>
15 # define QSC_ERROR_STREAM(x) std::cerr << x << "\n"
16 # define QSC_WARN_STREAM(x) std::cerr << x << "\n"
17 # define QSC_INFO_STREAM(x) std::cout << x << "\n"
18 #else
19 # include <ros/console.h>
20 # define QSC_ERROR_STREAM ROS_ERROR_STREAM
21 # define QSC_WARN_STREAM ROS_WARN_STREAM
22 # define QSC_INFO_STREAM ROS_INFO_STREAM
23 #endif
24 
25 namespace Eigen
26 {
27 class QLDDirect;
28 class QuadProgDense;
29 class LSSOL_QP;
30 } // namespace Eigen
31 
32 namespace jrl
33 {
34 namespace qp
35 {
36 class GoldfarbIdnaniSolver;
37 } // namespace qp
38 } // namespace jrl
39 
40 namespace qpOASES
41 {
42 class SQProblem;
43 } // namespace qpOASES
44 
45 namespace OsqpEigen
46 {
47 class Solver;
48 } // namespace OsqpEigen
49 
50 struct d_dense_qp_dim;
51 struct d_dense_qp;
52 struct d_dense_qp_sol;
53 struct d_dense_qp_ipm_arg;
54 struct d_dense_qp_ipm_ws;
55 
56 namespace proxsuite
57 {
58 namespace proxqp
59 {
60 namespace dense
61 {
62 template<typename T>
63 class QP;
64 }
65 } // namespace proxqp
66 } // namespace proxsuite
67 
68 namespace qpmad
69 {
70 template<typename t_Scalar, int... t_Parameters>
73 } // namespace qpmad
74 
76 {
78 enum class QpSolverType
79 {
80  Any = -2,
81  Uninitialized = -1,
82  QLD = 0,
83  QuadProg,
84  LSSOL,
85  JRLQP,
86  qpOASES,
87  OSQP,
88  NASOQ,
89  HPIPM,
90  PROXQP,
91  QPMAD
92 };
93 
95 QpSolverType strToQpSolverType(const std::string & qp_solver_type);
96 } // namespace QpSolverCollection
97 
98 namespace std
99 {
101 
102 inline string to_string(QpSolverType qp_solver_type)
103 {
104  switch(qp_solver_type)
105  {
106  case QpSolverType::QLD:
107  return "QpSolverType::QLD";
108  case QpSolverType::QuadProg:
109  return "QpSolverType::QuadProg";
110  case QpSolverType::LSSOL:
111  return "QpSolverType::LSSOL";
112  case QpSolverType::JRLQP:
113  return "QpSolverType::JRLQP";
114  case QpSolverType::qpOASES:
115  return "QpSolverType::qpOASES";
116  case QpSolverType::OSQP:
117  return "QpSolverType::OSQP";
118  case QpSolverType::NASOQ:
119  return "QpSolverType::NASOQ";
120  case QpSolverType::HPIPM:
121  return "QpSolverType::HPIPM";
122  case QpSolverType::PROXQP:
123  return "QpSolverType::PROXQP";
124  case QpSolverType::QPMAD:
125  return "QpSolverType::QPMAD";
126  default:
127  QSC_ERROR_STREAM("[QpSolverType] Unsupported value: " << std::to_string(static_cast<int>(qp_solver_type)));
128  }
129 
130  return "";
131 }
132 } // namespace std
133 
134 namespace QpSolverCollection
135 {
137 class QpCoeff
138 {
139 public:
141  QpCoeff() {}
142 
148  void setup(int dim_var, int dim_eq, int dim_ineq);
149 
151  void printInfo(bool verbose = false, const std::string & header = "") const;
152 
154  void dump(std::ofstream & ofs) const;
155 
156 public:
158  int dim_var_ = 0;
159 
161  int dim_eq_ = 0;
162 
164  int dim_ineq_ = 0;
165 
167  Eigen::MatrixXd obj_mat_;
168 
170  Eigen::VectorXd obj_vec_;
171 
173  Eigen::MatrixXd eq_mat_;
174 
176  Eigen::VectorXd eq_vec_;
177 
179  Eigen::MatrixXd ineq_mat_;
180 
182  Eigen::VectorXd ineq_vec_;
183 
185  Eigen::VectorXd x_min_;
186 
188  Eigen::VectorXd x_max_;
189 };
190 
192 class QpSolver
193 {
194 public:
195  using clock = typename std::conditional<std::chrono::high_resolution_clock::is_steady,
196  std::chrono::high_resolution_clock,
197  std::chrono::steady_clock>::type;
198 
199 public:
201  QpSolver() {}
202 
204  void printInfo(bool verbose = false, const std::string & header = "") const;
205 
232  virtual Eigen::VectorXd solve(int dim_var,
233  int dim_eq,
234  int dim_ineq,
235  Eigen::Ref<Eigen::MatrixXd> Q,
236  const Eigen::Ref<const Eigen::VectorXd> & c,
237  const Eigen::Ref<const Eigen::MatrixXd> & A,
238  const Eigen::Ref<const Eigen::VectorXd> & b,
239  const Eigen::Ref<const Eigen::MatrixXd> & C,
240  const Eigen::Ref<const Eigen::VectorXd> & d,
241  const Eigen::Ref<const Eigen::VectorXd> & x_min,
242  const Eigen::Ref<const Eigen::VectorXd> & x_max) = 0;
243 
247  virtual Eigen::VectorXd solve(QpCoeff & qp_coeff);
248 
250  inline QpSolverType type() const
251  {
252  return type_;
253  }
254 
256  inline bool solveFailed() const
257  {
258  return solve_failed_;
259  }
260 
261 protected:
264 
266  bool solve_failed_ = false;
267 };
268 
269 #if ENABLE_QLD
270 
271 class QpSolverQld : public QpSolver
272 {
273 public:
275  QpSolverQld();
276 
278  virtual Eigen::VectorXd solve(int dim_var,
279  int dim_eq,
280  int dim_ineq,
281  Eigen::Ref<Eigen::MatrixXd> Q,
282  const Eigen::Ref<const Eigen::VectorXd> & c,
283  const Eigen::Ref<const Eigen::MatrixXd> & A,
284  const Eigen::Ref<const Eigen::VectorXd> & b,
285  const Eigen::Ref<const Eigen::MatrixXd> & C,
286  const Eigen::Ref<const Eigen::VectorXd> & d,
287  const Eigen::Ref<const Eigen::VectorXd> & x_min,
288  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
289 
290 protected:
291  std::unique_ptr<Eigen::QLDDirect> qld_;
292 };
293 #endif
294 
295 #if ENABLE_QUADPROG
296 
298 {
299 public:
302 
304  virtual Eigen::VectorXd solve(int dim_var,
305  int dim_eq,
306  int dim_ineq,
307  Eigen::Ref<Eigen::MatrixXd> Q,
308  const Eigen::Ref<const Eigen::VectorXd> & c,
309  const Eigen::Ref<const Eigen::MatrixXd> & A,
310  const Eigen::Ref<const Eigen::VectorXd> & b,
311  const Eigen::Ref<const Eigen::MatrixXd> & C,
312  const Eigen::Ref<const Eigen::VectorXd> & d,
313  const Eigen::Ref<const Eigen::VectorXd> & x_min,
314  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
315 
316 protected:
317  std::unique_ptr<Eigen::QuadProgDense> quadprog_;
318 };
319 #endif
320 
321 #if ENABLE_LSSOL
322 
323 class QpSolverLssol : public QpSolver
324 {
325 public:
327  QpSolverLssol();
328 
330  virtual Eigen::VectorXd solve(int dim_var,
331  int dim_eq,
332  int dim_ineq,
333  Eigen::Ref<Eigen::MatrixXd> Q,
334  const Eigen::Ref<const Eigen::VectorXd> & c,
335  const Eigen::Ref<const Eigen::MatrixXd> & A,
336  const Eigen::Ref<const Eigen::VectorXd> & b,
337  const Eigen::Ref<const Eigen::MatrixXd> & C,
338  const Eigen::Ref<const Eigen::VectorXd> & d,
339  const Eigen::Ref<const Eigen::VectorXd> & x_min,
340  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
341 
342 protected:
343  std::unique_ptr<Eigen::LSSOL_QP> lssol_;
344 };
345 #endif
346 
347 #if ENABLE_JRLQP
348 
349 class QpSolverJrlqp : public QpSolver
350 {
351 public:
353  QpSolverJrlqp();
354 
356  virtual Eigen::VectorXd solve(int dim_var,
357  int dim_eq,
358  int dim_ineq,
359  Eigen::Ref<Eigen::MatrixXd> Q,
360  const Eigen::Ref<const Eigen::VectorXd> & c,
361  const Eigen::Ref<const Eigen::MatrixXd> & A,
362  const Eigen::Ref<const Eigen::VectorXd> & b,
363  const Eigen::Ref<const Eigen::MatrixXd> & C,
364  const Eigen::Ref<const Eigen::VectorXd> & d,
365  const Eigen::Ref<const Eigen::VectorXd> & x_min,
366  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
367 
368 protected:
369  std::unique_ptr<jrl::qp::GoldfarbIdnaniSolver> jrlqp_;
370 };
371 #endif
372 
373 #if ENABLE_QPOASES
374 
377 class QpSolverQpoases : public QpSolver
378 {
379 public:
381  QpSolverQpoases();
382 
384  virtual Eigen::VectorXd solve(int dim_var,
385  int dim_eq,
386  int dim_ineq,
387  Eigen::Ref<Eigen::MatrixXd> Q,
388  const Eigen::Ref<const Eigen::VectorXd> & c,
389  const Eigen::Ref<const Eigen::MatrixXd> & A,
390  const Eigen::Ref<const Eigen::VectorXd> & b,
391  const Eigen::Ref<const Eigen::MatrixXd> & C,
392  const Eigen::Ref<const Eigen::VectorXd> & d,
393  const Eigen::Ref<const Eigen::VectorXd> & x_min,
394  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
395 
396 public:
397  int n_wsr_ = 10000;
398 
403  bool force_initialize_ = true;
404 
405 protected:
406  std::unique_ptr<qpOASES::SQProblem> qpoases_;
407 };
408 #endif
409 
410 #if ENABLE_OSQP
411 
414 class QpSolverOsqp : public QpSolver
415 {
416 public:
418  QpSolverOsqp();
419 
421  virtual Eigen::VectorXd solve(int dim_var,
422  int dim_eq,
423  int dim_ineq,
424  Eigen::Ref<Eigen::MatrixXd> Q,
425  const Eigen::Ref<const Eigen::VectorXd> & c,
426  const Eigen::Ref<const Eigen::MatrixXd> & A,
427  const Eigen::Ref<const Eigen::VectorXd> & b,
428  const Eigen::Ref<const Eigen::MatrixXd> & C,
429  const Eigen::Ref<const Eigen::VectorXd> & d,
430  const Eigen::Ref<const Eigen::VectorXd> & x_min,
431  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
432 
433 public:
438  bool force_initialize_ = true;
439 
440 protected:
441  std::unique_ptr<OsqpEigen::Solver> osqp_;
442 
443  Eigen::SparseMatrix<double> Q_sparse_;
444  Eigen::VectorXd c_;
445  Eigen::SparseMatrix<double> AC_with_bound_sparse_;
446  Eigen::VectorXd bd_with_bound_min_;
447  Eigen::VectorXd bd_with_bound_max_;
448 
449  double sparse_duration_ = 0; // [ms]
450 };
451 #endif
452 
453 #if ENABLE_NASOQ
454 
457 class QpSolverNasoq : public QpSolver
458 {
459 public:
461  QpSolverNasoq();
462 
464  virtual Eigen::VectorXd solve(int dim_var,
465  int dim_eq,
466  int dim_ineq,
467  Eigen::Ref<Eigen::MatrixXd> Q,
468  const Eigen::Ref<const Eigen::VectorXd> & c,
469  const Eigen::Ref<const Eigen::MatrixXd> & A,
470  const Eigen::Ref<const Eigen::VectorXd> & b,
471  const Eigen::Ref<const Eigen::MatrixXd> & C,
472  const Eigen::Ref<const Eigen::VectorXd> & d,
473  const Eigen::Ref<const Eigen::VectorXd> & x_min,
474  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
475 
476 protected:
477  Eigen::SparseMatrix<double, Eigen::ColMajor, int> Q_sparse_;
478  Eigen::SparseMatrix<double, Eigen::ColMajor, int> A_sparse_;
479  Eigen::SparseMatrix<double, Eigen::ColMajor, int> C_with_bound_sparse_;
480 
481  double sparse_duration_ = 0; // [ms]
482 };
483 #endif
484 
485 #if ENABLE_HPIPM
486 
487 class QpSolverHpipm : public QpSolver
488 {
489 public:
491  QpSolverHpipm();
492 
494  ~QpSolverHpipm();
495 
497  virtual Eigen::VectorXd solve(int dim_var,
498  int dim_eq,
499  int dim_ineq,
500  Eigen::Ref<Eigen::MatrixXd> Q,
501  const Eigen::Ref<const Eigen::VectorXd> & c,
502  const Eigen::Ref<const Eigen::MatrixXd> & A,
503  const Eigen::Ref<const Eigen::VectorXd> & b,
504  const Eigen::Ref<const Eigen::MatrixXd> & C,
505  const Eigen::Ref<const Eigen::VectorXd> & d,
506  const Eigen::Ref<const Eigen::VectorXd> & x_min,
507  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
508 
509 public:
515  double bound_limit_ = 1e10;
516 
517 protected:
518  std::unique_ptr<struct d_dense_qp_dim> qp_dim_;
519  std::unique_ptr<struct d_dense_qp> qp_;
520  std::unique_ptr<struct d_dense_qp_sol> qp_sol_;
521  std::unique_ptr<struct d_dense_qp_ipm_arg> ipm_arg_;
522  std::unique_ptr<struct d_dense_qp_ipm_ws> ipm_ws_;
523 
524  void * qp_dim_mem_ = nullptr;
525  void * qp_mem_ = nullptr;
526  void * qp_sol_mem_ = nullptr;
527  void * ipm_arg_mem_ = nullptr;
528  void * ipm_ws_mem_ = nullptr;
529 
530  double * opt_x_mem_;
531 };
532 #endif
533 
534 #if ENABLE_PROXQP
535 
536 class QpSolverProxqp : public QpSolver
537 {
538 public:
540  QpSolverProxqp();
541 
543  virtual Eigen::VectorXd solve(int dim_var,
544  int dim_eq,
545  int dim_ineq,
546  Eigen::Ref<Eigen::MatrixXd> Q,
547  const Eigen::Ref<const Eigen::VectorXd> & c,
548  const Eigen::Ref<const Eigen::MatrixXd> & A,
549  const Eigen::Ref<const Eigen::VectorXd> & b,
550  const Eigen::Ref<const Eigen::MatrixXd> & C,
551  const Eigen::Ref<const Eigen::VectorXd> & d,
552  const Eigen::Ref<const Eigen::VectorXd> & x_min,
553  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
554 
555 protected:
556  std::unique_ptr<proxsuite::proxqp::dense::QP<double>> proxqp_;
557 };
558 #endif
559 
560 #if ENABLE_QPMAD
561 
562 class QpSolverQpmad : public QpSolver
563 {
564 public:
566  QpSolverQpmad();
567 
569  virtual Eigen::VectorXd solve(int dim_var,
570  int dim_eq,
571  int dim_ineq,
572  Eigen::Ref<Eigen::MatrixXd> Q,
573  const Eigen::Ref<const Eigen::VectorXd> & c,
574  const Eigen::Ref<const Eigen::MatrixXd> & A,
575  const Eigen::Ref<const Eigen::VectorXd> & b,
576  const Eigen::Ref<const Eigen::MatrixXd> & C,
577  const Eigen::Ref<const Eigen::VectorXd> & d,
578  const Eigen::Ref<const Eigen::VectorXd> & x_min,
579  const Eigen::Ref<const Eigen::VectorXd> & x_max) override;
580 
581 protected:
582  std::unique_ptr<qpmad::Solver> qpmad_;
583 };
584 #endif
585 
592 
596 bool isQpSolverEnabled(const QpSolverType & qp_solver_type);
597 
601 std::shared_ptr<QpSolver> allocateQpSolver(const QpSolverType & qp_solver_type);
602 } // namespace QpSolverCollection
QpSolverCollection::QpSolverQld
QP solver QLD.
Definition: QpSolverCollection.h:271
QpSolverCollection::QpSolverHpipm::qp_mem_
void * qp_mem_
Definition: QpSolverCollection.h:525
QpSolverCollection::QpCoeff
Class of QP coefficient.
Definition: QpSolverCollection.h:137
QpSolverCollection::QpSolverQld::qld_
std::unique_ptr< Eigen::QLDDirect > qld_
Definition: QpSolverCollection.h:291
Eigen
Definition: QpSolverCollection.h:25
QpSolverCollection::QpSolverHpipm::qp_
std::unique_ptr< struct d_dense_qp > qp_
Definition: QpSolverCollection.h:519
QpSolverCollection::QpCoeff::obj_vec_
Eigen::VectorXd obj_vec_
Objective vector (corresponding to in QpSolver::solve.)
Definition: QpSolverCollection.h:170
QpSolverCollection::QpSolver::clock
typename std::conditional< std::chrono::high_resolution_clock::is_steady, std::chrono::high_resolution_clock, std::chrono::steady_clock >::type clock
Definition: QpSolverCollection.h:197
QpSolverCollection::QpSolverOsqp::bd_with_bound_max_
Eigen::VectorXd bd_with_bound_max_
Definition: QpSolverCollection.h:447
QpSolverCollection::isQpSolverEnabled
bool isQpSolverEnabled(const QpSolverType &qp_solver_type)
Check whether QP solver is enabled.
QpSolverCollection::QpSolverType::HPIPM
@ HPIPM
QpSolverCollection::QpCoeff::ineq_vec_
Eigen::VectorXd ineq_vec_
Inequality constraint vector (corresponding to in QpSolver::solve.)
Definition: QpSolverCollection.h:182
QpSolverCollection::QpSolverHpipm::bound_limit_
double bound_limit_
Maximum limits of inequality bounds.
Definition: QpSolverCollection.h:515
QpSolverCollection::QpSolverHpipm
QP solver HPIPM.
Definition: QpSolverCollection.h:487
QpSolverCollection
Definition: QpSolverCollection.h:75
QpSolverCollection::QpSolverJrlqp
QP solver JRLQP.
Definition: QpSolverCollection.h:349
QpSolverCollection::QpSolverType::QuadProg
@ QuadProg
QpSolverCollection::QpSolverQpmad::solve
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
Solve QP.
QpSolverCollection::QpSolverQpoases::qpoases_
std::unique_ptr< qpOASES::SQProblem > qpoases_
Definition: QpSolverCollection.h:406
QpSolverCollection::QpSolverOsqp::solve
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
Solve QP.
QpSolverCollection::QpSolverHpipm::~QpSolverHpipm
~QpSolverHpipm()
Destructor.
std::to_string
string to_string(QpSolverType qp_solver_type)
Definition: QpSolverCollection.h:102
QpSolverCollection::QpSolverType::PROXQP
@ PROXQP
QpSolverCollection::QpSolverType::QPMAD
@ QPMAD
QpSolverCollection::QpSolver::solve
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
Solve QP.
QpSolverCollection::QpSolverLssol::solve
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
Solve QP.
QpSolverCollection::QpCoeff::dim_var_
int dim_var_
Dimension of decision variable.
Definition: QpSolverCollection.h:158
QpSolverCollection::QpSolverJrlqp::QpSolverJrlqp
QpSolverJrlqp()
Constructor.
qpmad::SolverTemplate
Definition: QpSolverCollection.h:71
QpSolverCollection::QpSolverQpoases::QpSolverQpoases
QpSolverQpoases()
Constructor.
QpSolverCollection::QpSolver::solve_failed_
bool solve_failed_
Whether it failed to solve the QP.
Definition: QpSolverCollection.h:266
QpSolverCollection::allocateQpSolver
std::shared_ptr< QpSolver > allocateQpSolver(const QpSolverType &qp_solver_type)
Allocate the specified QP solver.
QpSolverCollection::QpSolverHpipm::qp_dim_mem_
void * qp_dim_mem_
Definition: QpSolverCollection.h:524
QpSolverCollection::QpCoeff::dim_eq_
int dim_eq_
Dimension of equality constraint.
Definition: QpSolverCollection.h:161
QpSolverCollection::QpSolverOsqp
QP solver OSQP.
Definition: QpSolverCollection.h:414
qpmad
Definition: QpSolverCollection.h:68
QpSolverCollection::QpSolverType::JRLQP
@ JRLQP
QpSolverCollection::QpSolverQpmad
QP solver QPMAD.
Definition: QpSolverCollection.h:562
QpSolverCollection::QpSolverNasoq::Q_sparse_
Eigen::SparseMatrix< double, Eigen::ColMajor, int > Q_sparse_
Definition: QpSolverCollection.h:477
QpSolverCollection::QpSolverType::qpOASES
@ qpOASES
QpSolverCollection::QpSolver::type_
QpSolverType type_
QP solver type.
Definition: QpSolverCollection.h:263
QpSolverCollection::QpSolverOsqp::force_initialize_
bool force_initialize_
Whether to initialize each time instead of doing a warm start.
Definition: QpSolverCollection.h:438
QpSolverCollection::QpCoeff::printInfo
void printInfo(bool verbose=false, const std::string &header="") const
Print information.
QpSolverCollection::QpSolverNasoq::solve
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
Solve QP.
QpSolverCollection::QpSolver::printInfo
void printInfo(bool verbose=false, const std::string &header="") const
Print information.
QpSolverCollection::QpSolverOsqp::Q_sparse_
Eigen::SparseMatrix< double > Q_sparse_
Definition: QpSolverCollection.h:443
jrl
Definition: QpSolverCollection.h:32
QpSolverCollection::QpSolver
Virtual class of QP solver.
Definition: QpSolverCollection.h:192
QpSolverCollection::QpSolverHpipm::ipm_arg_mem_
void * ipm_arg_mem_
Definition: QpSolverCollection.h:527
QpSolverCollection::QpCoeff::obj_mat_
Eigen::MatrixXd obj_mat_
Objective matrix (corresponding to in QpSolver::solve.)
Definition: QpSolverCollection.h:167
QpSolverCollection::QpSolverOsqp::bd_with_bound_min_
Eigen::VectorXd bd_with_bound_min_
Definition: QpSolverCollection.h:446
QpSolverCollection::QpSolverQpoases::n_wsr_
int n_wsr_
Definition: QpSolverCollection.h:397
QpSolverCollection::QpCoeff::ineq_mat_
Eigen::MatrixXd ineq_mat_
Inequality constraint matrix (corresponding to in QpSolver::solve.)
Definition: QpSolverCollection.h:179
QpSolverCollection::QpSolverType::Any
@ Any
QpSolverCollection::QpSolver::QpSolver
QpSolver()
Constructor.
Definition: QpSolverCollection.h:201
QpSolverCollection::QpSolverQpoases::solve
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
Solve QP.
QpSolverCollection::QpSolverProxqp::QpSolverProxqp
QpSolverProxqp()
Constructor.
QpSolverCollection::QpSolver::solveFailed
bool solveFailed() const
Get whether it failed to solve the QP.
Definition: QpSolverCollection.h:256
QpSolverCollection::QpCoeff::x_max_
Eigen::VectorXd x_max_
Upper bound (corresponding to in QpSolver::solve.)
Definition: QpSolverCollection.h:188
QpSolverCollection::QpSolverQuadprog
QP solver QuadProg.
Definition: QpSolverCollection.h:297
QpSolverCollection::QpSolverJrlqp::solve
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
Solve QP.
QpSolverCollection::QpSolverHpipm::ipm_ws_mem_
void * ipm_ws_mem_
Definition: QpSolverCollection.h:528
QpSolverCollection::QpSolverLssol::QpSolverLssol
QpSolverLssol()
Constructor.
QpSolverCollection::QpSolverQuadprog::QpSolverQuadprog
QpSolverQuadprog()
Constructor.
QpSolverCollection::getAnyQpSolverType
QpSolverType getAnyQpSolverType()
Get one QP solver type that is enabled.
QpSolverCollection::QpSolverHpipm::ipm_ws_
std::unique_ptr< struct d_dense_qp_ipm_ws > ipm_ws_
Definition: QpSolverCollection.h:522
proxsuite
Definition: QpSolverCollection.h:56
QpSolverCollection::QpSolverHpipm::QpSolverHpipm
QpSolverHpipm()
Constructor.
QpSolverCollection::QpSolverProxqp
QP solver PROXQP.
Definition: QpSolverCollection.h:536
QpSolverCollection::QpSolverProxqp::proxqp_
std::unique_ptr< proxsuite::proxqp::dense::QP< double > > proxqp_
Definition: QpSolverCollection.h:556
QpSolverCollection::QpSolverOsqp::sparse_duration_
double sparse_duration_
Definition: QpSolverCollection.h:449
QpSolverCollection::QpSolverHpipm::qp_sol_mem_
void * qp_sol_mem_
Definition: QpSolverCollection.h:526
QpSolverCollection::QpCoeff::dim_ineq_
int dim_ineq_
Dimension of inequality constraint.
Definition: QpSolverCollection.h:164
proxsuite::proxqp::dense::QP
Definition: QpSolverCollection.h:63
qpmad::Solver
SolverTemplate< double, Eigen::Dynamic, 1, Eigen::Dynamic > Solver
Definition: QpSolverCollection.h:72
QpSolverCollection::QpSolverType::OSQP
@ OSQP
QpSolverCollection::QpSolverLssol::lssol_
std::unique_ptr< Eigen::LSSOL_QP > lssol_
Definition: QpSolverCollection.h:343
QpSolverCollection::QpSolverQpmad::QpSolverQpmad
QpSolverQpmad()
Constructor.
QpSolverCollection::QpSolverLssol
QP solver LSSOL.
Definition: QpSolverCollection.h:323
QpSolverCollection::QpSolverOsqp::QpSolverOsqp
QpSolverOsqp()
Constructor.
QpSolverCollection::QpSolverProxqp::solve
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
Solve QP.
QpSolverCollection::QpSolverNasoq::A_sparse_
Eigen::SparseMatrix< double, Eigen::ColMajor, int > A_sparse_
Definition: QpSolverCollection.h:478
QpSolverCollection::QpCoeff::eq_mat_
Eigen::MatrixXd eq_mat_
Equality constraint matrix (corresponding to in QpSolver::solve.)
Definition: QpSolverCollection.h:173
QpSolverCollection::QpSolverNasoq::C_with_bound_sparse_
Eigen::SparseMatrix< double, Eigen::ColMajor, int > C_with_bound_sparse_
Definition: QpSolverCollection.h:479
QpSolverCollection::QpSolverJrlqp::jrlqp_
std::unique_ptr< jrl::qp::GoldfarbIdnaniSolver > jrlqp_
Definition: QpSolverCollection.h:369
QpSolverCollection::QpSolverOsqp::AC_with_bound_sparse_
Eigen::SparseMatrix< double > AC_with_bound_sparse_
Definition: QpSolverCollection.h:445
QpSolverCollection::QpSolverHpipm::opt_x_mem_
double * opt_x_mem_
Definition: QpSolverCollection.h:530
QpSolverCollection::QpSolverType
QpSolverType
QP solver type.
Definition: QpSolverCollection.h:78
QpSolverCollection::QpSolverType::QLD
@ QLD
QpSolverCollection::QpSolverHpipm::qp_dim_
std::unique_ptr< struct d_dense_qp_dim > qp_dim_
Definition: QpSolverCollection.h:518
std
Definition: QpSolverCollection.h:98
QpSolverCollection::QpCoeff::dump
void dump(std::ofstream &ofs) const
Dump coefficients.
QpSolverCollection::QpSolverQuadprog::quadprog_
std::unique_ptr< Eigen::QuadProgDense > quadprog_
Definition: QpSolverCollection.h:317
QpSolverCollection::QpSolverType::NASOQ
@ NASOQ
QpSolverCollection::QpSolverQld::QpSolverQld
QpSolverQld()
Constructor.
QpSolverCollection::QpSolverNasoq::sparse_duration_
double sparse_duration_
Definition: QpSolverCollection.h:481
qpOASES
Definition: QpSolverCollection.h:40
QpSolverCollection::QpSolverHpipm::ipm_arg_
std::unique_ptr< struct d_dense_qp_ipm_arg > ipm_arg_
Definition: QpSolverCollection.h:521
QpSolverCollection::QpSolverOsqp::c_
Eigen::VectorXd c_
Definition: QpSolverCollection.h:444
QpSolverCollection::QpSolverQld::solve
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
Solve QP.
QpSolverCollection::QpSolverOsqp::osqp_
std::unique_ptr< OsqpEigen::Solver > osqp_
Definition: QpSolverCollection.h:441
QpSolverCollection::QpCoeff::eq_vec_
Eigen::VectorXd eq_vec_
Equality constraint vector (corresponding to in QpSolver::solve.)
Definition: QpSolverCollection.h:176
QpSolverCollection::QpSolverType::LSSOL
@ LSSOL
QpSolverCollection::QpSolverQpoases
QP solver qpOASES.
Definition: QpSolverCollection.h:377
QpSolverCollection::QpSolverNasoq
QP solver NASOQ.
Definition: QpSolverCollection.h:457
QpSolverCollection::QpSolverNasoq::QpSolverNasoq
QpSolverNasoq()
Constructor.
QpSolverCollection::QpSolverHpipm::qp_sol_
std::unique_ptr< struct d_dense_qp_sol > qp_sol_
Definition: QpSolverCollection.h:520
OsqpEigen
Definition: QpSolverCollection.h:45
QpSolverCollection::QpSolverType::Uninitialized
@ Uninitialized
QpSolverCollection::QpSolverQpoases::force_initialize_
bool force_initialize_
Whether to initialize each time instead of doing a warm start.
Definition: QpSolverCollection.h:403
QpSolverCollection::QpSolverQuadprog::solve
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
Solve QP.
QpSolverCollection::QpSolverHpipm::solve
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
Solve QP.
QpSolverOptions.h
QpSolverCollection::QpCoeff::QpCoeff
QpCoeff()
Constructor.
Definition: QpSolverCollection.h:141
QpSolverCollection::QpSolverQpmad::qpmad_
std::unique_ptr< qpmad::Solver > qpmad_
Definition: QpSolverCollection.h:582
QpSolverCollection::QpCoeff::x_min_
Eigen::VectorXd x_min_
Lower bound (corresponding to in QpSolver::solve.)
Definition: QpSolverCollection.h:185
QpSolverCollection::QpCoeff::setup
void setup(int dim_var, int dim_eq, int dim_ineq)
Setup the coefficients with filling zero.
QpSolverCollection::QpSolver::type
QpSolverType type() const
Get QP solver type.
Definition: QpSolverCollection.h:250
QpSolverCollection::strToQpSolverType
QpSolverType strToQpSolverType(const std::string &qp_solver_type)
Convert std::string to QpSolverType.