GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/eiquadprog/eiquadprog-fast.hpp Lines: 11 12 91.7 %
Date: 2021-03-10 23:02:29 Branches: 13 26 50.0 %

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 EIQUADPROGFAST_HPP_
20
#define EIQUADPROGFAST_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
26
#define OPTIMIZE_UPDATE_Z
27
#define OPTIMIZE_HESSIAN_INVERSE
28
#define OPTIMIZE_UNCONSTR_MINIM
29
30
//#define USE_WARM_START
31
//#define PROFILE_EIQUADPROG
32
33
//#define DEBUG_STREAM(msg) std::cout<<msg;
34
#define DEBUG_STREAM(msg)
35
36
#ifdef PROFILE_EIQUADPROG
37
#define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
38
#define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
39
#else
40
#define START_PROFILER_EIQUADPROG_FAST(x)
41
#define STOP_PROFILER_EIQUADPROG_FAST(x)
42
#endif
43
44
#define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
45
#define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv"
46
#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
47
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
48
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
49
#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
50
#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
51
#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
52
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
53
#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
54
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
55
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
56
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
57
58
#define DEFAULT_MAX_ITER 1000
59
60
namespace eiquadprog {
61
62
namespace solvers {
63
64
#include "eiquadprog/eiquadprog-utils.hxx"
65
66
/**
67
 * Possible states of the solver.
68
 */
69
enum EiquadprogFast_status {
70
  EIQUADPROG_FAST_OPTIMAL = 0,
71
  EIQUADPROG_FAST_INFEASIBLE = 1,
72
  EIQUADPROG_FAST_UNBOUNDED = 2,
73
  EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
74
  EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
75
};
76
77
class EiquadprogFast {
78
  typedef Eigen::MatrixXd MatrixXd;
79
  typedef Eigen::VectorXd VectorXd;
80
  typedef Eigen::VectorXi VectorXi;
81
82
 public:
83
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
84
85
  EiquadprogFast();
86
  virtual ~EiquadprogFast();
87
88
  void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);
89
90
  int getMaxIter() const { return m_maxIter; }
91
92
  bool setMaxIter(int maxIter) {
93
    if (maxIter < 0) return false;
94
    m_maxIter = maxIter;
95
    return true;
96
  }
97
98
  /**
99
   * @return The size of the active set, namely the number of
100
   * active constraints (including the equalities).
101
   */
102
  size_t getActiveSetSize() const { return q; }
103
104
  /**
105
   * @return The number of active-set iteratios.
106
   */
107
  int getIteratios() const { return iter; }
108
109
  /**
110
   * @return The value of the objective function.
111
   */
112
7
  double getObjValue() const { return f_value; }
113
114
  /**
115
   * @return The Lagrange multipliers
116
   */
117
  const VectorXd& getLagrangeMultipliers() const { return u; }
118
119
  /**
120
   * Return the active set, namely the indeces of active constraints.
121
   * The first nEqCon indexes are for the equalities and are negative.
122
   * The last nIneqCon indexes are for the inequalities and start from 0.
123
   * Only the first q elements of the return vector are valid, where q
124
   * is the size of the active set.
125
   * @return The set of indexes of the active constraints.
126
   */
127
  const VectorXi& getActiveSet() const { return A; }
128
129
  /**
130
   * solves the problem
131
   * min. x' Hess x + 2 g0' x
132
   * s.t. CE x + ce0 = 0
133
   *      CI x + ci0 >= 0
134
   */
135
  EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
136
                                       const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);
137
138
  MatrixXd m_J;  // J * J' = Hessian <nVars,nVars>::d
139
  bool is_inverse_provided_;
140
141
 private:
142
  size_t m_nVars;
143
  size_t m_nEqCon;
144
  size_t m_nIneqCon;
145
146
  int m_maxIter;   /// max number of active-set iterations
147
  double f_value;  /// current value of cost function
148
149
  Eigen::LLT<MatrixXd, Eigen::Lower> chol_;  // <nVars,nVars>::d
150
151
  /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
152
  MatrixXd R;  // <nVars,nVars>::d
153
154
  /// CI*x+ci0
155
  VectorXd s;  // <nIneqCon>::d
156
157
  /// infeasibility multipliers, i.e. negative step direction in dual space
158
  VectorXd r;  // <nIneqCon+nEqCon>::d
159
160
  /// Lagrange multipliers
161
  VectorXd u;  // <nIneqCon+nEqCon>::d
162
163
  /// step direction in primal space
164
  VectorXd z;  // <nVars>::d
165
166
  /// J' np
167
  VectorXd d;  //<nVars>::d
168
169
  /// current constraint normal
170
  VectorXd np;  //<nVars>::d
171
172
  /// active set (indeces of active constraints)
173
  /// the first nEqCon indeces are for the equalities and are negative
174
  /// the last nIneqCon indeces are for the inequalities are start from 0
175
  VectorXi A;  // <nIneqCon+nEqCon>
176
177
  /// initialized as K \ A
178
  /// iai(i)=-1 iff inequality constraint i is in the active set
179
  /// iai(i)=i otherwise
180
  VectorXi iai;  // <nIneqCon>::i
181
182
  /// initialized as [1, ..., 1, .]
183
  /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
184
  /// if adding ineq constraint i fails => iaexcl(i)=0
185
  /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
186
  /// iaexcl(i)=1 otherwise
187
  VectorXi iaexcl;  //<nIneqCon>::i
188
189
  VectorXd x_old;  // old value of x <nVars>::d
190
  VectorXd u_old;  // old value of u <nIneqCon+nEqCon>::d
191
  VectorXi A_old;  // old value of A <nIneqCon+nEqCon>::i
192
193
#ifdef OPTIMIZE_ADD_CONSTRAINT
194
  VectorXd T1;  /// tmp variable used in add_constraint
195
#endif
196
197
  /// size of the active set A (containing the indices of the active constraints)
198
  size_t q;
199
200
  /// number of active-set iterations
201
  int iter;
202
203
12
  inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
204
#ifdef OPTIMIZE_COMPUTE_D
205

12
    d.noalias() = J.adjoint() * np;
206
#else
207
    d = J.adjoint() * np;
208
#endif
209
12
  }
210
211
11
  inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, size_t iq) {
212
#ifdef OPTIMIZE_UPDATE_Z
213


11
    z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
214
#else
215
    z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
216
#endif
217
11
  }
218
219
12
  inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, size_t iq) {
220

12
    r.head(iq) = d.head(iq);
221

12
    R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
222
12
  }
223
224
  inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm);
225
226
  inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
227
                                size_t l);
228
};
229
230
} /* namespace solvers */
231
} /* namespace eiquadprog */
232
233
#endif /* EIQUADPROGFAST_HPP_ */