GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/eiquadprog/eiquadprog-rt.hpp Lines: 19 22 86.4 %
Date: 2021-03-10 23:02:29 Branches: 82 196 41.8 %

Line Branch Exec Source
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
#define OPTIMIZE_STEP_1_2         // compute s(x) = ci^T * x + ci0
25
#define OPTIMIZE_COMPUTE_D        // use noalias
26
#define OPTIMIZE_UPDATE_Z         // use noalias
27
#define OPTIMIZE_HESSIAN_INVERSE  // use solveInPlace
28
#define OPTIMIZE_UNCONSTR_MINIM
29
30
//#define USE_WARM_START
31
//#define PROFILE_EIQUADPROG
32
//#define DEBUG_STREAM(msg) std::cout<<msg;
33
#define DEBUG_STREAM(msg)
34
35
#ifdef PROFILE_EIQUADPROG
36
#define START_PROFILER_EIQUADPROG_RT(x) START_PROFILER(x)
37
#define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x)
38
#else
39
#define START_PROFILER_EIQUADPROG_RT(x)
40
#define STOP_PROFILER_EIQUADPROG_RT(x)
41
#endif
42
43
#define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION "EIQUADPROG_RT Chowlesky dec"
44
#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE "EIQUADPROG_RT Chowlesky inv"
45
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR "EIQUADPROG_RT ADD_EQ_CONSTR"
46
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1 "EIQUADPROG_RT ADD_EQ_CONSTR_1"
47
#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2 "EIQUADPROG_RT ADD_EQ_CONSTR_2"
48
#define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1"
49
#define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1"
50
#define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2"
51
#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM "EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
52
#define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2"
53
#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
54
#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
55
#define PROFILE_EIQUADPROG_STEP_2C "EIQUADPROG_RT STEP_2C"
56
57
#define DEFAULT_MAX_ITER 1000
58
59
template <int Rows, int Cols>
60
struct RtMatrixX {
61
  typedef Eigen::Matrix<double, Rows, Cols> d;
62
};
63
64
template <int Rows>
65
struct RtVectorX {
66
  typedef Eigen::Matrix<double, Rows, 1> d;
67
  typedef Eigen::Matrix<int, Rows, 1> i;
68
};
69
70
namespace eiquadprog {
71
72
namespace solvers {
73
74
#include "eiquadprog/eiquadprog-utils.hxx"
75
/**
76
 * Possible states of the solver.
77
 */
78
enum RtEiquadprog_status {
79
  RT_EIQUADPROG_OPTIMAL = 0,
80
  RT_EIQUADPROG_INFEASIBLE = 1,
81
  RT_EIQUADPROG_UNBOUNDED = 2,
82
  RT_EIQUADPROG_MAX_ITER_REACHED = 3,
83
  RT_EIQUADPROG_REDUNDANT_EQUALITIES = 4
84
};
85
86
template <int nVars, int nEqCon, int nIneqCon>
87
class RtEiquadprog {
88
 public:
89
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
90
91
  RtEiquadprog();
92
  virtual ~RtEiquadprog();
93
94
  int getMaxIter() const { return m_maxIter; }
95
96
  bool setMaxIter(int maxIter) {
97
    if (maxIter < 0) return false;
98
    m_maxIter = maxIter;
99
    return true;
100
  }
101
102
  /**
103
   * @return The size of the active set, namely the number of
104
   * active constraints (including the equalities).
105
   */
106
  int getActiveSetSize() const { return q; }
107
108
  /**
109
   * @return The number of active-set iteratios.
110
   */
111
  int getIteratios() const { return iter; }
112
113
  /**
114
   * @return The value of the objective function.
115
   */
116
8
  double getObjValue() const { return f_value; }
117
118
  /**
119
   * @return The Lagrange multipliers
120
   */
121
  const typename RtVectorX<nIneqCon + nEqCon>::d& getLagrangeMultipliers() const { return u; }
122
123
  /**
124
   * Return the active set, namely the indeces of active constraints.
125
   * The first nEqCon indexes are for the equalities and are negative.
126
   * The last nIneqCon indexes are for the inequalities and start from 0.
127
   * Only the first q elements of the return vector are valid, where q
128
   * is the size of the active set.
129
   * @return The set of indexes of the active constraints.
130
   */
131
  const typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const { return A; }
132
133
  /**
134
   * solves the problem
135
   * min. x' Hess x + 2 g0' x
136
   * s.t. CE x + ce0 = 0
137
   *      CI x + ci0 >= 0
138
   */
139
  RtEiquadprog_status solve_quadprog(const typename RtMatrixX<nVars, nVars>::d& Hess,
140
                                     const typename RtVectorX<nVars>::d& g0,
141
                                     const typename RtMatrixX<nEqCon, nVars>::d& CE,
142
                                     const typename RtVectorX<nEqCon>::d& ce0,
143
                                     const typename RtMatrixX<nIneqCon, nVars>::d& CI,
144
                                     const typename RtVectorX<nIneqCon>::d& ci0, typename RtVectorX<nVars>::d& x);
145
146
  typename RtMatrixX<nVars, nVars>::d m_J;  // J * J' = Hessian
147
  bool is_inverse_provided_;
148
149
 private:
150
  int m_maxIter;   /// max number of active-set iterations
151
  double f_value;  /// current value of cost function
152
153
  Eigen::LLT<typename RtMatrixX<nVars, nVars>::d, Eigen::Lower> chol_;
154
  double solver_return_;
155
156
  /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
157
  typename RtMatrixX<nVars, nVars>::d R;
158
159
  /// CI*x+ci0
160
  typename RtVectorX<nIneqCon>::d s;
161
162
  /// infeasibility multipliers, i.e. negative step direction in dual space
163
  typename RtVectorX<nIneqCon + nEqCon>::d r;
164
165
  /// Lagrange multipliers
166
  typename RtVectorX<nIneqCon + nEqCon>::d u;
167
168
  /// step direction in primal space
169
  typename RtVectorX<nVars>::d z;
170
171
  /// J' np
172
  typename RtVectorX<nVars>::d d;
173
174
  /// current constraint normal
175
  typename RtVectorX<nVars>::d np;
176
177
  /// active set (indeces of active constraints)
178
  /// the first nEqCon indeces are for the equalities and are negative
179
  /// the last nIneqCon indeces are for the inequalities are start from 0
180
  typename RtVectorX<nIneqCon + nEqCon>::i A;
181
182
  /// initialized as K \ A
183
  /// iai(i)=-1 iff inequality constraint i is in the active set
184
  /// iai(i)=i otherwise
185
  typename RtVectorX<nIneqCon>::i iai;
186
187
  /// initialized as [1, ..., 1, .]
188
  /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
189
  /// if adding ineq constraint i fails => iaexcl(i)=0
190
  /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
191
  /// iaexcl(i)=1 otherwise
192
  typename RtVectorX<nIneqCon>::i iaexcl;
193
194
  typename RtVectorX<nVars>::d x_old;              // old value of x
195
  typename RtVectorX<nIneqCon + nEqCon>::d u_old;  // old value of u
196
  typename RtVectorX<nIneqCon + nEqCon>::i A_old;  // old value of A
197
198
#ifdef OPTIMIZE_ADD_CONSTRAINT
199
  typename RtVectorX<nVars>::d T1;  // tmp vector
200
#endif
201
202
  /// size of the active set A (containing the indices of the active constraints)
203
  int q;
204
205
  /// number of active-set iterations
206
  int iter;
207
208
  template <typename Scalar>
209
7
  inline Scalar distance(Scalar a, Scalar b) {
210
    Scalar a1, b1, t;
211
7
    a1 = std::abs(a);
212
7
    b1 = std::abs(b);
213



7
    if (a1 > b1) {
214
3
      t = (b1 / a1);
215
3
      return a1 * std::sqrt(1.0 + t * t);
216



4
    } else if (b1 > a1) {
217
      t = (a1 / b1);
218
      return b1 * std::sqrt(1.0 + t * t);
219
    }
220
4
    return a1 * std::sqrt(2.0);
221
  }
222
223
13
  inline void compute_d(typename RtVectorX<nVars>::d& d, const typename RtMatrixX<nVars, nVars>::d& J,
224
                        const typename RtVectorX<nVars>::d& np) {
225
#ifdef OPTIMIZE_COMPUTE_D
226










13
    d.noalias() = J.adjoint() * np;
227
#else
228
    d = J.adjoint() * np;
229
#endif
230
13
  }
231
232
12
  inline void update_z(typename RtVectorX<nVars>::d& z, const typename RtMatrixX<nVars, nVars>::d& J,
233
                       const typename RtVectorX<nVars>::d& d, int iq) {
234
#ifdef OPTIMIZE_UPDATE_Z
235














12
    z.noalias() = J.rightCols(nVars - iq) * d.tail(nVars - iq);
236
#else
237
    z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
238
#endif
239
12
  }
240
241
13
  inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R, typename RtVectorX<nIneqCon + nEqCon>::d& r,
242
                       const typename RtVectorX<nVars>::d& d, int iq) {
243







13
    r.head(iq) = d.head(iq);
244










13
    R.topLeftCorner(iq, iq).template triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
245
13
  }
246
247
  bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
248
                      typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
249
250
  void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R, typename RtMatrixX<nVars, nVars>::d& J,
251
                         typename RtVectorX<nIneqCon + nEqCon>::i& A, typename RtVectorX<nIneqCon + nEqCon>::d& u,
252
                         int& iq, int l);
253
};
254
255
} /* namespace solvers */
256
} /* namespace eiquadprog */
257
258
#include "eiquadprog/eiquadprog-rt.hxx"
259
/* --- Details -------------------------------------------------------------------- */
260
261
#endif /* __eiquadprog_rt_hpp__ */