| 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 |
#include <iostream> |
||
19 |
|||
20 |
#include <boost/test/unit_test.hpp> |
||
21 |
#include <boost/utility/binary.hpp> |
||
22 |
|||
23 |
#include <tsid/solvers/solver-HQP-factory.hxx> |
||
24 |
#include <tsid/solvers/solver-HQP-eiquadprog.hpp> |
||
25 |
#include <tsid/solvers/solver-HQP-eiquadprog-rt.hpp> |
||
26 |
|||
27 |
#ifdef TSID_WITH_PROXSUITE |
||
28 |
#include <tsid/solvers/solver-proxqp.hpp> |
||
29 |
#endif |
||
30 |
#ifdef TSID_WITH_OSQP |
||
31 |
#include <tsid/solvers/solver-osqp.hpp> |
||
32 |
#endif |
||
33 |
#ifdef TSID_QPMAD_FOUND |
||
34 |
#include <tsid/solvers/solver-HQP-qpmad.hpp> |
||
35 |
#endif |
||
36 |
|||
37 |
#include <tsid/math/utils.hpp> |
||
38 |
#include <tsid/math/constraint-equality.hpp> |
||
39 |
#include <tsid/math/constraint-inequality.hpp> |
||
40 |
#include <tsid/math/constraint-bound.hpp> |
||
41 |
#include <tsid/utils/stop-watch.hpp> |
||
42 |
#include <tsid/utils/statistics.hpp> |
||
43 |
|||
44 |
#define CHECK_LESS_THAN(A, B) \ |
||
45 |
BOOST_CHECK_MESSAGE(A < B, #A << ": " << A << ">" << B) |
||
46 |
#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A << ": " << A) |
||
47 |
|||
48 |
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) |
||
49 |
|||
50 |
// BOOST_AUTO_TEST_CASE ( test_eiquadprog_unconstrained) |
||
51 |
//{ |
||
52 |
// std::cout << "test_eiquadprog_unconstrained\n"; |
||
53 |
// using namespace tsid; |
||
54 |
// using namespace math; |
||
55 |
// using namespace solvers; |
||
56 |
// using namespace std; |
||
57 |
|||
58 |
// const unsigned int n = 5; |
||
59 |
// const unsigned int m = 3; |
||
60 |
// const unsigned int neq = 0; |
||
61 |
// const unsigned int nin = 0; |
||
62 |
// const double damping = 1e-4; |
||
63 |
// SolverHQPBase * solver = SolverHQPBase::getNewSolver(SOLVER_HQP_EIQUADPROG, |
||
64 |
// "solver-eiquadprog"); solver->resize(n, neq, nin); |
||
65 |
|||
66 |
// HQPData HQPData(2); |
||
67 |
// Matrix A = Matrix::Random(m, n); |
||
68 |
// Vector b = Vector::Random(m); |
||
69 |
// ConstraintEquality constraint1("c1", A, b); |
||
70 |
// HQPData[1].push_back(make_pair<double, ConstraintBase*>(1.0, &constraint1)); |
||
71 |
|||
72 |
// ConstraintEquality constraint2("c2", Matrix::Identity(n,n), |
||
73 |
// Vector::Zero(n)); HQPData[1].push_back(make_pair<double, |
||
74 |
// ConstraintBase*>(damping, &constraint2)); |
||
75 |
|||
76 |
// const HQPOutput & output = solver->solve(HQPData); |
||
77 |
// BOOST_CHECK_MESSAGE(output.status==HQP_STATUS_OPTIMAL, "Status |
||
78 |
// "+toString(output.status)); |
||
79 |
|||
80 |
// Vector x(n); |
||
81 |
// svdSolveWithDamping(A, b, x, damping); |
||
82 |
// BOOST_CHECK_MESSAGE(x.isApprox(output.x, 1e-3), "Solution error: |
||
83 |
// "+toString(x-output.x)); |
||
84 |
//} |
||
85 |
|||
86 |
// BOOST_AUTO_TEST_CASE ( test_eiquadprog_equality_constrained) |
||
87 |
//{ |
||
88 |
// std::cout << "test_eiquadprog_equality_constrained\n"; |
||
89 |
// using namespace tsid; |
||
90 |
// using namespace math; |
||
91 |
// using namespace solvers; |
||
92 |
// using namespace std; |
||
93 |
|||
94 |
// const unsigned int n = 5; |
||
95 |
// const unsigned int m = 3; |
||
96 |
// const unsigned int neq = 2; |
||
97 |
// const unsigned int nin = 0; |
||
98 |
// const double damping = 1e-4; |
||
99 |
// SolverHQPBase * solver = SolverHQPBase::getNewSolver(SOLVER_HQP_EIQUADPROG, |
||
100 |
// "solver-eiquadprog"); |
||
101 |
// solver->resize(n, neq, nin); |
||
102 |
|||
103 |
// HQPData HQPData(2); |
||
104 |
|||
105 |
// Matrix A_eq = Matrix::Random(neq, n); |
||
106 |
// Vector b_eq = Vector::Random(neq); |
||
107 |
// ConstraintEquality eq_constraint("eq1", A_eq, b_eq); |
||
108 |
// HQPData[0].push_back(make_pair<double, ConstraintBase*>(1.0, |
||
109 |
// &eq_constraint)); |
||
110 |
|||
111 |
// Matrix A = Matrix::Random(m, n); |
||
112 |
// Vector b = Vector::Random(m); |
||
113 |
// ConstraintEquality constraint1("c1", A, b); |
||
114 |
// HQPData[1].push_back(make_pair<double, ConstraintBase*>(1.0, &constraint1)); |
||
115 |
|||
116 |
// ConstraintEquality constraint2("c2", Matrix::Identity(n,n), |
||
117 |
// Vector::Zero(n)); HQPData[1].push_back(make_pair<double, |
||
118 |
// ConstraintBase*>(damping, &constraint2)); |
||
119 |
|||
120 |
// const HQPOutput & output = solver->solve(HQPData); |
||
121 |
// BOOST_REQUIRE_MESSAGE(output.status==HQP_STATUS_OPTIMAL, |
||
122 |
// "Status "+toString(output.status)); |
||
123 |
// BOOST_CHECK_MESSAGE(b_eq.isApprox(A_eq*output.x), |
||
124 |
// "Constraint error: "+toString(b_eq-A_eq*output.x)); |
||
125 |
//} |
||
126 |
|||
127 |
// BOOST_AUTO_TEST_CASE ( test_eiquadprog_inequality_constrained) |
||
128 |
//{ |
||
129 |
// std::cout << "test_eiquadprog_inequality_constrained\n"; |
||
130 |
// using namespace tsid; |
||
131 |
// using namespace math; |
||
132 |
// using namespace solvers; |
||
133 |
// using namespace std; |
||
134 |
|||
135 |
// const unsigned int n = 5; |
||
136 |
// const unsigned int m = 3; |
||
137 |
// const unsigned int neq = 0; |
||
138 |
// const unsigned int nin = 3; |
||
139 |
// const double damping = 1e-5; |
||
140 |
// SolverHQPBase * solver = SolverHQPBase::getNewSolver(SOLVER_HQP_EIQUADPROG, |
||
141 |
// "solver-eiquadprog"); |
||
142 |
// solver->resize(n, neq, nin); |
||
143 |
|||
144 |
// HQPData HQPData(2); |
||
145 |
|||
146 |
// Matrix A = Matrix::Random(m, n); |
||
147 |
// Vector b = Vector::Random(m); |
||
148 |
// ConstraintEquality constraint1("c1", A, b); |
||
149 |
// HQPData[1].push_back(make_pair<double, ConstraintBase*>(1.0, &constraint1)); |
||
150 |
|||
151 |
// ConstraintEquality constraint2("c2", Matrix::Identity(n,n), |
||
152 |
// Vector::Zero(n)); HQPData[1].push_back(make_pair<double, |
||
153 |
// ConstraintBase*>(damping, &constraint2)); |
||
154 |
|||
155 |
// Vector x(n); |
||
156 |
// svdSolveWithDamping(A, b, x, damping); |
||
157 |
|||
158 |
// Matrix A_in = Matrix::Random(nin, n); |
||
159 |
// Vector A_lb = A_in*x - Vector::Ones(nin) + Vector::Random(nin); |
||
160 |
// Vector A_ub = A_in*x + Vector::Ones(nin) + Vector::Random(nin); |
||
161 |
// ConstraintInequality in_constraint("in1", A_in, A_lb, A_ub); |
||
162 |
// HQPData[0].push_back(make_pair<double, ConstraintBase*>(1.0, |
||
163 |
// &in_constraint)); |
||
164 |
|||
165 |
// const HQPOutput & output = solver->solve(HQPData); |
||
166 |
// BOOST_REQUIRE_MESSAGE(output.status==HQP_STATUS_OPTIMAL, |
||
167 |
// "Status "+toString(output.status)); |
||
168 |
// BOOST_CHECK_MESSAGE(((A_in*output.x).array() <= A_ub.array()).all(), |
||
169 |
// "Upper bound error: "+toString(A_ub - A_in*output.x)); |
||
170 |
// BOOST_CHECK_MESSAGE(((A_in*output.x).array() >= A_lb.array()).all(), |
||
171 |
// "Lower bound error: "+toString(A_in*output.x-A_lb)); |
||
172 |
|||
173 |
// A_lb[0] += 2.0; |
||
174 |
// in_constraint.setLowerBound(A_lb); |
||
175 |
// const HQPOutput & output2 = solver->solve(HQPData); |
||
176 |
// BOOST_REQUIRE_MESSAGE(output.status==HQP_STATUS_OPTIMAL, |
||
177 |
// "Status "+toString(output.status)); |
||
178 |
// BOOST_CHECK_MESSAGE((A_in.row(0)*output.x).isApproxToConstant(A_lb[0]), |
||
179 |
// "Active constraint error: |
||
180 |
// "+toString(A_in.row(0)*output.x-A_lb.head<1>())); |
||
181 |
//} |
||
182 |
|||
183 |
#define PROFILE_EIQUADPROG "Eiquadprog" |
||
184 |
#define PROFILE_EIQUADPROG_RT "Eiquadprog Real Time" |
||
185 |
#define PROFILE_EIQUADPROG_FAST "Eiquadprog Fast" |
||
186 |
#define PROFILE_PROXQP "Proxqp" |
||
187 |
#define PROFILE_OSQP "OSQP" |
||
188 |
#define PROFILE_QPMAD "QPMAD" |
||
189 |
|||
190 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(test_eiquadprog_classic_vs_rt_vs_fast_vs_proxqp) { |
191 |
✓✗ | 2 |
std::cout << "test_eiquadprog_classic_vs_rt_vs_fast\n"; |
192 |
using namespace tsid; |
||
193 |
using namespace math; |
||
194 |
using namespace solvers; |
||
195 |
|||
196 |
2 |
const double EPS = 1e-8; |
|
197 |
#ifdef NDEBUG |
||
198 |
const unsigned int nTest = 100; |
||
199 |
const unsigned int nsmooth = 50; |
||
200 |
#else |
||
201 |
2 |
const unsigned int nTest = 100; |
|
202 |
2 |
const unsigned int nsmooth = 5; |
|
203 |
#endif |
||
204 |
2 |
const unsigned int n = 60; |
|
205 |
2 |
const unsigned int neq = 36; |
|
206 |
2 |
const unsigned int nin = 40; |
|
207 |
2 |
const double damping = 1e-10; |
|
208 |
|||
209 |
// variance of the normal distribution used for generating initial bounds |
||
210 |
2 |
const double NORMAL_DISTR_VAR = 10.0; |
|
211 |
// each cycle the gradient is perturbed by a Gaussian random variable with |
||
212 |
// this covariance |
||
213 |
2 |
const double GRADIENT_PERTURBATION_VARIANCE = 1e-2; |
|
214 |
// each cycle the Hessian is perturbed by a Gaussian random variable with this |
||
215 |
// covariance |
||
216 |
2 |
const double HESSIAN_PERTURBATION_VARIANCE = 1e-1; |
|
217 |
// min margin of activation of the inequality constraints for the first QP |
||
218 |
2 |
const double MARGIN_PERC = 1e-3; |
|
219 |
|||
220 |
✓✗✓✗ ✓✗✓✗ |
2 |
std::cout << "Gonna perform " << nTest << " tests (smooth " << nsmooth |
221 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
<< " times) with " << n << " variables, " << neq << " equalities, " |
222 |
✓✗✓✗ |
2 |
<< nin << " inequalities\n"; |
223 |
|||
224 |
// CREATE SOLVERS |
||
225 |
✓✗✓✗ |
2 |
SolverHQPBase* solver_rt = SolverHQPFactory::createNewSolver<n, neq, nin>( |
226 |
SOLVER_HQP_EIQUADPROG_RT, "eiquadprog_rt"); |
||
227 |
✓✗ | 2 |
solver_rt->resize(n, neq, nin); |
228 |
|||
229 |
✓✗✓✗ |
2 |
SolverHQPBase* solver_fast = SolverHQPFactory::createNewSolver( |
230 |
SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog_fast"); |
||
231 |
✓✗ | 2 |
solver_fast->resize(n, neq, nin); |
232 |
|||
233 |
SolverHQPBase* solver = |
||
234 |
✓✗✓✗ |
2 |
SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG, "eiquadprog"); |
235 |
✓✗ | 2 |
solver->resize(n, neq, nin); |
236 |
|||
237 |
#ifdef TSID_WITH_PROXSUITE |
||
238 |
SolverHQPBase* solver_proxqp = |
||
239 |
SolverHQPFactory::createNewSolver(SOLVER_HQP_PROXQP, "proxqp"); |
||
240 |
#endif |
||
241 |
|||
242 |
#ifdef TSID_WITH_OSQP |
||
243 |
SolverHQPBase* solver_osqp = |
||
244 |
SolverHQPFactory::createNewSolver(SOLVER_HQP_OSQP, "osqp"); |
||
245 |
#endif |
||
246 |
|||
247 |
#ifdef TSID_QPMAD_FOUND |
||
248 |
SolverHQPBase* solver_qpmad = |
||
249 |
SolverHQPFactory::createNewSolver(SOLVER_HQP_QPMAD, "qpmad"); |
||
250 |
solver_qpmad->resize(n, neq, nin); |
||
251 |
#endif |
||
252 |
|||
253 |
// CREATE PROBLEM DATA |
||
254 |
✓✗ | 6 |
HQPData HQPData(2); |
255 |
|||
256 |
✓✗✓✗ |
4 |
Matrix A1 = Matrix::Random(n, n); |
257 |
✓✗✓✗ |
4 |
Vector b1 = Vector::Random(n); |
258 |
✓✗ | 4 |
auto cost = std::make_shared<ConstraintEquality>("c1", A1, b1); |
259 |
✓✗ | 4 |
HQPData[1].push_back( |
260 |
✓✗ | 4 |
solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(1.0, cost)); |
261 |
|||
262 |
✓✗ | 4 |
Vector x(n); |
263 |
✓✗✓✗ ✓✗✓✗ |
2 |
svdSolveWithDamping(A1, b1, x, damping); |
264 |
|||
265 |
✓✗✓✗ |
4 |
Matrix A_in = Matrix::Random(nin, n); |
266 |
✓✗✓✗ ✓✗ |
4 |
Vector A_lb = Vector::Random(nin) * NORMAL_DISTR_VAR; |
267 |
✓✗✓✗ ✓✗ |
4 |
Vector A_ub = Vector::Random(nin) * NORMAL_DISTR_VAR; |
268 |
✓✗✓✗ |
4 |
Vector constrVal = A_in * x; |
269 |
✓✓ | 82 |
for (unsigned int i = 0; i < nin; i++) { |
270 |
✓✗✓✗ ✓✓ |
80 |
if (constrVal[i] > A_ub[i]) { |
271 |
// std::cout<<"Inequality constraint "<<i<<" active at first |
||
272 |
// iteration. UB="<<A_ub[i]<<", value="<<constrVal[i]<<endl; |
||
273 |
✓✗✓✗ ✓✗ |
40 |
A_ub[i] = constrVal[i] + MARGIN_PERC * fabs(constrVal[i]); |
274 |
} |
||
275 |
✓✗✓✗ ✓✓ |
80 |
if (constrVal[i] < A_lb[i]) { |
276 |
// std::cout<<"Inequality constraint "<<i<<" active at first |
||
277 |
// iteration. LB="<<A_lb[i]<<", value="<<constrVal[i]<<endl; |
||
278 |
✓✗✓✗ ✓✗ |
36 |
A_lb[i] = constrVal[i] - MARGIN_PERC * fabs(constrVal[i]); |
279 |
} |
||
280 |
} |
||
281 |
auto in_constraint = |
||
282 |
✓✗ | 4 |
std::make_shared<ConstraintInequality>("in1", A_in, A_lb, A_ub); |
283 |
✓✗ | 4 |
HQPData[0].push_back( |
284 |
4 |
solvers::make_pair<double, std::shared_ptr<ConstraintBase>>( |
|
285 |
✓✗ | 4 |
1.0, in_constraint)); |
286 |
|||
287 |
✓✗✓✗ |
4 |
Matrix A_eq = Matrix::Random(neq, n); |
288 |
✓✗✓✗ |
4 |
Vector b_eq = A_eq * x; |
289 |
✓✗ | 4 |
auto eq_constraint = std::make_shared<ConstraintEquality>("eq1", A_eq, b_eq); |
290 |
✓✗ | 4 |
HQPData[0].push_back( |
291 |
4 |
solvers::make_pair<double, std::shared_ptr<ConstraintBase>>( |
|
292 |
✓✗ | 4 |
1.0, eq_constraint)); |
293 |
|||
294 |
// Prepare random data to perturb initial QP |
||
295 |
✓✗ | 4 |
std::vector<Vector> gradientPerturbations(nTest); |
296 |
✓✗ | 4 |
std::vector<Matrix> hessianPerturbations(nTest); |
297 |
✓✓ | 202 |
for (unsigned int i = 0; i < nTest; i++) { |
298 |
200 |
gradientPerturbations[i] = |
|
299 |
✓✗✓✗ ✓✗ |
400 |
Vector::Random(n) * GRADIENT_PERTURBATION_VARIANCE; |
300 |
200 |
hessianPerturbations[i] = |
|
301 |
✓✗✓✗ ✓✗ |
400 |
Matrix::Random(n, n) * HESSIAN_PERTURBATION_VARIANCE; |
302 |
} |
||
303 |
|||
304 |
// START COMPUTING |
||
305 |
✓✓ | 202 |
for (unsigned int i = 0; i < nTest; i++) { |
306 |
if (true || i == 0) { |
||
307 |
✓✗✓✗ |
200 |
cost->matrix() += hessianPerturbations[i]; |
308 |
✓✗✓✗ |
200 |
cost->vector() += gradientPerturbations[i]; |
309 |
} |
||
310 |
// First run to init outputs |
||
311 |
✓✗✓✗ ✓✗ |
200 |
getProfiler().start(PROFILE_EIQUADPROG_FAST); |
312 |
✓✗ | 200 |
const HQPOutput& output_fast = solver_fast->solve(HQPData); |
313 |
✓✗✓✗ ✓✗ |
200 |
getProfiler().stop(PROFILE_EIQUADPROG_FAST); |
314 |
|||
315 |
✓✗✓✗ ✓✗ |
200 |
getProfiler().start(PROFILE_EIQUADPROG_RT); |
316 |
✓✗ | 200 |
const HQPOutput& output_rt = solver_rt->solve(HQPData); |
317 |
✓✗✓✗ ✓✗ |
200 |
getProfiler().stop(PROFILE_EIQUADPROG_RT); |
318 |
|||
319 |
✓✗✓✗ ✓✗ |
200 |
getProfiler().start(PROFILE_EIQUADPROG); |
320 |
✓✗ | 200 |
const HQPOutput& output = solver->solve(HQPData); |
321 |
✓✗✓✗ ✓✗ |
200 |
getProfiler().stop(PROFILE_EIQUADPROG); |
322 |
|||
323 |
#ifdef TSID_WITH_PROXSUITE |
||
324 |
getProfiler().start(PROFILE_PROXQP); |
||
325 |
const HQPOutput& output_proxqp = solver_proxqp->solve(HQPData); |
||
326 |
getProfiler().stop(PROFILE_PROXQP); |
||
327 |
#endif |
||
328 |
|||
329 |
#ifdef TSID_WITH_OSQP |
||
330 |
getProfiler().start(PROFILE_OSQP); |
||
331 |
const HQPOutput& output_osqp = solver_osqp->solve(HQPData); |
||
332 |
getProfiler().stop(PROFILE_OSQP); |
||
333 |
#endif |
||
334 |
|||
335 |
#ifdef TSID_QPMAD_FOUND |
||
336 |
getProfiler().start(PROFILE_QPMAD); |
||
337 |
const HQPOutput& output_qpmad = solver_qpmad->solve(HQPData); |
||
338 |
getProfiler().stop(PROFILE_QPMAD); |
||
339 |
#endif |
||
340 |
// Loop to smooth variance for each problem |
||
341 |
✓✓ | 1200 |
for (unsigned int j = 0; j < nsmooth; j++) { |
342 |
✓✗✓✗ ✓✗ |
1000 |
getProfiler().start(PROFILE_EIQUADPROG_FAST); |
343 |
✓✗ | 1000 |
(void)solver_fast->solve(HQPData); |
344 |
✓✗✓✗ ✓✗ |
1000 |
getProfiler().stop(PROFILE_EIQUADPROG_FAST); |
345 |
|||
346 |
✓✗✓✗ ✓✗ |
1000 |
getProfiler().start(PROFILE_EIQUADPROG_RT); |
347 |
✓✗ | 1000 |
(void)solver_rt->solve(HQPData); |
348 |
✓✗✓✗ ✓✗ |
1000 |
getProfiler().stop(PROFILE_EIQUADPROG_RT); |
349 |
|||
350 |
✓✗✓✗ ✓✗ |
1000 |
getProfiler().start(PROFILE_EIQUADPROG); |
351 |
✓✗ | 1000 |
(void)solver->solve(HQPData); |
352 |
✓✗✓✗ ✓✗ |
1000 |
getProfiler().stop(PROFILE_EIQUADPROG); |
353 |
|||
354 |
#ifdef TSID_WITH_PROXSUITE |
||
355 |
getProfiler().start(PROFILE_PROXQP); |
||
356 |
(void)solver_proxqp->solve(HQPData); |
||
357 |
getProfiler().stop(PROFILE_PROXQP); |
||
358 |
#endif |
||
359 |
|||
360 |
#ifdef TSID_WITH_OSQP |
||
361 |
getProfiler().start(PROFILE_OSQP); |
||
362 |
(void)solver_osqp->solve(HQPData); |
||
363 |
getProfiler().stop(PROFILE_OSQP); |
||
364 |
#endif |
||
365 |
|||
366 |
#ifdef TSID_QPMAD_FOUND |
||
367 |
getProfiler().start(PROFILE_QPMAD); |
||
368 |
(void)solver_qpmad->solve(HQPData); |
||
369 |
getProfiler().stop(PROFILE_QPMAD); |
||
370 |
#endif |
||
371 |
} |
||
372 |
|||
373 |
✓✗✓✗ ✓✗ |
400 |
getStatistics().store("active inequalities", |
374 |
✓✗ | 200 |
(double)output_rt.activeSet.size()); |
375 |
✓✗✓✗ ✓✗ |
200 |
getStatistics().store("solver iterations", output_rt.iterations); |
376 |
|||
377 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
200 |
BOOST_REQUIRE_MESSAGE( |
378 |
output.status == output_rt.status, |
||
379 |
"Status " + SolverHQPBase::HQP_status_string[output.status] + |
||
380 |
" Status RT " + SolverHQPBase::HQP_status_string[output_rt.status]); |
||
381 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
200 |
BOOST_REQUIRE_MESSAGE( |
382 |
output.status == output_fast.status, |
||
383 |
"Status " + SolverHQPBase::HQP_status_string[output.status] + |
||
384 |
" Status FAST " + |
||
385 |
SolverHQPBase::HQP_status_string[output_fast.status]); |
||
386 |
|||
387 |
#ifdef TSID_WITH_PROXSUITE |
||
388 |
BOOST_REQUIRE_MESSAGE( |
||
389 |
output.status == output_proxqp.status, |
||
390 |
"Status " + SolverHQPBase::HQP_status_string[output.status] + |
||
391 |
" Status Proxqp " + |
||
392 |
SolverHQPBase::HQP_status_string[output_proxqp.status]); |
||
393 |
#endif |
||
394 |
#ifdef TSID_WITH_OSQP |
||
395 |
BOOST_REQUIRE_MESSAGE( |
||
396 |
output.status == output_osqp.status, |
||
397 |
"Status " + SolverHQPBase::HQP_status_string[output.status] + |
||
398 |
" Status Proxqp " + |
||
399 |
SolverHQPBase::HQP_status_string[output_osqp.status]); |
||
400 |
#endif |
||
401 |
|||
402 |
✓✗ | 200 |
if (output.status == HQP_STATUS_OPTIMAL) { |
403 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
200 |
CHECK_LESS_THAN((A_eq * output.x - b_eq).norm(), EPS); |
404 |
|||
405 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
200 |
BOOST_CHECK_MESSAGE( |
406 |
((A_in * output.x).array() <= A_ub.array() + EPS).all(), |
||
407 |
"Lower bounds violated: " + |
||
408 |
toString((A_ub - A_in * output.x).transpose())); |
||
409 |
|||
410 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
200 |
BOOST_CHECK_MESSAGE( |
411 |
((A_in * output.x).array() >= A_lb.array() - EPS).all(), |
||
412 |
"Upper bounds violated: " + |
||
413 |
toString((A_in * output.x - A_lb).transpose())); |
||
414 |
|||
415 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
200 |
BOOST_CHECK_MESSAGE( |
416 |
output.x.isApprox(output_rt.x, EPS), |
||
417 |
// "Sol "+toString(output.x.transpose())+ |
||
418 |
// "\nSol RT |
||
419 |
// "+toString(output_rt.x.transpose())+ |
||
420 |
"\nDiff RT: " + toString((output.x - output_rt.x).norm())); |
||
421 |
|||
422 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓ |
200 |
BOOST_CHECK_MESSAGE( |
423 |
output_rt.x.isApprox(output_fast.x, EPS), |
||
424 |
// "Sol RT"+toString(output_rt.x.transpose())+ |
||
425 |
// "\nSol FAST |
||
426 |
// "+toString(output_fast.x.transpose())+ |
||
427 |
"\nDiff FAST: " + toString((output_rt.x - output_fast.x).norm())); |
||
428 |
|||
429 |
#ifdef TSID_WITH_PROXSUITE |
||
430 |
BOOST_CHECK_MESSAGE( |
||
431 |
output.x.isApprox(output_proxqp.x, 1e-4), |
||
432 |
"\nDiff PROXQP: " + toString((output.x - output_proxqp.x).norm())); |
||
433 |
#endif |
||
434 |
#ifdef TSID_WITH_OSQP |
||
435 |
BOOST_CHECK_MESSAGE( |
||
436 |
output.x.isApprox(output_osqp.x, 1e-4), |
||
437 |
"\nDiff OSQP: " + toString((output.x - output_osqp.x).norm())); |
||
438 |
#endif |
||
439 |
|||
440 |
#ifdef TSID_QPMAD_FOUND |
||
441 |
BOOST_CHECK_MESSAGE( |
||
442 |
output.x.isApprox(output_qpmad.x, EPS), |
||
443 |
"\nDiff QPMAD: " + toString((output.x - output_qpmad.x).norm())); |
||
444 |
#endif |
||
445 |
} |
||
446 |
} |
||
447 |
|||
448 |
✓✗ | 2 |
std::cout << "\n### TEST FINISHED ###\n"; |
449 |
✓✗✓✗ |
2 |
getProfiler().report_all(3, std::cout); |
450 |
✓✗✓✗ |
2 |
getStatistics().report_all(1, std::cout); |
451 |
|||
452 |
✓✗ | 2 |
delete solver; |
453 |
✓✗ | 2 |
delete solver_rt; |
454 |
✓✗ | 2 |
delete solver_fast; |
455 |
|||
456 |
#ifdef TSID_WITH_PROXSUITE |
||
457 |
delete solver_proxqp; |
||
458 |
#endif |
||
459 |
|||
460 |
#ifdef TSID_WITH_OSQP |
||
461 |
delete solver_osqp; |
||
462 |
#endif |
||
463 |
2 |
} |
|
464 |
|||
465 |
BOOST_AUTO_TEST_SUITE_END() |
| Generated by: GCOVR (Version 4.2) |