GCC Code Coverage Report


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