eiquadprog-fast.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 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 \
53  "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
54 #define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
55 #define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
56 #define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
57 #define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
58 
59 #define DEFAULT_MAX_ITER 1000
60 
62 
63 namespace eiquadprog {
64 
65 namespace solvers {
66 
76 };
77 
79  typedef Eigen::MatrixXd MatrixXd;
80  typedef Eigen::VectorXd VectorXd;
81  typedef Eigen::VectorXi VectorXi;
82 
83  public:
84  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
85 
87  virtual ~EiquadprogFast();
88 
89  void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);
90 
91  int getMaxIter() const { return m_maxIter; }
92 
93  bool setMaxIter(int maxIter) {
94  if (maxIter < 0) return false;
95  m_maxIter = maxIter;
96  return true;
97  }
98 
103  size_t getActiveSetSize() const { return q; }
104 
108  int getIteratios() const { return iter; }
109 
113  double getObjValue() const { return f_value; }
114 
118  const VectorXd& getLagrangeMultipliers() const { return u; }
119 
128  const VectorXi& getActiveSet() const { return A; }
129 
136  EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0,
137  const MatrixXd& CE, const VectorXd& ce0,
138  const MatrixXd& CI, const VectorXd& ci0,
139  VectorXd& x);
140 
141  MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
143 
144  private:
145  size_t m_nVars;
146  size_t m_nEqCon;
147  size_t m_nIneqCon;
148 
149  int m_maxIter;
150  double f_value;
151 
152  Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d
153 
156  MatrixXd R; // <nVars,nVars>::d
157 
159  VectorXd s; // <nIneqCon>::d
160 
162  VectorXd r; // <nIneqCon+nEqCon>::d
163 
165  VectorXd u; // <nIneqCon+nEqCon>::d
166 
168  VectorXd z; // <nVars>::d
169 
171  VectorXd d; //<nVars>::d
172 
174  VectorXd np; //<nVars>::d
175 
179  VectorXi A; // <nIneqCon+nEqCon>
180 
184  VectorXi iai; // <nIneqCon>::i
185 
191  VectorXi iaexcl; //<nIneqCon>::i
192 
193  VectorXd x_old; // old value of x <nVars>::d
194  VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d
195  VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i
196 
197 #ifdef OPTIMIZE_ADD_CONSTRAINT
198  VectorXd T1;
199 #endif
200 
203  size_t q;
204 
206  int iter;
207 
208  inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
209 #ifdef OPTIMIZE_COMPUTE_D
210  d.noalias() = J.adjoint() * np;
211 #else
212  d = J.adjoint() * np;
213 #endif
214  }
215 
216  inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,
217  size_t iq) {
218 #ifdef OPTIMIZE_UPDATE_Z
219  z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
220 #else
221  z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
222 #endif
223  }
224 
225  inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d,
226  size_t iq) {
227  r.head(iq) = d.head(iq);
228  R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
229  r.head(iq));
230  }
231 
232  inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq,
233  double& R_norm);
234 
235  inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
236  VectorXd& u, size_t nEqCon, size_t& iq,
237  size_t l);
238 };
239 
240 } /* namespace solvers */
241 } /* namespace eiquadprog */
242 
243 #endif /* EIQUADPROGFAST_HPP_ */
Definition: eiquadprog-fast.hpp:78
double getObjValue() const
Definition: eiquadprog-fast.hpp:113
int getIteratios() const
Definition: eiquadprog-fast.hpp:108
size_t getActiveSetSize() const
Definition: eiquadprog-fast.hpp:103
int getMaxIter() const
Definition: eiquadprog-fast.hpp:91
virtual ~EiquadprogFast()
Definition: eiquadprog-fast.cpp:18
void reset(size_t dim_qp, size_t num_eq, size_t num_ineq)
Definition: eiquadprog-fast.cpp:20
const VectorXi & getActiveSet() const
Definition: eiquadprog-fast.hpp:128
MatrixXd m_J
Definition: eiquadprog-fast.hpp:141
bool setMaxIter(int maxIter)
Definition: eiquadprog-fast.hpp:93
const VectorXd & getLagrangeMultipliers() const
Definition: eiquadprog-fast.hpp:118
EiquadprogFast_status solve_quadprog(const MatrixXd &Hess, const VectorXd &g0, const MatrixXd &CE, const VectorXd &ce0, const MatrixXd &CI, const VectorXd &ci0, VectorXd &x)
Definition: eiquadprog-fast.cpp:189
bool is_inverse_provided_
Definition: eiquadprog-fast.hpp:142
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EiquadprogFast()
Definition: eiquadprog-fast.cpp:8
EiquadprogFast_status
Definition: eiquadprog-fast.hpp:70
@ EIQUADPROG_FAST_MAX_ITER_REACHED
Definition: eiquadprog-fast.hpp:74
@ EIQUADPROG_FAST_REDUNDANT_EQUALITIES
Definition: eiquadprog-fast.hpp:75
@ EIQUADPROG_FAST_OPTIMAL
Definition: eiquadprog-fast.hpp:71
@ EIQUADPROG_FAST_INFEASIBLE
Definition: eiquadprog-fast.hpp:72
@ EIQUADPROG_FAST_UNBOUNDED
Definition: eiquadprog-fast.hpp:73
Definition: eiquadprog-fast.hpp:63