GCC Code Coverage Report

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		9692	inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
225			#ifdef OPTIMIZE_COMPUTE_D
226	3/6 ✓ Branch 2 taken 9692 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 9692 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 9692 times. ✗ Branch 9 not taken.	9692	d.noalias() = J.adjoint() * np;
227			#else
228			d = J.adjoint() * np;
229			#endif
230		9692	}
231
232		9691	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 9691 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 9691 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 9691 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 9691 times. ✗ Branch 14 not taken.	9691	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		9691	}
240
241		9692	inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d,
242			int iq) {
243	2/4 ✓ Branch 2 taken 9692 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 9692 times. ✗ Branch 6 not taken.	9692	r.head(iq) = d.head(iq);
244	2/4 ✓ Branch 2 taken 9692 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 9692 times. ✗ Branch 6 not taken.	19384	R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
245	1/2 ✓ Branch 1 taken 9692 times. ✗ Branch 2 not taken.	9692	r.head(iq));
246		9692	}
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