Loading...
Searching...
No Matches
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
63namespace eiquadprog {
64
65namespace solvers {
66
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
MatrixXd m_J
Definition eiquadprog-fast.hpp:141
bool setMaxIter(int maxIter)
Definition eiquadprog-fast.hpp:93
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
const VectorXi & getActiveSet() const
Definition eiquadprog-fast.hpp:128
bool is_inverse_provided_
Definition eiquadprog-fast.hpp:142
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EiquadprogFast()
Definition eiquadprog-fast.cpp:8
const VectorXd & getLagrangeMultipliers() const
Definition eiquadprog-fast.hpp:118
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