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 
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 
62 template <int Rows, int Cols>
63 struct RtMatrixX {
64  typedef Eigen::Matrix<double, Rows, Cols> d;
65 };
66 
67 template <int Rows>
68 struct RtVectorX {
69  typedef Eigen::Matrix<double, Rows, 1> d;
70  typedef Eigen::Matrix<int, Rows, 1> i;
71 };
72 
73 namespace eiquadprog {
74 
75 namespace solvers {
76 
86 };
87 
88 template <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 
124  const {
125  return u;
126  }
127 
137  return A;
138  }
139 
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
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 
174 
177 
179  typename RtVectorX<nVars>::d z;
180 
182  typename RtVectorX<nVars>::d d;
183 
185  typename RtVectorX<nVars>::d np;
186 
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,
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,
270  typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq,
271  int l);
272 };
273 
274 } /* namespace solvers */
275 } /* namespace eiquadprog */
276 
278 /* --- Details
279  * -------------------------------------------------------------------- */
280 
281 #endif /* __eiquadprog_rt_hpp__ */
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