GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: tests/eiquadprog-rt.cpp Lines: 253 253 100.0 %
Date: 2021-03-10 23:02:29 Branches: 735 1470 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2019 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
#include <iostream>
20
21
#include <Eigen/Core>
22
23
#include <boost/test/unit_test.hpp>
24
25
#include "eiquadprog/eiquadprog-rt.hpp"
26
27
using namespace eiquadprog::solvers;
28
29
/**
30
 * solves the problem
31
 * min. 0.5 * x' Hess x + g0' x
32
 * s.t. CE x + ce0 = 0
33
 *      CI x + ci0 >= 0
34
 */
35
36

1
BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE)
37
38
// min ||x||^2
39
40


















3
BOOST_AUTO_TEST_CASE(test_unbiased) {
41
2
  RtEiquadprog<2, 0, 0> qp;
42
43
1
  RtMatrixX<2, 2>::d Q;
44
1
  Q.setZero();
45
1
  Q(0, 0) = 1.0;
46
1
  Q(1, 1) = 1.0;
47
48
1
  RtVectorX<2>::d C;
49
1
  C.setZero();
50
51
1
  RtMatrixX<0, 2>::d Aeq;
52
53
1
  RtVectorX<0>::d Beq;
54
55
1
  RtMatrixX<0, 2>::d Aineq;
56
57
1
  RtVectorX<0>::d Bineq;
58
59
1
  RtVectorX<2>::d x;
60
61
1
  RtVectorX<2>::d solution;
62
1
  solution.setZero();
63
64
1
  double val = 0.0;
65
66
1
  RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
67
68
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
69
70



1
  BOOST_CHECK_EQUAL(status, expected);
71
72



1
  BOOST_CHECK_CLOSE(qp.getObjValue(), val, 1e-6);
73
74




3
  BOOST_CHECK(x.isApprox(solution));
75
1
}
76
77
// min ||x-x_0||^2, x_0 = (1 1)^T
78
79


















3
BOOST_AUTO_TEST_CASE(test_biased) {
80
2
  RtEiquadprog<2, 0, 0> qp;
81
82
1
  RtMatrixX<2, 2>::d Q;
83
1
  Q.setZero();
84
1
  Q(0, 0) = 1.0;
85
1
  Q(1, 1) = 1.0;
86
87
1
  RtVectorX<2>::d C;
88
1
  C(0) = -1.;
89
1
  C(1) = -1.;
90
91
1
  RtMatrixX<0, 2>::d Aeq;
92
93
1
  RtVectorX<0>::d Beq;
94
95
1
  RtMatrixX<0, 2>::d Aineq;
96
97
1
  RtVectorX<0>::d Bineq;
98
99
1
  RtVectorX<2>::d x;
100
101
1
  RtVectorX<2>::d solution;
102
1
  solution(0) = 1.;
103
1
  solution(1) = 1.;
104
105
1
  double val = -1.;
106
107
1
  RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
108
109
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
110
111



1
  BOOST_CHECK_EQUAL(status, expected);
112
113



1
  BOOST_CHECK_CLOSE(qp.getObjValue(), val, 1e-6);
114
115




3
  BOOST_CHECK(x.isApprox(solution));
116
1
}
117
118
// min ||x||^2
119
//    s.t.
120
// x[1] = 1 - x[0]
121
122


















