9 #include <Eigen/SparseCore>
13 #ifdef QP_SOLVER_COLLECTION_STANDALONE
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"
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
36 class GoldfarbIdnaniSolver;
50 struct d_dense_qp_dim;
52 struct d_dense_qp_sol;
53 struct d_dense_qp_ipm_arg;
54 struct d_dense_qp_ipm_ws;
70 template<
typename t_Scalar,
int... t_Parameters>
104 switch(qp_solver_type)
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";
127 QSC_ERROR_STREAM(
"[QpSolverType] Unsupported value: " <<
std::to_string(
static_cast<int>(qp_solver_type)));
148 void setup(
int dim_var,
int dim_eq,
int dim_ineq);
151 void printInfo(
bool verbose =
false,
const std::string & header =
"")
const;
154 void dump(std::ofstream & ofs)
const;
195 using clock =
typename std::conditional<std::chrono::high_resolution_clock::is_steady,
196 std::chrono::high_resolution_clock,
204 void printInfo(
bool verbose =
false,
const std::string & header =
"")
const;
232 virtual Eigen::VectorXd
solve(
int dim_var,
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;
278 virtual Eigen::VectorXd
solve(
int dim_var,
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;
291 std::unique_ptr<Eigen::QLDDirect>
qld_;
304 virtual Eigen::VectorXd
solve(
int dim_var,
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;
330 virtual Eigen::VectorXd
solve(
int dim_var,
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;
356 virtual Eigen::VectorXd
solve(
int dim_var,
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;
369 std::unique_ptr<jrl::qp::GoldfarbIdnaniSolver>
jrlqp_;
384 virtual Eigen::VectorXd
solve(
int dim_var,
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;
421 virtual Eigen::VectorXd
solve(
int dim_var,
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;
441 std::unique_ptr<OsqpEigen::Solver>
osqp_;
464 virtual Eigen::VectorXd
solve(
int dim_var,
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;
477 Eigen::SparseMatrix<double, Eigen::ColMajor, int>
Q_sparse_;
478 Eigen::SparseMatrix<double, Eigen::ColMajor, int>
A_sparse_;
497 virtual Eigen::VectorXd
solve(
int dim_var,
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;
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_;
543 virtual Eigen::VectorXd
solve(
int dim_var,
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;
556 std::unique_ptr<proxsuite::proxqp::dense::QP<double>>
proxqp_;
569 virtual Eigen::VectorXd
solve(
int dim_var,
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;