18 #ifndef EIQUADPROGFAST_HH_ 19 #define EIQUADPROGFAST_HH_ 21 #include <Eigen/Dense> 22 #include <Eigen/Sparse> 24 #define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0 25 #define OPTIMIZE_COMPUTE_D 26 #define OPTIMIZE_UPDATE_Z 27 #define OPTIMIZE_HESSIAN_INVERSE 28 #define OPTIMIZE_UNCONSTR_MINIM 35 #define DEBUG_STREAM(msg) 37 #ifdef PROFILE_EIQUADPROG 38 #define START_PROFILER_EIQUADPROG_FAST START_PROFILER 39 #define STOP_PROFILER_EIQUADPROG_FAST STOP_PROFILER 41 #define START_PROFILER_EIQUADPROG_FAST 42 #define STOP_PROFILER_EIQUADPROG_FAST 45 #define EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION "EIQUADPROG_FAST Chowlesky dec" 46 #define EIQUADPROG_FAST_CHOWLESKY_INVERSE "EIQUADPROG_FAST Chowlesky inv" 47 #define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR" 48 #define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1" 49 #define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2" 50 #define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1" 51 #define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1" 52 #define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2" 53 #define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \ 54 "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM" 55 #define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2" 56 #define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A" 57 #define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B" 58 #define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C" 60 #define DEFAULT_MAX_ITER 1000 83 typedef Eigen::SparseMatrix<double>
SpMat;
84 typedef Eigen::SparseVector<double>
SpVec;
85 typedef Eigen::SparseVector<int>
SpVeci;
88 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
93 void reset(
int dim_qp,
int num_eq,
int num_ineq);
98 if (maxIter < 0)
return false;
140 const MatrixXd& CE,
const VectorXd& ce0,
141 const MatrixXd& CI,
const VectorXd& ci0,
154 const VectorXd& ci0, VectorXd& x);
167 Eigen::LLT<MatrixXd, Eigen::Lower> chol_;
213 #ifdef OPTIMIZE_ADD_CONSTRAINT 224 inline void compute_d(VectorXd& d,
const MatrixXd& J,
const VectorXd& np) {
225 #ifdef OPTIMIZE_COMPUTE_D 226 d.noalias() = J.adjoint() * np;
228 d = J.adjoint() * np;
232 inline void update_z(VectorXd& z,
const MatrixXd& J,
const VectorXd& d,
234 #ifdef OPTIMIZE_UPDATE_Z 235 z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
237 z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
241 inline void update_r(
const MatrixXd& R, VectorXd& r,
const VectorXd& d,
243 r.head(iq) = d.head(iq);
244 R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
248 inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d,
int& iq,
251 inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
252 VectorXd& u,
int nEqCon,
int& iq,
int l);
EiquadprogFast_status solve_quadprog(const MatrixXd &Hess, const VectorXd &g0, const MatrixXd &CE, const VectorXd &ce0, const MatrixXd &CI, const VectorXd &ci0, VectorXd &x)
Definition: eiquadprog-fast.cpp:230
void reset(int dim_qp, int num_eq, int num_ineq)
Definition: eiquadprog-fast.cpp:52
Definition: glpk-wrapper.hpp:24
bool is_inverse_provided_
Definition: eiquadprog-fast.hpp:157
bool setMaxIter(int maxIter)
Definition: eiquadprog-fast.hpp:97
MatrixXd m_J
Definition: eiquadprog-fast.hpp:156
Definition: eiquadprog-fast.hpp:77
int getMaxIter() const
Definition: eiquadprog-fast.hpp:95
Definition: eiquadprog-fast.hpp:72
int getActiveSetSize() const
Definition: eiquadprog-fast.hpp:107
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EiquadprogFast()
Definition: eiquadprog-fast.cpp:40
Eigen::VectorXd VectorXd
Definition: solver-abstract.hpp:35
EiquadprogFast_status solve_quadprog_sparse(const SpMat &Hess, const VectorXd &g0, const MatrixXd &CE, const VectorXd &ce0, const MatrixXd &CI, const VectorXd &ci0, VectorXd &x)
Definition: eiquadprog-fast.cpp:655
Eigen::MatrixXd MatrixXd
Definition: eiquadprog-fast.hpp:79
Definition: eiquadprog-fast.hpp:71
Eigen::SparseVector< double > SpVec
Definition: eiquadprog-fast.hpp:84
const VectorXi & getActiveSet() const
Definition: eiquadprog-fast.hpp:131
Eigen::VectorXd VectorXd
Definition: eiquadprog-fast.hpp:80
Definition: eiquadprog-fast.hpp:74
Definition: eiquadprog-fast.hpp:73
double getObjValue() const
Definition: eiquadprog-fast.hpp:117
Eigen::SparseVector< int > SpVeci
Definition: eiquadprog-fast.hpp:85
Definition: eiquadprog-fast.hpp:70
const VectorXd & getLagrangeMultipliers() const
Definition: eiquadprog-fast.hpp:122
virtual ~EiquadprogFast()
Definition: eiquadprog-fast.cpp:50
EiquadprogFast_status
Definition: eiquadprog-fast.hpp:69
Eigen::VectorXi VectorXi
Definition: eiquadprog-fast.hpp:81
Eigen::SparseMatrix< double > SpMat
Definition: eiquadprog-fast.hpp:83
Definition: eiquadprog-fast.hpp:62
Eigen::MatrixXd MatrixXd
Definition: solver-abstract.hpp:34
Eigen::VectorXi VectorXi
Definition: solver-abstract.hpp:36
int getIteratios() const
Definition: eiquadprog-fast.hpp:112