19#ifndef __eiquadprog_rt_hpp__
20#define __eiquadprog_rt_hpp__
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
62template <
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;
88template <
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
int getIteratios() const
Definition eiquadprog-rt.hpp:113
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RtEiquadprog()
Definition eiquadprog-rt.hxx:29
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
const RtVectorX< nIneqCon+nEqCon >::d & getLagrangeMultipliers() const
Definition eiquadprog-rt.hpp:123
const RtVectorX< nIneqCon+nEqCon >::i & getActiveSet() const
Definition eiquadprog-rt.hpp:136
int getActiveSetSize() const
Definition eiquadprog-rt.hpp:108
RtMatrixX< nVars, nVars >::d m_J
Definition eiquadprog-rt.hpp:155
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