19 #ifndef EIQUADPROGFAST_HPP_
20 #define EIQUADPROGFAST_HPP_
22 #include <Eigen/Dense>
24 #define OPTIMIZE_STEP_1_2
25 #define OPTIMIZE_COMPUTE_D
26 #define OPTIMIZE_UPDATE_Z
27 #define OPTIMIZE_HESSIAN_INVERSE
28 #define OPTIMIZE_UNCONSTR_MINIM
34 #define DEBUG_STREAM(msg)
36 #ifdef PROFILE_EIQUADPROG
37 #define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
38 #define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
40 #define START_PROFILER_EIQUADPROG_FAST(x)
41 #define STOP_PROFILER_EIQUADPROG_FAST(x)
44 #define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
45 #define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv"
46 #define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
47 #define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
48 #define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
49 #define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
50 #define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
51 #define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
52 #define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \
53 "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
54 #define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
55 #define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
56 #define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
57 #define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
59 #define DEFAULT_MAX_ITER 1000
79 typedef Eigen::MatrixXd MatrixXd;
80 typedef Eigen::VectorXd VectorXd;
81 typedef Eigen::VectorXi VectorXi;
84 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89 void reset(
size_t dim_qp,
size_t num_eq,
size_t num_ineq);
94 if (maxIter < 0)
return false;
137 const MatrixXd& CE,
const VectorXd& ce0,
138 const MatrixXd& CI,
const VectorXd& ci0,
152 Eigen::LLT<MatrixXd, Eigen::Lower> chol_;
197 #ifdef OPTIMIZE_ADD_CONSTRAINT
208 inline void compute_d(VectorXd& d,
const MatrixXd& J,
const VectorXd& np) {
209 #ifdef OPTIMIZE_COMPUTE_D
210 d.noalias() = J.adjoint() * np;
212 d = J.adjoint() * np;
216 inline void update_z(VectorXd& z,
const MatrixXd& J,
const VectorXd& d,
218 #ifdef OPTIMIZE_UPDATE_Z
219 z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
221 z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
225 inline void update_r(
const MatrixXd& R, VectorXd& r,
const VectorXd& d,
227 r.head(iq) = d.head(iq);
228 R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
232 inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d,
size_t& iq,
235 inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
236 VectorXd& u,
size_t nEqCon,
size_t& iq,
Definition: eiquadprog-fast.hpp:78
double getObjValue() const
Definition: eiquadprog-fast.hpp:113
int getIteratios() const
Definition: eiquadprog-fast.hpp:108
size_t getActiveSetSize() const
Definition: eiquadprog-fast.hpp:103
int getMaxIter() const
Definition: eiquadprog-fast.hpp:91
virtual ~EiquadprogFast()
Definition: eiquadprog-fast.cpp:18
void reset(size_t dim_qp, size_t num_eq, size_t num_ineq)
Definition: eiquadprog-fast.cpp:20
const VectorXi & getActiveSet() const
Definition: eiquadprog-fast.hpp:128
MatrixXd m_J
Definition: eiquadprog-fast.hpp:141
bool setMaxIter(int maxIter)
Definition: eiquadprog-fast.hpp:93
const VectorXd & getLagrangeMultipliers() const
Definition: eiquadprog-fast.hpp:118
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:189
bool is_inverse_provided_
Definition: eiquadprog-fast.hpp:142
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EiquadprogFast()
Definition: eiquadprog-fast.cpp:8
EiquadprogFast_status
Definition: eiquadprog-fast.hpp:70
@ EIQUADPROG_FAST_MAX_ITER_REACHED
Definition: eiquadprog-fast.hpp:74
@ EIQUADPROG_FAST_REDUNDANT_EQUALITIES
Definition: eiquadprog-fast.hpp:75
@ EIQUADPROG_FAST_OPTIMAL
Definition: eiquadprog-fast.hpp:71
@ EIQUADPROG_FAST_INFEASIBLE
Definition: eiquadprog-fast.hpp:72
@ EIQUADPROG_FAST_UNBOUNDED
Definition: eiquadprog-fast.hpp:73
Definition: eiquadprog-fast.hpp:63