Directory: | ./ |
---|---|
File: | tests/hqp_solvers.cpp |
Date: | 2024-08-26 20:29:39 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 96 | 96 | 100.0% |
Branches: | 268 | 524 | 51.1% |
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 |
33/66✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
|
4 | BOOST_AUTO_TEST_CASE(test_eiquadprog_classic_vs_rt_vs_fast_vs_proxqp) { |
191 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | std::cout << "Gonna perform " << nTest << " tests (smooth " << nsmooth |
221 |
5/10✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
|
2 | << " times) with " << n << " variables, " << neq << " equalities, " |
222 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | << nin << " inequalities\n"; |
223 | |||
224 | // CREATE SOLVERS | ||
225 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPBase* solver_rt = SolverHQPFactory::createNewSolver<n, neq, nin>( |
226 | SOLVER_HQP_EIQUADPROG_RT, "eiquadprog_rt"); | ||
227 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | solver_rt->resize(n, neq, nin); |
228 | |||
229 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPBase* solver_fast = SolverHQPFactory::createNewSolver( |
230 | SOLVER_HQP_EIQUADPROG_FAST, "eiquadprog_fast"); | ||
231 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | solver_fast->resize(n, neq, nin); |
232 | |||
233 | SolverHQPBase* solver = | ||
234 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | SolverHQPFactory::createNewSolver(SOLVER_HQP_EIQUADPROG, "eiquadprog"); |
235 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
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 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | HQPData HQPData(2); |
255 | |||
256 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Matrix A1 = Matrix::Random(n, n); |
257 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector b1 = Vector::Random(n); |
258 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | auto cost = std::make_shared<ConstraintEquality>("c1", A1, b1); |
259 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | HQPData[1].push_back( |
260 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(1.0, cost)); |
261 | |||
262 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | Vector x(n); |
263 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
2 | svdSolveWithDamping(A1, b1, x, damping); |
264 | |||
265 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Matrix A_in = Matrix::Random(nin, n); |
266 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | Vector A_lb = Vector::Random(nin) * NORMAL_DISTR_VAR; |
267 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | Vector A_ub = Vector::Random(nin) * NORMAL_DISTR_VAR; |
268 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector constrVal = A_in * x; |
269 |
2/2✓ Branch 0 taken 40 times.
✓ Branch 1 taken 1 times.
|
82 | for (unsigned int i = 0; i < nin; i++) { |
270 |
4/6✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✓ Branch 7 taken 20 times.
|
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 |
3/6✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
|
40 | A_ub[i] = constrVal[i] + MARGIN_PERC * fabs(constrVal[i]); |
274 | } | ||
275 |
4/6✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
✓ Branch 7 taken 22 times.
|
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 |
3/6✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18 times.
✗ Branch 8 not taken.
|
36 | A_lb[i] = constrVal[i] - MARGIN_PERC * fabs(constrVal[i]); |
279 | } | ||
280 | } | ||
281 | auto in_constraint = | ||
282 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::make_shared<ConstraintInequality>("in1", A_in, A_lb, A_ub); |
283 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | HQPData[0].push_back( |
284 | 4 | solvers::make_pair<double, std::shared_ptr<ConstraintBase>>( | |
285 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | 1.0, in_constraint)); |
286 | |||
287 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Matrix A_eq = Matrix::Random(neq, n); |
288 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | Vector b_eq = A_eq * x; |
289 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | auto eq_constraint = std::make_shared<ConstraintEquality>("eq1", A_eq, b_eq); |
290 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | HQPData[0].push_back( |
291 | 4 | solvers::make_pair<double, std::shared_ptr<ConstraintBase>>( | |
292 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | 1.0, eq_constraint)); |
293 | |||
294 | // Prepare random data to perturb initial QP | ||
295 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<Vector> gradientPerturbations(nTest); |
296 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | std::vector<Matrix> hessianPerturbations(nTest); |
297 |
2/2✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
|
202 | for (unsigned int i = 0; i < nTest; i++) { |
298 | 200 | gradientPerturbations[i] = | |
299 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
400 | Vector::Random(n) * GRADIENT_PERTURBATION_VARIANCE; |
300 | 200 | hessianPerturbations[i] = | |
301 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
400 | Matrix::Random(n, n) * HESSIAN_PERTURBATION_VARIANCE; |
302 | } | ||
303 | |||
304 | // START COMPUTING | ||
305 |
2/2✓ Branch 0 taken 100 times.
✓ Branch 1 taken 1 times.
|
202 | for (unsigned int i = 0; i < nTest; i++) { |
306 | if (true || i == 0) { | ||
307 |
2/4✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
|
200 | cost->matrix() += hessianPerturbations[i]; |
308 |
2/4✓ Branch 3 taken 100 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 100 times.
✗ Branch 7 not taken.
|
200 | cost->vector() += gradientPerturbations[i]; |
309 | } | ||
310 | // First run to init outputs | ||
311 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
|
200 | getProfiler().start(PROFILE_EIQUADPROG_FAST); |
312 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
200 | const HQPOutput& output_fast = solver_fast->solve(HQPData); |
313 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
|
200 | getProfiler().stop(PROFILE_EIQUADPROG_FAST); |
314 | |||
315 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
|
200 | getProfiler().start(PROFILE_EIQUADPROG_RT); |
316 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
200 | const HQPOutput& output_rt = solver_rt->solve(HQPData); |
317 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
|
200 | getProfiler().stop(PROFILE_EIQUADPROG_RT); |
318 | |||
319 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
|
200 | getProfiler().start(PROFILE_EIQUADPROG); |
320 |
1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
|
200 | const HQPOutput& output = solver->solve(HQPData); |
321 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
|
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 |
2/2✓ Branch 0 taken 500 times.
✓ Branch 1 taken 100 times.
|
1200 | for (unsigned int j = 0; j < nsmooth; j++) { |
342 |
3/6✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 500 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 500 times.
✗ Branch 9 not taken.
|
1000 | getProfiler().start(PROFILE_EIQUADPROG_FAST); |
343 |
1/2✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
|
1000 | (void)solver_fast->solve(HQPData); |
344 |
3/6✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 500 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 500 times.
✗ Branch 9 not taken.
|
1000 | getProfiler().stop(PROFILE_EIQUADPROG_FAST); |
345 | |||
346 |
3/6✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 500 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 500 times.
✗ Branch 9 not taken.
|
1000 | getProfiler().start(PROFILE_EIQUADPROG_RT); |
347 |
1/2✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
|
1000 | (void)solver_rt->solve(HQPData); |
348 |
3/6✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 500 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 500 times.
✗ Branch 9 not taken.
|
1000 | getProfiler().stop(PROFILE_EIQUADPROG_RT); |
349 | |||
350 |
3/6✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 500 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 500 times.
✗ Branch 9 not taken.
|
1000 | getProfiler().start(PROFILE_EIQUADPROG); |
351 |
1/2✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
|
1000 | (void)solver->solve(HQPData); |
352 |
3/6✓ Branch 1 taken 500 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 500 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 500 times.
✗ Branch 9 not taken.
|
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 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 100 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 100 times.
✗ Branch 8 not taken.
|
400 | getStatistics().store("active inequalities", |
374 | 200 | (double)output_rt.activeSet.size()); | |
375 |
3/6✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 100 times.
✗ Branch 9 not taken.
|
200 | getStatistics().store("solver iterations", output_rt.iterations); |
376 | |||
377 |
9/18✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 100 times.
|
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 |
9/18✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✗ Branch 33 not taken.
✓ Branch 34 taken 100 times.
|
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 |
1/2✓ Branch 0 taken 100 times.
✗ Branch 1 not taken.
|
200 | if (output.status == HQP_STATUS_OPTIMAL) { |
403 |
16/32✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 100 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 100 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 100 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 100 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 100 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 100 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 100 times.
✗ Branch 47 not taken.
✗ Branch 55 not taken.
✓ Branch 56 taken 100 times.
|
200 | CHECK_LESS_THAN((A_eq * output.x - b_eq).norm(), EPS); |
404 | |||
405 |
17/34✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 100 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 100 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 100 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 100 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 100 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 100 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 100 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 100 times.
✗ Branch 50 not taken.
✗ Branch 56 not taken.
✓ Branch 57 taken 100 times.
|
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 |
17/34✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 100 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 100 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 100 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 100 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 100 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 100 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 100 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 100 times.
✗ Branch 50 not taken.
✗ Branch 56 not taken.
✓ Branch 57 taken 100 times.
|
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 |
11/22✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 100 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 100 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 100 times.
|
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 |
11/22✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.
✓ Branch 10 taken 100 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 100 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 100 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 100 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 100 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 100 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 100 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 100 times.
✗ Branch 32 not taken.
✗ Branch 38 not taken.
✓ Branch 39 taken 100 times.
|
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 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | std::cout << "\n### TEST FINISHED ###\n"; |
449 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | getProfiler().report_all(3, std::cout); |
450 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | getStatistics().report_all(1, std::cout); |
451 | |||
452 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | delete solver; |
453 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
2 | delete solver_rt; |
454 |
1/2✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
|
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() | ||
466 |