hpp-bezier-com-traj  4.14.0
Multi contact trajectory generation for the COM using Bezier curves
eiquadprog-fast.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2017 CNRS
3 //
4 // This file is part of tsid
5 // tsid is free software: you can redistribute it
6 // and/or modify it under the terms of the GNU Lesser General Public
7 // License as published by the Free Software Foundation, either version
8 // 3 of the License, or (at your option) any later version.
9 // tsid is distributed in the hope that it will be
10 // useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11 // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Lesser Public License for more details. You should have
13 // received a copy of the GNU Lesser General Public License along with
14 // tsid If not, see
15 // <http://www.gnu.org/licenses/>.
16 //
17 
18 #ifndef EIQUADPROGFAST_HH_
19 #define EIQUADPROGFAST_HH_
20 
21 #include <Eigen/Dense>
22 #include <Eigen/Sparse>
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 //#define TRACE_SOLVER 1
30 
31 //#define USE_WARM_START
32 //#define PROFILE_EIQUADPROG
33 
34 //#define DEBUG_STREAM(msg) std::cout<<msg;
35 #define DEBUG_STREAM(msg)
36 
37 #ifdef PROFILE_EIQUADPROG
38 #define START_PROFILER_EIQUADPROG_FAST START_PROFILER
39 #define STOP_PROFILER_EIQUADPROG_FAST STOP_PROFILER
40 #else
41 #define START_PROFILER_EIQUADPROG_FAST
42 #define STOP_PROFILER_EIQUADPROG_FAST
43 #endif
44 
45 #define EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION "EIQUADPROG_FAST Chowlesky dec"
46 #define EIQUADPROG_FAST_CHOWLESKY_INVERSE "EIQUADPROG_FAST Chowlesky inv"
47 #define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
48 #define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
49 #define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
50 #define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
51 #define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
52 #define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
53 #define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \
54  "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
55 #define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
56 #define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
57 #define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
58 #define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
59 
60 #define DEFAULT_MAX_ITER 1000
61 
62 namespace tsid {
63 
64 namespace solvers {
65 
75 };
76 
78  public:
82 
83  typedef Eigen::SparseMatrix<double> SpMat;
84  typedef Eigen::SparseVector<double> SpVec;
85  typedef Eigen::SparseVector<int> SpVeci;
86 
87  public:
88  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
89 
91  virtual ~EiquadprogFast();
92 
93  void reset(int dim_qp, int num_eq, int num_ineq);
94 
95  int getMaxIter() const { return m_maxIter; }
96 
97  bool setMaxIter(int maxIter) {
98  if (maxIter < 0) return false;
99  m_maxIter = maxIter;
100  return true;
101  }
102 
107  int getActiveSetSize() const { return q; }
108 
112  int getIteratios() const { return iter; }
113 
117  double getObjValue() const { return f_value; }
118 
122  const VectorXd& getLagrangeMultipliers() const { return u; }
131  const VectorXi& getActiveSet() const { return A; }
132 
139  EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0,
140  const MatrixXd& CE, const VectorXd& ce0,
141  const MatrixXd& CI, const VectorXd& ci0,
142  VectorXd& x);
150  const VectorXd& g0,
151  const MatrixXd& CE,
152  const VectorXd& ce0,
153  const MatrixXd& CI,
154  const VectorXd& ci0, VectorXd& x);
155 
156  MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
158 
159  private:
160  int m_nVars;
161  int m_nEqCon;
162  int m_nIneqCon;
163 
164  int m_maxIter;
165  double f_value;
166 
167  Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d
168  // Eigen::LLT<MatrixXd,Eigen::Lower> chol_; // <nVars,nVars>::d
169 
172  MatrixXd R; // <nVars,nVars>::d
173 
175  VectorXd s; // <nIneqCon>::d
176 
178  VectorXd r; // <nIneqCon+nEqCon>::d
179 
181  VectorXd u; // <nIneqCon+nEqCon>::d
182 
184  VectorXd z; // <nVars>::d
185 
187  VectorXd d; //<nVars>::d
188 
190  VectorXd np; //<nVars>::d
191 
195  VectorXi A; // <nIneqCon+nEqCon>
196 
200  VectorXi iai; // <nIneqCon>::i
201 
207  VectorXi iaexcl; //<nIneqCon>::i
208 
209  VectorXd x_old; // old value of x <nVars>::d
210  VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d
211  VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i
212 
213 #ifdef OPTIMIZE_ADD_CONSTRAINT
214  VectorXd T1;
215 #endif
216 
219  int q;
220 
222  int iter;
223 
224  inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
225 #ifdef OPTIMIZE_COMPUTE_D
226  d.noalias() = J.adjoint() * np;
227 #else
228  d = J.adjoint() * np;
229 #endif
230  }
231 
232  inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,
233  int iq) {
234 #ifdef OPTIMIZE_UPDATE_Z
235  z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
236 #else
237  z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
238 #endif
239  }
240 
241  inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d,
242  int iq) {
243  r.head(iq) = d.head(iq);
244  R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
245  r.head(iq));
246  }
247 
248  inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq,
249  double& R_norm);
250 
251  inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
252  VectorXd& u, int nEqCon, int& iq, int l);
253 };
254 
255 } /* namespace solvers */
256 } /* namespace tsid */
257 
258 #endif /* EIQUADPROGFAST_HH_ */
Definition: eiquadprog-fast.hpp:77
Eigen::SparseVector< int > SpVeci
Definition: eiquadprog-fast.hpp:85
Eigen::VectorXi VectorXi
Definition: eiquadprog-fast.hpp:81
Eigen::MatrixXd MatrixXd
Definition: eiquadprog-fast.hpp:79
bool setMaxIter(int maxIter)
Definition: eiquadprog-fast.hpp:97
double getObjValue() const
Definition: eiquadprog-fast.hpp:117
const VectorXd & getLagrangeMultipliers() const
Definition: eiquadprog-fast.hpp:122
int getActiveSetSize() const
Definition: eiquadprog-fast.hpp:107
bool is_inverse_provided_
Definition: eiquadprog-fast.hpp:157
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:230
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EiquadprogFast()
Definition: eiquadprog-fast.cpp:40
Eigen::SparseMatrix< double > SpMat
Definition: eiquadprog-fast.hpp:83
EiquadprogFast_status solve_quadprog_sparse(const SpMat &Hess, const VectorXd &g0, const MatrixXd &CE, const VectorXd &ce0, const MatrixXd &CI, const VectorXd &ci0, VectorXd &x)
Definition: eiquadprog-fast.cpp:655
const VectorXi & getActiveSet() const
Definition: eiquadprog-fast.hpp:131
Eigen::VectorXd VectorXd
Definition: eiquadprog-fast.hpp:80
void reset(int dim_qp, int num_eq, int num_ineq)
Definition: eiquadprog-fast.cpp:52
Eigen::SparseVector< double > SpVec
Definition: eiquadprog-fast.hpp:84
int getIteratios() const
Definition: eiquadprog-fast.hpp:112
int getMaxIter() const
Definition: eiquadprog-fast.hpp:95
virtual ~EiquadprogFast()
Definition: eiquadprog-fast.cpp:50
MatrixXd m_J
Definition: eiquadprog-fast.hpp:156
Definition: glpk-wrapper.hpp:24
Eigen::MatrixXd MatrixXd
Definition: solver-abstract.hpp:34
Eigen::VectorXi VectorXi
Definition: solver-abstract.hpp:36
Eigen::VectorXd VectorXd
Definition: solver-abstract.hpp:35
EiquadprogFast_status
Definition: eiquadprog-fast.hpp:69
@ EIQUADPROG_FAST_UNBOUNDED
Definition: eiquadprog-fast.hpp:72
@ EIQUADPROG_FAST_OPTIMAL
Definition: eiquadprog-fast.hpp:70
@ EIQUADPROG_FAST_REDUNDANT_EQUALITIES
Definition: eiquadprog-fast.hpp:74
@ EIQUADPROG_FAST_MAX_ITER_REACHED
Definition: eiquadprog-fast.hpp:73
@ EIQUADPROG_FAST_INFEASIBLE
Definition: eiquadprog-fast.hpp:71
Definition: eiquadprog-fast.hpp:62