3
BOOST_AUTO_TEST_CASE(test_equality_constraints) {
123
2
  RtEiquadprog<2, 1, 0> qp;
124
125
1
  RtMatrixX<2, 2>::d Q;
126
1
  Q.setZero();
127
1
  Q(0, 0) = 1.0;
128
1
  Q(1, 1) = 1.0;
129
130
1
  RtVectorX<2>::d C;
131
1
  C.setZero();
132
133
1
  RtMatrixX<1, 2>::d Aeq;
134
1
  Aeq(0, 0) = 1.;
135
1
  Aeq(0, 1) = 1.;
136
137
1
  RtVectorX<1>::d Beq;
138
1
  Beq(0) = -1.;
139
140
1
  RtMatrixX<0, 2>::d Aineq;
141
142
1
  RtVectorX<0>::d Bineq;
143
144
1
  RtVectorX<2>::d x;
145
146
1
  RtVectorX<2>::d solution;
147
1
  solution(0) = 0.5;
148
1
  solution(1) = 0.5;
149
150
1
  double val = 0.25;
151
152
1
  RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
153
154
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
155
156



1
  BOOST_CHECK_EQUAL(status, expected);
157
158



1
  BOOST_CHECK_CLOSE(qp.getObjValue(), val, 1e-6);
159
160




3
  BOOST_CHECK(x.isApprox(solution));
161
1
}
162
163
// min ||x||^2
164
//    s.t.
165
// x[i] >= 1
166
167


















3
BOOST_AUTO_TEST_CASE(test_inequality_constraints) {
168
2
  RtEiquadprog<2, 0, 2> qp;
169
170
1
  RtMatrixX<2, 2>::d Q;
171
1
  Q.setZero();
172
1
  Q(0, 0) = 1.0;
173
1
  Q(1, 1) = 1.0;
174
175
1
  RtVectorX<2>::d C;
176
1
  C.setZero();
177
178
1
  RtMatrixX<0, 2>::d Aeq;
179
180
1
  RtVectorX<0>::d Beq(0);
181
182
1
  RtMatrixX<2, 2>::d Aineq;
183
1
  Aineq.setZero();
184
1
  Aineq(0, 0) = 1.;
185
1
  Aineq(1, 1) = 1.;
186
187
1
  RtVectorX<2>::d Bineq;
188
1
  Bineq(0) = -1.;
189
1
  Bineq(1) = -1.;
190
191
1
  RtVectorX<2>::d x;
192
193
1
  RtVectorX<2>::d solution;
194
1
  solution(0) = 1.;
195
1
  solution(1) = 1.;
196
197
1
  double val = 1.;
198
199
1
  RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
200
201
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
202
203



1
  BOOST_CHECK_EQUAL(status, expected);
204
205



1
  BOOST_CHECK_CLOSE(qp.getObjValue(), val, 1e-6);
206
207




3
  BOOST_CHECK(x.isApprox(solution));
208
1
}
209
210
// min ||x-x_0||^2, x_0 = (1 1)^T
211
//    s.t.
212
// x[1] = 5 - x[0]
213
// x[1] >= 3
214
215


















3
BOOST_AUTO_TEST_CASE(test_full) {
216
2
  RtEiquadprog<2, 1, 1> qp;
217
218
1
  RtMatrixX<2, 2>::d Q;
219
1
  Q.setZero();
220
1
  Q(0, 0) = 1.0;
221
1
  Q(1, 1) = 1.0;
222
223
1
  RtVectorX<2>::d C;
224
1
  C(0) = -1.;
225
1
  C(1) = -1.;
226
227
1
  RtMatrixX<1, 2>::d Aeq;
228
1
  Aeq(0, 0) = 1.;
229
1
  Aeq(0, 1) = 1.;
230
231
1
  RtVectorX<1>::d Beq;
232
1
  Beq(0) = -5.;
233
234
1
  RtMatrixX<1, 2>::d Aineq;
235
1
  Aineq.setZero();
236
1
  Aineq(0, 1) = 1.;
237
238
1
  RtVectorX<1>::d Bineq;
239
1
  Bineq(0) = -3.;
240
241
1
  RtVectorX<2>::d x;
242
243
1
  RtVectorX<2>::d solution;
244
1
  solution(0) = 2.;
245
1
  solution(1) = 3.;
246
247
1
  double val = 1.5;
248
249
1
  RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
250
251
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
252
253



1
  BOOST_CHECK_EQUAL(status, expected);
254
255



1
  BOOST_CHECK_CLOSE(qp.getObjValue(), val, 1e-6);
256
257




3
  BOOST_CHECK(x.isApprox(solution));
258
1
}
259
260
// min ||x||^2
261
//    s.t.
262
// x[0] =  1
263
// x[0] = -1
264
265


















