eiquadprog-fast.hpp 6.55 KB
Newer Older
1
2
3
4
//
// Copyright (c) 2017 CNRS
//

5
6
#ifndef EIQUADPROGFAST_HPP_
#define EIQUADPROGFAST_HPP_
7
8
9

#include <Eigen/Dense>

Guilhem Saurel's avatar
Guilhem Saurel committed
10
#define OPTIMIZE_STEP_1_2  // compute s(x) = ci^T * x + ci0
11
12
13
14
15
16
17
18
19
20
21
22
23
#define OPTIMIZE_COMPUTE_D
#define OPTIMIZE_UPDATE_Z
#define OPTIMIZE_HESSIAN_INVERSE
#define OPTIMIZE_UNCONSTR_MINIM

//#define USE_WARM_START
//#define PROFILE_EIQUADPROG

//#define DEBUG_STREAM(msg) std::cout<<msg;
#define DEBUG_STREAM(msg)

#ifdef PROFILE_EIQUADPROG
#define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
Guilhem Saurel's avatar
Guilhem Saurel committed
24
#define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
25
26
27
28
29
#else
#define START_PROFILER_EIQUADPROG_FAST(x)
#define STOP_PROFILER_EIQUADPROG_FAST(x)
#endif

30
#define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
Guilhem Saurel's avatar
Guilhem Saurel committed
31
32
33
34
35
36
37
38
39
40
41
42
#define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
43
44
45

#define DEFAULT_MAX_ITER 1000

Guilhem Saurel's avatar
Guilhem Saurel committed
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
namespace eiquadprog {

namespace solvers {

/**
 * Possible states of the solver.
 */
enum EiquadprogFast_status {
  EIQUADPROG_FAST_OPTIMAL = 0,
  EIQUADPROG_FAST_INFEASIBLE = 1,
  EIQUADPROG_FAST_UNBOUNDED = 2,
  EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
  EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
};

class EiquadprogFast {
  typedef Eigen::MatrixXd MatrixXd;
  typedef Eigen::VectorXd VectorXd;
  typedef Eigen::VectorXi VectorXi;

 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EiquadprogFast();
  virtual ~EiquadprogFast();

  void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);

  int getMaxIter() const { return m_maxIter; }

  bool setMaxIter(int maxIter) {
    if (maxIter < 0) return false;
    m_maxIter = maxIter;
    return true;
  }

  /**
   * @return The size of the active set, namely the number of
   * active constraints (including the equalities).
   */
  size_t getActiveSetSize() const { return q; }

  /**
   * @return The number of active-set iteratios.
   */
  int getIteratios() const { return iter; }

  /**
   * @return The value of the objective function.
   */
  double getObjValue() const { return f_value; }

  /**
   * @return The Lagrange multipliers
   */
  const VectorXd& getLagrangeMultipliers() const { return u; }

  /**
   * Return the active set, namely the indeces of active constraints.
   * The first nEqCon indexes are for the equalities and are negative.
   * The last nIneqCon indexes are for the inequalities and start from 0.
   * Only the first q elements of the return vector are valid, where q
   * is the size of the active set.
   * @return The set of indexes of the active constraints.
   */
  const VectorXi& getActiveSet() const { return A; }

  /**
   * solves the problem
   * min. x' Hess x + 2 g0' x
   * s.t. CE x + ce0 = 0
   *      CI x + ci0 >= 0
   */
  EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
                                       const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x);

  MatrixXd m_J;  // J * J' = Hessian <nVars,nVars>::d
  bool is_inverse_provided_;

 private:
  size_t m_nVars;
  size_t m_nEqCon;
  size_t m_nIneqCon;

  int m_maxIter;   /// max number of active-set iterations
  double f_value;  /// current value of cost function

  Eigen::LLT<MatrixXd, Eigen::Lower> chol_;  // <nVars,nVars>::d

  /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the matrix of active constraints
  MatrixXd R;  // <nVars,nVars>::d

  /// CI*x+ci0
  VectorXd s;  // <nIneqCon>::d

  /// infeasibility multipliers, i.e. negative step direction in dual space
  VectorXd r;  // <nIneqCon+nEqCon>::d

  /// Lagrange multipliers
  VectorXd u;  // <nIneqCon+nEqCon>::d

  /// step direction in primal space
  VectorXd z;  // <nVars>::d

  /// J' np
  VectorXd d;  //<nVars>::d

  /// current constraint normal
  VectorXd np;  //<nVars>::d

  /// active set (indeces of active constraints)
  /// the first nEqCon indeces are for the equalities and are negative
  /// the last nIneqCon indeces are for the inequalities are start from 0
  VectorXi A;  // <nIneqCon+nEqCon>

  /// initialized as K \ A
  /// iai(i)=-1 iff inequality constraint i is in the active set
  /// iai(i)=i otherwise
  VectorXi iai;  // <nIneqCon>::i

  /// initialized as [1, ..., 1, .]
  /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
  /// if adding ineq constraint i fails => iaexcl(i)=0
  /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active constraints
  /// iaexcl(i)=1 otherwise
  VectorXi iaexcl;  //<nIneqCon>::i

  VectorXd x_old;  // old value of x <nVars>::d
  VectorXd u_old;  // old value of u <nIneqCon+nEqCon>::d
  VectorXi A_old;  // old value of A <nIneqCon+nEqCon>::i
176

177
#ifdef OPTIMIZE_ADD_CONSTRAINT
Guilhem Saurel's avatar
Guilhem Saurel committed
178
  VectorXd T1;  /// tmp variable used in add_constraint
179
180
#endif

Guilhem Saurel's avatar
Guilhem Saurel committed
181
182
  /// size of the active set A (containing the indices of the active constraints)
  size_t q;
183

Guilhem Saurel's avatar
Guilhem Saurel committed
184
185
  /// number of active-set iterations
  int iter;
186

Guilhem Saurel's avatar
Guilhem Saurel committed
187
  inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
188
#ifdef OPTIMIZE_COMPUTE_D
Guilhem Saurel's avatar
Guilhem Saurel committed
189
    d.noalias() = J.adjoint() * np;
190
#else
Guilhem Saurel's avatar
Guilhem Saurel committed
191
    d = J.adjoint() * np;
192
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
193
  }
194

Guilhem Saurel's avatar
Guilhem Saurel committed
195
  inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, size_t iq) {
196
#ifdef OPTIMIZE_UPDATE_Z
Guilhem Saurel's avatar
Guilhem Saurel committed
197
    z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
198
#else
Guilhem Saurel's avatar
Guilhem Saurel committed
199
    z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
200
#endif
Guilhem Saurel's avatar
Guilhem Saurel committed
201
202
203
204
205
206
207
208
209
210
211
212
213
214
  }

  inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, size_t iq) {
    r.head(iq) = d.head(iq);
    R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
  }

  inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, double& R_norm);

  inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, size_t nEqCon, size_t& iq,
                                size_t l);
};

} /* namespace solvers */
215
} /* namespace eiquadprog */
216

217
218
219
220
/* --- Details -------------------------------------------------------------------- */
#include "eiquadprog/eiquadprog-fast.hxx"

#endif /* EIQUADPROGFAST_HPP_ */