GCC Code Coverage Report

Line	Branch	Exec	Source
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 __eiquadprog_rt_hpp__
20			#define __eiquadprog_rt_hpp__
21
22			#include <Eigen/Dense>
23
24			#include "eiquadprog/eiquadprog-utils.hxx"
25
26			#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
27			#define OPTIMIZE_COMPUTE_D // use noalias
28			#define OPTIMIZE_UPDATE_Z // use noalias
29			#define OPTIMIZE_HESSIAN_INVERSE // use solveInPlace
30			#define OPTIMIZE_UNCONSTR_MINIM
31
32			// #define USE_WARM_START
33			// #define PROFILE_EIQUADPROG
34			// #define DEBUG_STREAM(msg) std::cout<<msg;
35			#define DEBUG_STREAM(msg)
36
37			#ifdef PROFILE_EIQUADPROG
38			#define START_PROFILER_EIQUADPROG_RT(x) START_PROFILER(x)
39			#define STOP_PROFILER_EIQUADPROG_RT(x) STOP_PROFILER(x)
40			#else
41			#define START_PROFILER_EIQUADPROG_RT(x)
42			#define STOP_PROFILER_EIQUADPROG_RT(x)
43			#endif
44
45			#define PROFILE_EIQUADPROG_CHOWLESKY_DECOMPOSITION "EIQUADPROG_RT Chowlesky dec"
46			#define PROFILE_EIQUADPROG_CHOWLESKY_INVERSE "EIQUADPROG_RT Chowlesky inv"
47			#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR "EIQUADPROG_RT ADD_EQ_CONSTR"
48			#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_1 "EIQUADPROG_RT ADD_EQ_CONSTR_1"
49			#define PROFILE_EIQUADPROG_ADD_EQ_CONSTR_2 "EIQUADPROG_RT ADD_EQ_CONSTR_2"
50			#define PROFILE_EIQUADPROG_STEP_1 "EIQUADPROG_RT STEP_1"
51			#define PROFILE_EIQUADPROG_STEP_1_1 "EIQUADPROG_RT STEP_1_1"
52			#define PROFILE_EIQUADPROG_STEP_1_2 "EIQUADPROG_RT STEP_1_2"
53			#define PROFILE_EIQUADPROG_STEP_1_UNCONSTR_MINIM \
54			"EIQUADPROG_RT STEP_1_UNCONSTR_MINIM"
55			#define PROFILE_EIQUADPROG_STEP_2 "EIQUADPROG_RT STEP_2"
56			#define PROFILE_EIQUADPROG_STEP_2A "EIQUADPROG_RT STEP_2A"
57			#define PROFILE_EIQUADPROG_STEP_2B "EIQUADPROG_RT STEP_2B"
58			#define PROFILE_EIQUADPROG_STEP_2C "EIQUADPROG_RT STEP_2C"
59
60			#define DEFAULT_MAX_ITER 1000
61
62			template <int Rows, int Cols>
63			struct RtMatrixX {
64			typedef Eigen::Matrix<double, Rows, Cols> d;
65			};
66
67			template <int Rows>
68			struct RtVectorX {
69			typedef Eigen::Matrix<double, Rows, 1> d;
70			typedef Eigen::Matrix<int, Rows, 1> i;
71			};
72
73			namespace eiquadprog {
74
75			namespace solvers {
76
77			/**
78			* Possible states of the solver.
79			*/
80			enum RtEiquadprog_status {
81			RT_EIQUADPROG_OPTIMAL = 0,
82			RT_EIQUADPROG_INFEASIBLE = 1,
83			RT_EIQUADPROG_UNBOUNDED = 2,
84			RT_EIQUADPROG_MAX_ITER_REACHED = 3,
85			RT_EIQUADPROG_REDUNDANT_EQUALITIES = 4
86			};
87
88			template <int nVars, int nEqCon, int nIneqCon>
89			class RtEiquadprog {
90			public:
91			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
92
93			RtEiquadprog();
94			virtual ~RtEiquadprog();
95
96			int getMaxIter() const { return m_maxIter; }
97
98			bool setMaxIter(int maxIter) {
99			if (maxIter < 0) return false;
100			m_maxIter = maxIter;
101			return true;
102			}
103
104			/**
105			* @return The size of the active set, namely the number of
106			* active constraints (including the equalities).
107			*/
108			int getActiveSetSize() const { return q; }
109
110			/**
111			* @return The number of active-set iteratios.
112			*/
113			int getIteratios() const { return iter; }
114
115			/**
116			* @return The value of the objective function.
117			*/
118		16	double getObjValue() const { return f_value; }
119
120			/**
121			* @return The Lagrange multipliers
122			*/
123			const typename RtVectorX<nIneqCon + nEqCon>::d& getLagrangeMultipliers()
124			const {
125			return u;
126			}
127
128			/**
129			* Return the active set, namely the indeces of active constraints.
130			* The first nEqCon indexes are for the equalities and are negative.
131			* The last nIneqCon indexes are for the inequalities and start from 0.
132			* Only the first q elements of the return vector are valid, where q
133			* is the size of the active set.
134			* @return The set of indexes of the active constraints.
135			*/
136			const typename RtVectorX<nIneqCon + nEqCon>::i& getActiveSet() const {
137			return A;
138			}
139
140			/**
141			* solves the problem
142			* min. x' Hess x + 2 g0' x
143			* s.t. CE x + ce0 = 0
144			* CI x + ci0 >= 0
145			*/
146			RtEiquadprog_status solve_quadprog(
147			const typename RtMatrixX<nVars, nVars>::d& Hess,
148			const typename RtVectorX<nVars>::d& g0,
149			const typename RtMatrixX<nEqCon, nVars>::d& CE,
150			const typename RtVectorX<nEqCon>::d& ce0,
151			const typename RtMatrixX<nIneqCon, nVars>::d& CI,
152			const typename RtVectorX<nIneqCon>::d& ci0,
153			typename RtVectorX<nVars>::d& x);
154
155			typename RtMatrixX<nVars, nVars>::d m_J; // J * J' = Hessian
156			bool is_inverse_provided_;
157
158			private:
159			int m_maxIter; /// max number of active-set iterations
160			double f_value; /// current value of cost function
161
162			Eigen::LLT<typename RtMatrixX<nVars, nVars>::d, Eigen::Lower> chol_;
163			double solver_return_;
164
165			/// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the
166			/// matrix of active constraints
167			typename RtMatrixX<nVars, nVars>::d R;
168
169			/// CI*x+ci0
170			typename RtVectorX<nIneqCon>::d s;
171
172			/// infeasibility multipliers, i.e. negative step direction in dual space
173			typename RtVectorX<nIneqCon + nEqCon>::d r;
174
175			/// Lagrange multipliers
176			typename RtVectorX<nIneqCon + nEqCon>::d u;
177
178			/// step direction in primal space
179			typename RtVectorX<nVars>::d z;
180
181			/// J' np
182			typename RtVectorX<nVars>::d d;
183
184			/// current constraint normal
185			typename RtVectorX<nVars>::d np;
186
187			/// active set (indeces of active constraints)
188			/// the first nEqCon indeces are for the equalities and are negative
189			/// the last nIneqCon indeces are for the inequalities are start from 0
190			typename RtVectorX<nIneqCon + nEqCon>::i A;
191
192			/// initialized as K \ A
193			/// iai(i)=-1 iff inequality constraint i is in the active set
194			/// iai(i)=i otherwise
195			typename RtVectorX<nIneqCon>::i iai;
196
197			/// initialized as [1, ..., 1, .]
198			/// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set
199			/// if adding ineq constraint i fails => iaexcl(i)=0
200			/// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active
201			/// constraints iaexcl(i)=1 otherwise
202			typename RtVectorX<nIneqCon>::i iaexcl;
203
204			typename RtVectorX<nVars>::d x_old; // old value of x
205			typename RtVectorX<nIneqCon + nEqCon>::d u_old; // old value of u
206			typename RtVectorX<nIneqCon + nEqCon>::i A_old; // old value of A
207
208			#ifdef OPTIMIZE_ADD_CONSTRAINT
209			typename RtVectorX<nVars>::d T1; // tmp vector
210			#endif
211
212			/// size of the active set A (containing the indices of the active
213			/// constraints)
214			int q;
215
216			/// number of active-set iterations
217			int iter;
218
219			template <typename Scalar>
220			inline Scalar distance(Scalar a, Scalar b) {
221			Scalar a1, b1, t;
222			a1 = std::abs(a);
223			b1 = std::abs(b);
224			if (a1 > b1) {
225			t = (b1 / a1);
226			return a1 * std::sqrt(1.0 + t * t);
227			} else if (b1 > a1) {
228			t = (a1 / b1);
229			return b1 * std::sqrt(1.0 + t * t);
230			}
231			return a1 * std::sqrt(2.0);
232			}
233
234		26	inline void compute_d(typename RtVectorX<nVars>::d& d,
235			const typename RtMatrixX<nVars, nVars>::d& J,
236			const typename RtVectorX<nVars>::d& np) {
237			#ifdef OPTIMIZE_COMPUTE_D
238	3/6 ✓ Branch 2 taken 13 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 13 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 13 times. ✗ Branch 9 not taken.	26	d.noalias() = J.adjoint() * np;
239			#else
240			d = J.adjoint() * np;
241			#endif
242			}
243
244		24	inline void update_z(typename RtVectorX<nVars>::d& z,
245			const typename RtMatrixX<nVars, nVars>::d& J,
246			const typename RtVectorX<nVars>::d& d, int iq) {
247			#ifdef OPTIMIZE_UPDATE_Z
248	4/8 ✓ Branch 2 taken 12 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 12 times. ✗ Branch 6 not taken. ✓ Branch 8 taken 12 times. ✗ Branch 9 not taken. ✓ Branch 11 taken 12 times. ✗ Branch 12 not taken.	24	z.noalias() = J.rightCols(nVars - iq) * d.tail(nVars - iq);
249			#else
250			z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
251			#endif
252			}
253
254		26	inline void update_r(const typename RtMatrixX<nVars, nVars>::d& R,
255			typename RtVectorX<nIneqCon + nEqCon>::d& r,
256			const typename RtVectorX<nVars>::d& d, int iq) {
257	2/4 ✓ Branch 2 taken 13 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 13 times. ✗ Branch 6 not taken.	26	r.head(iq) = d.head(iq);
258		26	R.topLeftCorner(iq, iq)
259	1/2 ✓ Branch 1 taken 13 times. ✗ Branch 2 not taken.	26	.template triangularView<Eigen::Upper>()
260	2/4 ✓ Branch 1 taken 13 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 13 times. ✗ Branch 5 not taken.	26	.solveInPlace(r.head(iq));
261			}
262
263			bool add_constraint(typename RtMatrixX<nVars, nVars>::d& R,
264			typename RtMatrixX<nVars, nVars>::d& J,
265			typename RtVectorX<nVars>::d& d, int& iq, double& R_norm);
266
267			void delete_constraint(typename RtMatrixX<nVars, nVars>::d& R,
268			typename RtMatrixX<nVars, nVars>::d& J,
269			typename RtVectorX<nIneqCon + nEqCon>::i& A,
270			typename RtVectorX<nIneqCon + nEqCon>::d& u, int& iq,
271			int l);
272			};
273
274			} /* namespace solvers */
275			} /* namespace eiquadprog */
276
277			#include "eiquadprog/eiquadprog-rt.hxx"
278			/* --- Details
279			* -------------------------------------------------------------------- */
280
281			#endif /* __eiquadprog_rt_hpp__ */
282