3
BOOST_AUTO_TEST_CASE(test_unfeasible_equalities) {
266
2
  RtEiquadprog<2, 2, 0> qp;
267
268
1
  RtMatrixX<2, 2>::d Q;
269
1
  Q.setZero();
270
1
  Q(0, 0) = 1.0;
271
1
  Q(1, 1) = 1.0;
272
273
1
  RtVectorX<2>::d C;
274
1
  C.setZero();
275
276
1
  RtMatrixX<2, 2>::d Aeq;
277
1
  Aeq.setZero();
278
1
  Aeq(0, 0) = 1.;
279
1
  Aeq(1, 0) = 1.;
280
281
1
  RtVectorX<2>::d Beq;
282
1
  Beq(0) = -1.;
283
1
  Beq(1) = 1.;
284
285
1
  RtMatrixX<0, 2>::d Aineq;
286
287
1
  RtVectorX<0>::d Bineq;
288
289
1
  RtVectorX<2>::d x;
290
291
1
  RtEiquadprog_status expected = RT_EIQUADPROG_REDUNDANT_EQUALITIES;
292
293
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
294
295



1
  BOOST_CHECK_EQUAL(status, expected);
296
1
}
297
298
// min ||x||^2
299
//    s.t.
300
// x[0] >=  1
301
// x[0] <= -1
302
//
303
// correctly fails, but returns wrong error code
304
305


















3
BOOST_AUTO_TEST_CASE(test_unfeasible_inequalities) {
306
2
  RtEiquadprog<2, 0, 2> qp;
307
308
1
  RtMatrixX<2, 2>::d Q;
309
1
  Q.setZero();
310
1
  Q(0, 0) = 1.0;
311
1
  Q(1, 1) = 1.0;
312
313
1
  RtVectorX<2>::d C;
314
1
  C.setZero();
315
316
1
  RtMatrixX<0, 2>::d Aeq;
317
318
1
  RtVectorX<0>::d Beq;
319
320
1
  RtMatrixX<2, 2>::d Aineq;
321
1
  Aineq.setZero();
322
1
  Aineq(0, 0) = 1.;
323
1
  Aineq(1, 0) = -1.;
324
325
1
  RtVectorX<2>::d Bineq;
326
1
  Bineq(0) = -1;
327
1
  Bineq(1) = -1;
328
329
1
  RtVectorX<2>::d x;
330
331
1
  RtEiquadprog_status expected = RT_EIQUADPROG_INFEASIBLE;
332
333
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
334
335



1
  BOOST_WARN_EQUAL(status, expected);
336



1
  BOOST_CHECK(status != RT_EIQUADPROG_OPTIMAL);
337
1
}
338
339
// min ||x-x_0||^2, x_0 = (1 1)^T
340
//    s.t.
341
// x[1] = 1 - x[0]
342
// x[0] <= 0
343
// x[1] <= 0
344
//
345
// correctly fails, but returns wrong error code
346
347


















