eiquadprog: include/eiquadprog/eiquadprog-rt.hpp Source File

eiquadprog 1.3.2 C++ reimplementation of eiquadprog

Loading...
Searching...
No Matches
eiquadprog-rt.hpp
Go to the documentation of this file.
1//
2// Copyright (c) 2017 CNRS
3//
4// This file is part of eiquadprog.
5//
6// eiquadprog is free software: you can redistribute it and/or modify
7// it under the terms of the GNU Lesser General Public License as published by
8// the Free Software Foundation, either version 3 of the License, or
9//(at your option) any later version.
10
11// eiquadprog is distributed in the hope that it will be useful,
12// but WITHOUT ANY WARRANTY; without even the implied warranty of
13// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14// GNU Lesser General Public License for more details.
15
16// You should have received a copy of the GNU Lesser General Public License
17// along with eiquadprog. If not, see <https://www.gnu.org/licenses/>.
18
19#ifndef __eiquadprog_rt_hpp__
20#define __eiquadprog_rt_hpp__
21
22#include <Eigen/Dense>
23
24#include "eiquadprog/eiquadprog-utils.hxx"
25
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
31
32// #define USE_WARM_START
33// #define PROFILE_EIQUADPROG
34// #define DEBUG_STREAM(msg) std::cout<<msg;
35#define DEBUG_STREAM(msg)
36
37#ifdef PROFILE_EIQUADPROG
38#define START_PROFILER_EIQUADPROG_RT(x) START_PROFILER(x)
39#define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x)
40#else
41#define START_PROFILER_EIQUADPROG_RT(x)
42#define STOP_PROFILER_EIQUADPROG_RT(x)
43#endif
44
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"
59
60#define DEFAULT_MAX_ITER 1000
61
62template <int Rows, int Cols>
63 struct RtMatrixX {
64 typedef Eigen::Matrix<double, Rows, Cols> d;
65};
66
67template <int Rows>
68 struct RtVectorX {
69 typedef Eigen::Matrix<double, Rows, 1> d;
70 typedef Eigen::Matrix<int, Rows, 1> i;
71};
72
73namespace eiquadprog {
74
75namespace solvers {
76
80 enum RtEiquadprog_status {
81 RT_EIQUADPROG_OPTIMAL = 0,
82 RT_EIQUADPROG_INFEASIBLE = 1,
83 RT_EIQUADPROG_UNBOUNDED = 2,
84 RT_EIQUADPROG_MAX_ITER_REACHED = 3,
85 RT_EIQUADPROG_REDUNDANT_EQUALITIES = 4
86};
87
88template <int nVars, int nEqCon, int nIneqCon>
89 class RtEiquadprog {
90 public:
91 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
92
93 RtEiquadprog();
94 virtual ~RtEiquadprog();
95
96 int getMaxIter() const { return m_maxIter; }
97
98 bool setMaxIter(int maxIter) {
99 if (maxIter < 0) return false;
100 m_maxIter = maxIter;
101 return true;
102 }
103
108 int getActiveSetSize() const { return q; }
109
113 int getIteratios() const { return iter; }
114
118 double getObjValue() const { return f_value; }
119
123 const typename RtVectorX<nIneqCon + nEqCon>::d& getLagrangeMultipliers()
124 const {
125 return u;
126 }
127
136 const typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const {
137 return A;
138 }
139
146 RtEiquadprog_status solve_quadprog(
147 const typename RtMatrixX<nVars, nVars>::d& Hess,
148 const typename RtVectorX<nVars>::d& g0,
149 const typename RtMatrixX<nEqCon, nVars>::d& CE,
150 const typename RtVectorX<nEqCon>::d& ce0,
151 const typename RtMatrixX<nIneqCon, nVars>::d& CI,
152 const typename RtVectorX<nIneqCon>::d& ci0,
153 typename RtVectorX<nVars>::d& x);
154
155 typename RtMatrixX<nVars, nVars>::d m_J; // J * J' = Hessian
156 bool is_inverse_provided_;
157
158 private:
159 int m_maxIter;
160 double f_value;
161
162 Eigen::LLT<typename RtMatrixX<nVars, nVars>::d, Eigen::Lower> chol_;
163 double solver_return_;
164
167 typename RtMatrixX<nVars, nVars>::d R;
168
170 typename RtVectorX<nIneqCon>::d s;
171
173 typename RtVectorX<nIneqCon + nEqCon>::d r;
174
176 typename RtVectorX<nIneqCon + nEqCon>::d u;
177
179 typename RtVectorX<nVars>::d z;
180
182 typename RtVectorX<nVars>::d d;
183
185 typename RtVectorX<nVars>::d np;
186
190 typename RtVectorX<nIneqCon + nEqCon>::i A;
191
195 typename RtVectorX<nIneqCon>::i iai;
196
202 typename RtVectorX<nIneqCon>::i iaexcl;
203
204 typename RtVectorX<nVars>::d x_old; // old value of x
205 typename RtVectorX<nIneqCon + nEqCon>::d u_old; // old value of u
206 typename RtVectorX<nIneqCon + nEqCon>::i A_old; // old value of A
207
208#ifdef OPTIMIZE_ADD_CONSTRAINT
209 typename RtVectorX<nVars>::d T1; // tmp vector
210#endif
211
214 int q;
215
217 int iter;
218
219 template <typename Scalar>
220 inline Scalar distance(Scalar a, Scalar b) {
221 Scalar a1, b1, t;
222 a1 = std::abs(a);
223 b1 = std::abs(b);
224 if (a1 > b1) {
225 t = (b1 / a1);
226 return a1 * std::sqrt(1.0 + t * t);
227 } else if (b1 > a1) {
228 t = (a1 / b1);
229 return b1 * std::sqrt(1.0 + t * t);
230 }
231 return a1 * std::sqrt(2.0);
232 }
233
234 inline void compute_d(typename RtVectorX<nVars>::d& d,
235 const typename RtMatrixX<nVars, nVars>::d& J,
236 const typename RtVectorX<nVars>::d& np) {
237#ifdef OPTIMIZE_COMPUTE_D
238 d.noalias() = J.adjoint() * np;
239#else
240 d = J.adjoint() * np;
241#endif
242 }
243
244 inline void update_z(typename RtVectorX<nVars>::d& z,
245 const typename RtMatrixX<nVars, nVars>::d& J,
246 const typename RtVectorX<nVars>::d& d, int iq) {
247#ifdef OPTIMIZE_UPDATE_Z
248 z.noalias() = J.rightCols(nVars - iq) * d.tail(nVars - iq);
249#else
250 z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
251#endif
252 }
253
254 inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R,
255 typename RtVectorX<nIneqCon + nEqCon>::d& r,
256 const typename RtVectorX<nVars>::d& d, int iq) {
257 r.head(iq) = d.head(iq);
258 R.topLeftCorner(iq, iq)
259 .template triangularView<Eigen::Upper>()
260 .solveInPlace(r.head(iq));
261 }
262
263 bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
264 typename RtMatrixX<nVars, nVars>::d& J,
265 typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
266
267 void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R,
268 typename RtMatrixX<nVars, nVars>::d& J,
269 typename RtVectorX<nIneqCon + nEqCon>::i& A,
270 typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq,
271 int l);
272};
273
274} /* namespace solvers */
275} /* namespace eiquadprog */
276
277#include "eiquadprog/eiquadprog-rt.hxx"
278/* --- Details
279 * -------------------------------------------------------------------- */
280
281#endif /* __eiquadprog_rt_hpp__ */
eiquadprog::solvers::RtEiquadprog
Definition eiquadprog-rt.hpp:89
eiquadprog::solvers::RtEiquadprog::setMaxIter
bool setMaxIter(int maxIter)
Definition eiquadprog-rt.hpp:98
eiquadprog::solvers::RtEiquadprog::getIteratios
int getIteratios() const
Definition eiquadprog-rt.hpp:113
eiquadprog::solvers::RtEiquadprog::RtEiquadprog
EIGEN_MAKE_ALIGNED_OPERATOR_NEW RtEiquadprog()
Definition eiquadprog-rt.hxx:29
eiquadprog::solvers::RtEiquadprog::solve_quadprog
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
eiquadprog::solvers::RtEiquadprog::~RtEiquadprog
virtual ~RtEiquadprog()
Definition eiquadprog-rt.hxx:38
eiquadprog::solvers::RtEiquadprog::getObjValue
double getObjValue() const
Definition eiquadprog-rt.hpp:118
eiquadprog::solvers::RtEiquadprog::is_inverse_provided_
bool is_inverse_provided_
Definition eiquadprog-rt.hpp:156
eiquadprog::solvers::RtEiquadprog::getLagrangeMultipliers
const RtVectorX< nIneqCon+nEqCon >::d & getLagrangeMultipliers() const
Definition eiquadprog-rt.hpp:123
eiquadprog::solvers::RtEiquadprog::getActiveSet
const RtVectorX< nIneqCon+nEqCon >::i & getActiveSet() const
Definition eiquadprog-rt.hpp:136
eiquadprog::solvers::RtEiquadprog::getActiveSetSize
int getActiveSetSize() const
Definition eiquadprog-rt.hpp:108
eiquadprog::solvers::RtEiquadprog::m_J
RtMatrixX< nVars, nVars >::d m_J
Definition eiquadprog-rt.hpp:155
eiquadprog::solvers::RtEiquadprog::getMaxIter
int getMaxIter() const
Definition eiquadprog-rt.hpp:96
eiquadprog-rt.hxx
eiquadprog-utils.hxx
eiquadprog::solvers::RtEiquadprog_status
RtEiquadprog_status
Definition eiquadprog-rt.hpp:80
eiquadprog::solvers::RT_EIQUADPROG_REDUNDANT_EQUALITIES
@ RT_EIQUADPROG_REDUNDANT_EQUALITIES
Definition eiquadprog-rt.hpp:85
eiquadprog::solvers::RT_EIQUADPROG_INFEASIBLE
@ RT_EIQUADPROG_INFEASIBLE
Definition eiquadprog-rt.hpp:82
eiquadprog::solvers::RT_EIQUADPROG_MAX_ITER_REACHED
@ RT_EIQUADPROG_MAX_ITER_REACHED
Definition eiquadprog-rt.hpp:84
eiquadprog::solvers::RT_EIQUADPROG_UNBOUNDED
@ RT_EIQUADPROG_UNBOUNDED
Definition eiquadprog-rt.hpp:83
eiquadprog::solvers::RT_EIQUADPROG_OPTIMAL
@ RT_EIQUADPROG_OPTIMAL
Definition eiquadprog-rt.hpp:81
eiquadprog
Definition eiquadprog-fast.hpp:64
RtMatrixX
Definition eiquadprog-rt.hpp:63
RtMatrixX::d
Eigen::Matrix< double, Rows, Cols > d
Definition eiquadprog-rt.hpp:64
RtVectorX
Definition eiquadprog-rt.hpp:68
RtVectorX::i
Eigen::Matrix< int, Rows, 1 > i
Definition eiquadprog-rt.hpp:70
RtVectorX::d
Eigen::Matrix< double, Rows, 1 > d
Definition eiquadprog-rt.hpp:69