Directory: | ./ |
---|---|
File: | include/hpp/bezier-com-traj/solver/eiquadprog-fast.hpp |
Date: | 2025-03-18 04:20:50 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 12 | 12 | 100.0% |
Branches: | 12 | 24 | 50.0% |
Line | Branch | Exec | Source |
---|---|---|---|
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 | |||
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 | public: | ||
79 | typedef Eigen::MatrixXd MatrixXd; | ||
80 | typedef Eigen::VectorXd VectorXd; | ||
81 | typedef Eigen::VectorXi VectorXi; | ||
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 | |||
90 | EiquadprogFast(); | ||
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 | |||
103 | /** | ||
104 | * @return The size of the active set, namely the number of | ||
105 | * active constraints (including the equalities). | ||
106 | */ | ||
107 | int getActiveSetSize() const { return q; } | ||
108 | |||
109 | /** | ||
110 | * @return The number of active-set iteratios. | ||
111 | */ | ||
112 | int getIteratios() const { return iter; } | ||
113 | |||
114 | /** | ||
115 | * @return The value of the objective function. | ||
116 | */ | ||
117 | 557 | double getObjValue() const { return f_value; } | |
118 | |||
119 | /** | ||
120 | * @return The Lagrange multipliers | ||
121 | */ | ||
122 | const VectorXd& getLagrangeMultipliers() const { return u; } | ||
123 | /** | ||
124 | * Return the active set, namely the indeces of active constraints. | ||
125 | * The first nEqCon indexes are for the equalities and are negative. | ||
126 | * The last nIneqCon indexes are for the inequalities and start from 0. | ||
127 | * Only the first q elements of the return vector are valid, where q | ||
128 | * is the size of the active set. | ||
129 | * @return The set of indexes of the active constraints. | ||
130 | */ | ||
131 | const VectorXi& getActiveSet() const { return A; } | ||
132 | |||
133 | /** | ||
134 | * solves the problem | ||
135 | * min. x' Hess x + 2 g0' x | ||
136 | * s.t. CE x + ce0 = 0 | ||
137 | * CI x + ci0 >= 0 | ||
138 | */ | ||
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); | ||
143 | /** | ||
144 | * solves the sparse problem | ||
145 | * min. x' Hess x + 2 g0' x | ||
146 | * s.t. CE x + ce0 = 0 | ||
147 | * CI x + ci0 >= 0 | ||
148 | */ | ||
149 | EiquadprogFast_status solve_quadprog_sparse(const SpMat& Hess, | ||
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 | ||
157 | bool is_inverse_provided_; | ||
158 | |||
159 | private: | ||
160 | int m_nVars; | ||
161 | int m_nEqCon; | ||
162 | int m_nIneqCon; | ||
163 | |||
164 | int m_maxIter; /// max number of active-set iterations | ||
165 | double f_value; /// current value of cost function | ||
166 | |||
167 | Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d | ||
168 | // Eigen::LLT<MatrixXd,Eigen::Lower> chol_; // <nVars,nVars>::d | ||
169 | |||
170 | /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the | ||
171 | /// matrix of active constraints | ||
172 | MatrixXd R; // <nVars,nVars>::d | ||
173 | |||
174 | /// CI*x+ci0 | ||
175 | VectorXd s; // <nIneqCon>::d | ||
176 | |||
177 | /// infeasibility multipliers, i.e. negative step direction in dual space | ||
178 | VectorXd r; // <nIneqCon+nEqCon>::d | ||
179 | |||
180 | /// Lagrange multipliers | ||
181 | VectorXd u; // <nIneqCon+nEqCon>::d | ||
182 | |||
183 | /// step direction in primal space | ||
184 | VectorXd z; // <nVars>::d | ||
185 | |||
186 | /// J' np | ||
187 | VectorXd d; //<nVars>::d | ||
188 | |||
189 | /// current constraint normal | ||
190 | VectorXd np; //<nVars>::d | ||
191 | |||
192 | /// active set (indeces of active constraints) | ||
193 | /// the first nEqCon indeces are for the equalities and are negative | ||
194 | /// the last nIneqCon indeces are for the inequalities are start from 0 | ||
195 | VectorXi A; // <nIneqCon+nEqCon> | ||
196 | |||
197 | /// initialized as K \ A | ||
198 | /// iai(i)=-1 iff inequality constraint i is in the active set | ||
199 | /// iai(i)=i otherwise | ||
200 | VectorXi iai; // <nIneqCon>::i | ||
201 | |||
202 | /// initialized as [1, ..., 1, .] | ||
203 | /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set | ||
204 | /// if adding ineq constraint i fails => iaexcl(i)=0 | ||
205 | /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active | ||
206 | /// constraints iaexcl(i)=1 otherwise | ||
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; /// tmp variable used in add_constraint | ||
215 | #endif | ||
216 | |||
217 | /// size of the active set A (containing the indices of the active | ||
218 | /// constraints) | ||
219 | int q; | ||
220 | |||
221 | /// number of active-set iterations | ||
222 | int iter; | ||
223 | |||
224 | 9880 | inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) { | |
225 | #ifdef OPTIMIZE_COMPUTE_D | ||
226 |
3/6✓ Branch 2 taken 9880 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9880 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9880 times.
✗ Branch 9 not taken.
|
9880 | d.noalias() = J.adjoint() * np; |
227 | #else | ||
228 | d = J.adjoint() * np; | ||
229 | #endif | ||
230 | 9880 | } | |
231 | |||
232 | 9879 | inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, | |
233 | int iq) { | ||
234 | #ifdef OPTIMIZE_UPDATE_Z | ||
235 |
4/8✓ Branch 4 taken 9879 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9879 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9879 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9879 times.
✗ Branch 14 not taken.
|
9879 | 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 | 9879 | } | |
240 | |||
241 | 9880 | inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, | |
242 | int iq) { | ||
243 |
2/4✓ Branch 2 taken 9880 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9880 times.
✗ Branch 6 not taken.
|
9880 | r.head(iq) = d.head(iq); |
244 |
2/4✓ Branch 2 taken 9880 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9880 times.
✗ Branch 6 not taken.
|
19760 | R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace( |
245 |
1/2✓ Branch 1 taken 9880 times.
✗ Branch 2 not taken.
|
9880 | r.head(iq)); |
246 | 9880 | } | |
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_ */ | ||
259 |