3
BOOST_AUTO_TEST_CASE(test_unfeasible_constraints) {
348
2
  RtEiquadprog<2, 1, 2> qp;
349
350
1
  RtMatrixX<2, 2>::d Q;
351
1
  Q.setZero();
352
1
  Q(0, 0) = 1.0;
353
1
  Q(1, 1) = 1.0;
354
355
1
  RtVectorX<2>::d C;
356
1
  C(0) = -1.;
357
1
  C(1) = -1.;
358
359
1
  RtMatrixX<1, 2>::d Aeq;
360
1
  Aeq(0, 0) = 1.;
361
1
  Aeq(0, 1) = 1.;
362
363
1
  RtVectorX<1>::d Beq;
364
1
  Beq(0) = -1.;
365
366
1
  RtMatrixX<2, 2>::d Aineq;
367
1
  Aineq.setZero();
368
1
  Aineq(0, 0) = -1.;
369
1
  Aineq(1, 1) = -1.;
370
371
1
  RtVectorX<2>::d Bineq;
372
1
  Bineq.setZero();
373
374
1
  RtVectorX<2>::d x;
375
376
1
  RtEiquadprog_status expected = RT_EIQUADPROG_INFEASIBLE;
377
378
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
379
380



1
  BOOST_WARN_EQUAL(status, expected);
381



1
  BOOST_CHECK(status != RT_EIQUADPROG_OPTIMAL);
382
1
}
383
384
// min -||x||^2
385
// DOES NOT WORK!
386
387


















3
BOOST_AUTO_TEST_CASE(test_unbounded) {
388
2
  RtEiquadprog<2, 0, 0> qp;
389
390
1
  RtMatrixX<2, 2>::d Q;
391
1
  Q.setZero();
392
1
  Q(0, 0) = -1.0;
393
1
  Q(1, 1) = -1.0;
394
395
1
  RtVectorX<2>::d C;
396
1
  C.setZero();
397
398
1
  RtMatrixX<0, 2>::d Aeq;
399
400
1
  RtVectorX<0>::d Beq;
401
402
1
  RtMatrixX<0, 2>::d Aineq;
403
404
1
  RtVectorX<0>::d Bineq;
405
406
1
  RtVectorX<2>::d x;
407
408
1
  RtEiquadprog_status expected = RT_EIQUADPROG_UNBOUNDED;
409
410
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
411
412



1
  BOOST_WARN_EQUAL(status, expected);
413



1
  BOOST_WARN(status != RT_EIQUADPROG_OPTIMAL);  // SHOULD pass!
414
1
}
415
416
// min -||x||^2
417
//    s.t.
418
// 0<= x[0] <= 1
419
// 0<= x[1] <= 1
420
// DOES NOT WORK!
421
422


















3
BOOST_AUTO_TEST_CASE(test_nonconvex) {
423
2
  RtEiquadprog<2, 0, 4> qp;
424
425
1
  RtMatrixX<2, 2>::d Q;
426
1
  Q.setZero();
427
1
  Q(0, 0) = -1.0;
428
1
  Q(1, 1) = -1.0;
429
430
1
  RtVectorX<2>::d C;
431
1
  C.setZero();
432
433
1
  RtMatrixX<0, 2>::d Aeq;
434
435
1
  RtVectorX<0>::d Beq;
436
437
1
  RtMatrixX<4, 2>::d Aineq(4, 2);
438
1
  Aineq.setZero();
439
1
  Aineq(0, 0) = 1.;
440
1
  Aineq(1, 0) = -1.;
441
1
  Aineq(2, 1) = 1.;
442
1
  Aineq(3, 1) = -1.;
443
444
1
  RtVectorX<4>::d Bineq;
445
1
  Bineq(0) = 0.;
446
1
  Bineq(1) = 1.;
447
1
  Bineq(2) = 0.;
448
1
  Bineq(3) = 1.;
449
450
1
  RtVectorX<2>::d x;
451
452
1
  RtVectorX<2>::d solution;
453
1
  solution(0) = 1.;
454
1
  solution(1) = 1.;
455
456
1
  double val = -1.;
457
458
1
  RtEiquadprog_status expected = RT_EIQUADPROG_OPTIMAL;
459
460
1
  RtEiquadprog_status status = qp.solve_quadprog(Q, C, Aeq, Beq, Aineq, Bineq, x);
461
462



1
  BOOST_CHECK_EQUAL(status, expected);
463
464



1
  BOOST_WARN_CLOSE(qp.getObjValue(), val, 1e-6);
465
466




3
  BOOST_WARN(x.isApprox(solution));
467
1
}
468
469

3
BOOST_AUTO_TEST_SUITE_END()