19 #ifndef __eiquadprog_rt_hpp__
20 #define __eiquadprog_rt_hpp__
22 #include <Eigen/Dense>
26 #define OPTIMIZE_STEP_1_2
27 #define OPTIMIZE_COMPUTE_D
28 #define OPTIMIZE_UPDATE_Z
29 #define OPTIMIZE_HESSIAN_INVERSE
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
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));
Definition: eiquadprog-rt.hpp:89
bool setMaxIter(int maxIter)
Definition: eiquadprog-rt.hpp:98
const RtVectorX< nIneqCon+nEqCon >::d & getLagrangeMultipliers() const
Definition: eiquadprog-rt.hpp:123
int getIteratios() const
Definition: eiquadprog-rt.hpp:113
const RtVectorX< nIneqCon+nEqCon >::i & getActiveSet() const
Definition: eiquadprog-rt.hpp:136
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RtEiquadprog()
Definition: eiquadprog-rt.hxx:29
RtMatrixX< nVars, nVars >::d m_J
Definition: eiquadprog-rt.hpp:155
RtEiquadprog_status solve_quadprog(const typename RtMatrixX< nVars, nVars >::d &Hess, const typename RtVectorX< nVars >::d &g0, const typename RtMatrixX< nEqCon, nVars >::d &CE, const typename RtVectorX< nEqCon >::d &ce0, const typename RtMatrixX< nIneqCon, nVars >::d &CI, const typename RtVectorX< nIneqCon >::d &ci0, typename RtVectorX< nVars >::d &x)
Definition: eiquadprog-rt.hxx:191
virtual ~RtEiquadprog()
Definition: eiquadprog-rt.hxx:38
double getObjValue() const
Definition: eiquadprog-rt.hpp:118
bool is_inverse_provided_
Definition: eiquadprog-rt.hpp:156
int getActiveSetSize() const
Definition: eiquadprog-rt.hpp:108
int getMaxIter() const
Definition: eiquadprog-rt.hpp:96
RtEiquadprog_status
Definition: eiquadprog-rt.hpp:80
@ RT_EIQUADPROG_REDUNDANT_EQUALITIES
Definition: eiquadprog-rt.hpp:85
@ RT_EIQUADPROG_INFEASIBLE
Definition: eiquadprog-rt.hpp:82
@ RT_EIQUADPROG_MAX_ITER_REACHED
Definition: eiquadprog-rt.hpp:84
@ RT_EIQUADPROG_UNBOUNDED
Definition: eiquadprog-rt.hpp:83
@ RT_EIQUADPROG_OPTIMAL
Definition: eiquadprog-rt.hpp:81
Definition: eiquadprog-fast.hpp:63
Definition: eiquadprog-rt.hpp:63
Eigen::Matrix< double, Rows, Cols > d
Definition: eiquadprog-rt.hpp:64
Definition: eiquadprog-rt.hpp:68
Eigen::Matrix< int, Rows, 1 > i
Definition: eiquadprog-rt.hpp:70
Eigen::Matrix< double, Rows, 1 > d
Definition: eiquadprog-rt.hpp:69