19 #ifndef __eiquadprog_rt_hpp__ 20 #define __eiquadprog_rt_hpp__ 22 #include <Eigen/Dense> 26 #define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0 27 #define OPTIMIZE_COMPUTE_D // use noalias 28 #define OPTIMIZE_UPDATE_Z // use noalias 29 #define OPTIMIZE_HESSIAN_INVERSE // use solveInPlace 30 #define OPTIMIZE_UNCONSTR_MINIM 35 #define DEBUG_STREAM(msg) 37 #ifdef PROFILE_EIQUADPROG 38 #define START_PROFILER_EIQUADPROG_RT(x) START_PROFILER(x) 39 #define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x) 41 #define START_PROFILER_EIQUADPROG_RT(x) 42 #define STOP_PROFILER_EIQUADPROG_RT(x) 45 #define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION "EIQUADPROG_RT Chowlesky dec" 46 #define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE "EIQUADPROG_RT Chowlesky inv" 47 #define PROFILE_EIQUADPROG_ADD_EQ_CONSTR "EIQUADPROG_RT ADD_EQ_CONSTR" 48 #define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1 "EIQUADPROG_RT ADD_EQ_CONSTR_1" 49 #define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2 "EIQUADPROG_RT ADD_EQ_CONSTR_2" 50 #define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1" 51 #define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1" 52 #define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2" 53 #define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM \ 54 "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM" 55 #define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2" 56 #define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A" 57 #define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B" 58 #define PROFILE_EIQUADPROG_STEP_2C "EIQUADPROG_RT STEP_2C" 60 #define DEFAULT_MAX_ITER 1000 62 template <
int Rows,
int Cols>
64 typedef Eigen::Matrix<double, Rows, Cols>
d;
69 typedef Eigen::Matrix<double, Rows, 1>
d;
70 typedef Eigen::Matrix<int, Rows, 1>
i;
88 template <
int nVars,
int nEqCon,
int nIneqCon>
91 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94 virtual ~RtEiquadprog();
99 if (maxIter < 0)
return false;
162 Eigen::LLT<typename RtMatrixX<nVars, nVars>::d, Eigen::Lower> chol_;
163 double solver_return_;
208 #ifdef OPTIMIZE_ADD_CONSTRAINT 219 template <
typename Scalar>
220 inline Scalar
distance(Scalar a, Scalar b) {
226 return a1 * std::sqrt(1.0 + t * t);
227 }
else if (b1 > a1) {
229 return b1 * std::sqrt(1.0 + t * t);
231 return a1 * std::sqrt(2.0);
237 #ifdef OPTIMIZE_COMPUTE_D 238 d.noalias() = J.adjoint() * np;
240 d = J.adjoint() * np;
247 #ifdef OPTIMIZE_UPDATE_Z 248 z.noalias() = J.rightCols(nVars - iq) * d.tail(nVars - iq);
250 z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
257 r.head(iq) = d.head(iq);
258 R.topLeftCorner(iq, iq)
259 .template triangularView<Eigen::Upper>()
260 .solveInPlace(r.head(iq));
RtMatrixX< nVars, nVars >::d m_J
Definition: eiquadprog-rt.hpp:155
Eigen::Matrix< double, Rows, 1 > d
Definition: eiquadprog-rt.hpp:69
Definition: eiquadprog-rt.hpp:68
Definition: eiquadprog-rt.hpp:89
bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d, size_t &iq, double &R_norm)
void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J, const Eigen::VectorXd &np)
Definition: eiquadprog.hpp:87
Definition: eiquadprog-fast.hpp:63
const RtVectorX< nIneqCon+nEqCon >::i & getActiveSet() const
Definition: eiquadprog-rt.hpp:136
Definition: eiquadprog-rt.hpp:85
bool setMaxIter(int maxIter)
Definition: eiquadprog-rt.hpp:98
int getMaxIter() const
Definition: eiquadprog-rt.hpp:96
Definition: eiquadprog-rt.hpp:82
const RtVectorX< nIneqCon+nEqCon >::d & getLagrangeMultipliers() const
Definition: eiquadprog-rt.hpp:123
void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r, const Eigen::VectorXd &d, size_t iq)
Definition: eiquadprog.hpp:97
int getIteratios() const
Definition: eiquadprog-rt.hpp:113
Scalar distance(Scalar a, Scalar b)
Compute sqrt(a^2 + b^2)
Definition: eiquadprog-utils.hxx:12
bool is_inverse_provided_
Definition: eiquadprog-rt.hpp:156
Definition: eiquadprog-rt.hpp:83
Definition: eiquadprog-rt.hpp:63
Eigen::Matrix< int, Rows, 1 > i
Definition: eiquadprog-rt.hpp:70
double getObjValue() const
Definition: eiquadprog-rt.hpp:118
double solve_quadprog(Eigen::LLT< Eigen::MatrixXd, Eigen::Lower > &chol, double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0, Eigen::VectorXd &x, Eigen::VectorXi &A, size_t &q)
Definition: eiquadprog-rt.hpp:81
Definition: eiquadprog-rt.hpp:84
RtEiquadprog_status
Definition: eiquadprog-rt.hpp:80
void delete_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXi &A, Eigen::VectorXd &u, size_t p, size_t &iq, size_t l)
void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J, const Eigen::VectorXd &d, size_t iq)
Definition: eiquadprog.hpp:92
int getActiveSetSize() const
Definition: eiquadprog-rt.hpp:108
Eigen::Matrix< double, Rows, Cols > d
Definition: eiquadprog-rt.hpp:64