GCC Code Coverage Report

Directory: ./
File: tests/Main.cpp
Date: 2025-05-07 13:05:43
Exec Total Coverage
Lines: 162 198 81.8%
Branches: 179 432 41.4%
Line Branch Exec Source
1 #include "parametric-curves/polynomial.hpp"
2 #include "parametric-curves/spline.hpp"
3 #ifdef EXTENDED
4 #include "parametric-curves/bezier_curve.h"
5 #include "parametric-curves/helpers/effector_spline.h"
6 #include "parametric-curves/helpers/effector_spline_rotation.h"
7 #include "parametric-curves/spline_deriv_constraint.h"
8 #endif
9 #include <cmath>
10 #include <iostream>
11 #include <string>
12
13 using namespace std;
14
15 namespace parametriccurves {
16 typedef Eigen::Vector3d point_t;
17 typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
18 typedef Polynomial<double, 3, point_t> polynom_t;
19 typedef Spline<double, 3, point_t> Spline_t;
20 #ifdef TEST_EXTENDED
21 typedef spline_deriv_constraint<double, double, 3, true, point_t>
22 spline_deriv_constraint_t;
23 typedef bezier_curve<double, double, 3, true, point_t> bezier_curve_t;
24 #endif
25 typedef Spline_t::spline_constraints spline_constraints_t;
26 typedef std::pair<double, point_t> Waypoint;
27 typedef std::vector<Waypoint> T_Waypoint;
28
29 typedef Eigen::Matrix<double, 1, 1> point_one;
30 typedef Polynomial<double, 1, point_one> polynom_one;
31 typedef Spline<double, 1, point_one> Spline_one;
32 typedef std::pair<double, point_one> WaypointOne;
33 typedef std::vector<WaypointOne> T_WaypointOne;
34
35 ✗ bool QuasiEqual(const double a, const double b, const float margin) {
36 ✗ if ((a <= 0 && b <= 0) || (a >= 0 && b >= 0)) {
37 ✗ return (abs(a - b)) <= margin;
38 } else {
39 ✗ return abs(a) + abs(b) <= margin;
40 }
41 }
42
43 const double margin = 0.001;
44
45 } // namespace parametriccurves
46 using namespace parametriccurves;
47 ✗ ostream& operator<<(ostream& os, const point_t& pt) {
48 ✗ os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
49 ✗ return os;
50 }
51
52 34 void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2,
53 const std::string& errmsg, bool& error,
54 bool notequal = false) {
55
4/10
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 34 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✓ Branch 11 taken 34 times.
 34 if ((pt1 - pt2).norm() > margin && !notequal) {
56 ✗ error = true;
57 ✗ std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose()
58 ✗ << std::endl;
59 }
60 34 }
61
62 /*Cubic Function tests*/
63
64 1 void CubicFunctionTest(bool& error) {
65
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Spline<double, Eigen::Dynamic> test;
66
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
67
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t a(1, 2, 3);
68
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t b(2, 3, 4);
69
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t c(3, 4, 5);
70
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t d(3, 6, 7);
71
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 t_point_t vec;
72
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vec.push_back(a);
73
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vec.push_back(b);
74
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vec.push_back(c);
75
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vec.push_back(d);
76
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 polynom_t cf(vec.begin(), vec.end(), 0, 1);
77
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t res1;
78
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf(0);
79
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t x0(1, 2, 3);
80
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.
 1 ComparePoints(x0, res1, errMsg + "(0) ", error);
81
82
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t x1(9, 15, 19);
83
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf(1);
84
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.
 1 ComparePoints(x1, res1, errMsg + "(1) ", error);
85
86
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t x2(3.125, 5.25, 7.125);
87
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf(0.5);
88
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.
 1 ComparePoints(x2, res1, errMsg + "(0.5) ", error);
89
90 1 vec.clear();
91
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vec.push_back(a);
92
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vec.push_back(b);
93
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vec.push_back(c);
94
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 vec.push_back(d);
95
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 polynom_t cf2(vec, 0.5, 1);
96
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = cf2(0.5);
97
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t x4(3.125, 5.25, 7.125);
98
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.
 1 ComparePoints(x4, res1, errMsg + "x3 ", error);
99 1 error = true;
100 try {
101
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cf2(0.4);
102 1 } catch (...) {
103 1 error = false;
104
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
105
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
106 std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range "
107 ✗ "value\n";
108 }
109 1 error = true;
110 try {
111
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 cf2(1.1);
112 1 } catch (...) {
113 1 error = false;
114
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
115
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
116 std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range "
117 ✗ "value\n";
118 }
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (cf.tmax() != 1) {
120 ✗ error = true;
121 std::cout
122 ✗ << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
123 }
124
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (cf.tmin() != 0) {
125 ✗ error = true;
126 std::cout
127 ✗ << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
128 }
129 1 }
130
131 #ifdef TEST_EXTENDED
132 /*bezier_curve Function tests*/
133 void BezierCurveTest(bool& error) {
134 std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
135 point_t a(1, 2, 3);
136 point_t b(2, 3, 4);
137 point_t c(3, 4, 5);
138 point_t d(3, 6, 7);
139
140 std::vector<point_t> params;
141 params.push_back(a);
142 params.push_back(b);
143
144 // 2d curve
145 bezier_curve_t cf(params.begin(), params.end());
146 point_t res1;
147 res1 = cf(0);
148 point_t x20 = a;
149 ComparePoints(x20, res1, errMsg + "2(0) ", error);
150
151 point_t x21 = b;
152 res1 = cf(1);
153 ComparePoints(x21, res1, errMsg + "2(1) ", error);
154
155 // 3d curve
156 params.push_back(c);
157 bezier_curve_t cf3(params.begin(), params.end());
158 res1 = cf3(0);
159 ComparePoints(a, res1, errMsg + "3(0) ", error);
160
161 res1 = cf3(1);
162 ComparePoints(c, res1, errMsg + "3(1) ", error);
163
164 // 4d curve
165 params.push_back(d);
166 bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2);
167 res1 = cf4(0.4);
168 ComparePoints(a, res1, errMsg + "3(0) ", error);
169
170 res1 = cf4(2);
171 ComparePoints(d, res1, errMsg + "3(1) ", error);
172
173 // testing bernstein polynomes
174 std::string errMsg2(
175 "In test BezierCurveTest ; Bernstein polynoms do not evaluate as "
176 "analytical evaluation");
177 for (double d = 0.; d < 1.; d += 0.1) {
178 ComparePoints(cf3.evalBernstein(d), cf3(d), errMsg2, error);
179 ComparePoints(cf3.evalHorner(d), cf3(d), errMsg2, error);
180 }
181
182 bool error_in(true);
183 try {
184 cf(-0.4);
185 } catch (...) {
186 error_in = false;
187 }
188 if (error_in) {
189 std::cout << "Evaluation of bezier cf error, -0.4 should be an out of "
190 "range value\n";
191 error = true;
192 }
193 error_in = true;
194 try {
195 cf(1.1);
196 } catch (...) {
197 error_in = false;
198 }
199 if (error_in) {
200 std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range "
201 "value\n";
202 error = true;
203 }
204 if (cf.max() != 1) {
205 error = true;
206 std::cout
207 << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
208 }
209 if (cf.min() != 0) {
210 error = true;
211 std::cout
212 << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
213 }
214 }
215
216 #include <ctime>
217 void BezierCurveTestCompareHornerAndBernstein(bool& error) {
218 using namespace std;
219 std::vector<double> values;
220 for (int i = 0; i < 100000; ++i) values.push_back(rand() / RAND_MAX);
221
222 // first compare regular evaluation (low dim pol)
223 point_t a(1, 2, 3);
224 point_t b(2, 3, 4);
225 point_t c(3, 4, 5);
226 point_t d(3, 6, 7);
227 point_t e(3, 61, 7);
228 point_t f(3, 56, 7);
229 point_t g(3, 36, 7);
230 point_t h(43, 6, 7);
231 point_t i(3, 6, 77);
232
233 std::vector<point_t> params;
234 params.push_back(a);
235 params.push_back(b);
236 params.push_back(c);
237
238 // 3d curve
239 bezier_curve_t cf(params.begin(), params.end());
240
241 clock_t s0, e0, s1, e1, s2, e2;
242 s0 = clock();
243 for (std::vector<double>::const_iterator cit = values.begin();
244 cit != values.end(); ++cit) {
245 cf(*cit);
246 }
247 e0 = clock();
248
249 s1 = clock();
250 for (std::vector<double>::const_iterator cit = values.begin();
251 cit != values.end(); ++cit) {
252 cf.evalBernstein(*cit);
253 }
254 e1 = clock();
255
256 s2 = clock();
257 for (std::vector<double>::const_iterator cit = values.begin();
258 cit != values.end(); ++cit) {
259 cf.evalHorner(*cit);
260 }
261 e2 = clock();
262
263 std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC
264 << std::endl;
265 std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC
266 << std::endl;
267 std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC
268 << std::endl;
269
270 std::cout << "now with high order polynom " << std::endl;
271
272 params.push_back(d);
273 params.push_back(e);
274 params.push_back(f);
275 params.push_back(g);
276 params.push_back(h);
277 params.push_back(i);
278
279 bezier_curve_t cf2(params.begin(), params.end());
280
281 s1 = clock();
282 for (std::vector<double>::const_iterator cit = values.begin();
283 cit != values.end(); ++cit) {
284 cf2.evalBernstein(*cit);
285 }
286 e1 = clock();
287
288 s2 = clock();
289 for (std::vector<double>::const_iterator cit = values.begin();
290 cit != values.end(); ++cit) {
291 cf2.evalHorner(*cit);
292 }
293 e2 = clock();
294
295 s0 = clock();
296 for (std::vector<double>::const_iterator cit = values.begin();
297 cit != values.end(); ++cit) {
298 cf2(*cit);
299 }
300 e0 = clock();
301
302 std::cout << "time for analytical eval " << double(e0 - s0) / CLOCKS_PER_SEC
303 << std::endl;
304 std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC
305 << std::endl;
306 std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC
307 << std::endl;
308 }
309
310 void BezierDerivativeCurveTest(bool& error) {
311 std::string errMsg(
312 "In test BezierDerivativeCurveTest ; unexpected result for x ");
313 point_t a(1, 2, 3);
314 point_t b(2, 3, 4);
315 point_t c(3, 4, 5);
316
317 std::vector<point_t> params;
318 params.push_back(a);
319 params.push_back(b);
320
321 params.push_back(c);
322 bezier_curve_t cf3(params.begin(), params.end());
323
324 ComparePoints(cf3(0), cf3.derivate(0., 0), errMsg, error);
325 ComparePoints(cf3(0), cf3.derivate(0., 1), errMsg, error, true);
326 ComparePoints(point_t::Zero(), cf3.derivate(0., 100), errMsg, error);
327 }
328
329 void BezierDerivativeCurveConstraintTest(bool& error) {
330 std::string errMsg(
331 "In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
332 point_t a(1, 2, 3);
333 point_t b(2, 3, 4);
334 point_t c(3, 4, 5);
335
336 bezier_curve_t::curve_constraints_t constraints;
337 constraints.init_vel = point_t(-1, -1, -1);
338 constraints.init_acc = point_t(-2, -2, -2);
339 constraints.end_vel = point_t(-10, -10, -10);
340 constraints.end_acc = point_t(-20, -20, -20);
341
342 std::vector<point_t> params;
343 params.push_back(a);
344 params.push_back(b);
345
346 params.push_back(c);
347 bezier_curve_t cf3(params.begin(), params.end(), constraints);
348
349 assert(cf3.degree_ == params.size() + 3);
350 assert(cf3.size_ == params.size() + 4);
351
352 ComparePoints(a, cf3(0), errMsg, error);
353 ComparePoints(c, cf3(1), errMsg, error);
354 ComparePoints(constraints.init_vel, cf3.derivate(0., 1), errMsg, error);
355 ComparePoints(constraints.end_vel, cf3.derivate(1., 1), errMsg, error);
356 ComparePoints(constraints.init_acc, cf3.derivate(0., 2), errMsg, error);
357 ComparePoints(constraints.end_vel, cf3.derivate(1., 1), errMsg, error);
358 ComparePoints(constraints.end_acc, cf3.derivate(1., 2), errMsg, error);
359 }
360
361 #endif
362 /*Exact Cubic Function tests*/
363 1 void ExactCubicNoErrorTest(bool& error) {
364 1 T_Waypoint waypoints;
365
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 1; i = i + 0.2) {
366
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 waypoints.push_back(std::make_pair(i, point_t(i, i, i)));
367 }
368
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Spline_t exactCubic;
369
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exactCubic.createSplineFromWayPoints(waypoints.begin(), waypoints.end());
370
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t res1;
371 try {
372
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 exactCubic(0);
373
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 exactCubic(1);
374 ✗ } catch (...) {
375 ✗ error = true;
376 ✗ std::cout << "Evaluation of ExactCubicNoErrorTest error\n";
377 }
378 1 error = true;
379 try {
380
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 exactCubic(1.2);
381 1 } catch (...) {
382 1 error = false;
383
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 }
384
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
385 std::cout << "Evaluation of exactCubic cf error, 1.2 should be an out of "
386 ✗ "range value\n";
387 }
388
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (exactCubic.tmax() != 1) {
389 ✗ error = true;
390 std::cout
391 ✗ << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
392 }
393
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 if (exactCubic.tmin() != 0) {
394 ✗ error = true;
395 std::cout
396 ✗ << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
397 }
398 1 }
399
400 /*Exact Cubic Function tests*/
401 1 void ExactCubicTwoPointsTest(bool& error) {
402 1 T_Waypoint waypoints;
403
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 1 times.
 3 for (double i = 0; i < 2; ++i) {
404
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 waypoints.push_back(std::make_pair(i, point_t(i, i, i)));
405 }
406
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Spline_t exactCubic;
407
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exactCubic.createSplineFromWayPoints(waypoints.begin(), waypoints.end());
408
409
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_t res1 = exactCubic(0);
410 std::string errmsg(
411 "in ExactCubic 2 points Error While checking that given wayPoints are "
412
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "crossed (expected / obtained)");
413
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.
 1 ComparePoints(point_t(0, 0, 0), res1, errmsg, error);
414
415
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = exactCubic(1);
416
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.
 1 ComparePoints(point_t(1, 1, 1), res1, errmsg, error);
417 1 }
418
419 1 void ExactCubicOneDimTest(bool& error) {
420 1 T_WaypointOne waypoints;
421
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_one zero;
422
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 zero(0, 0) = 9;
423
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_one one;
424
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 one(0, 0) = 14;
425
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_one two;
426
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 two(0, 0) = 25;
427
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 waypoints.push_back(std::make_pair(0., zero));
428
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 waypoints.push_back(std::make_pair(1., one));
429
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 waypoints.push_back(std::make_pair(2., two));
430
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Spline_one exactCubic;
431
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exactCubic.createSplineFromWayPoints(waypoints.begin(), waypoints.end());
432
433
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 point_one res1 = exactCubic(0);
434 std::string errmsg(
435 "in ExactCubicOneDim Error While checking that given wayPoints are "
436
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "crossed (expected / obtained)");
437
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.
 1 ComparePoints(zero, res1, errmsg, error);
438
439
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res1 = exactCubic(1);
440
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.
 1 ComparePoints(one, res1, errmsg, error);
441 1 }
442
443 3 void CheckWayPointConstraint(const std::string& errmsg, const double step,
444 const T_Waypoint&, const Spline_t* curve,
445 bool& error) {
446
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 point_t res1;
447
2/2
✓ Branch 0 taken 18 times.
✓ Branch 1 taken 3 times.
 21 for (double i = 0; i <= 1; i = i + step) {
448
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 res1 = (*curve)(i);
449
4/8
✓ 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.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
 18 ComparePoints(point_t(i, i, i), res1, errmsg, error);
450 }
451 3 }
452
453 8 void CheckDerivative(const std::string& errmsg, const double eval_point,
454 const std::size_t order, const point_t& target,
455 const Spline_t* curve, bool& error) {
456
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 point_t res1 = curve->derivate(eval_point, order);
457
3/6
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 ComparePoints(target, res1, errmsg, error);
458 8 }
459
460 1 void ExactCubicPointsCrossedTest(bool& error) {
461 1 T_Waypoint waypoints;
462
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 1; i = i + 0.2) {
463
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 waypoints.push_back(std::make_pair(i, point_t(i, i, i)));
464 }
465
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Spline_t exactCubic;
466
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exactCubic.createSplineFromWayPoints(waypoints.begin(), waypoints.end());
467 std::string errmsg(
468 "Error While checking that given wayPoints are crossed (expected / "
469
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
470
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
471 1 }
472 1 void ExactCubicVelocityConstraintsTest(bool& error) {
473 1 T_Waypoint waypoints;
474
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 1 times.
 7 for (double i = 0; i <= 1; i = i + 0.2) {
475
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 waypoints.push_back(std::make_pair(i, point_t(i, i, i)));
476 }
477 std::string errmsg(
478 "Error in ExactCubicVelocityConstraintsTest (1); while checking that "
479 "given wayPoints are crossed (expected / "
480
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
481
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 spline_constraints_t constraints;
482
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraints.end_vel = point_t(0, 0, 0);
483
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraints.init_vel = point_t(0, 0, 0);
484
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraints.end_acc = point_t(0, 0, 0);
485
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraints.init_acc = point_t(0, 0, 0);
486
487
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Spline_t exactCubic;
488
489
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exactCubic.createSplineFromWayPointsConstr(waypoints.begin(), waypoints.end(),
490 constraints);
491 // now check that init and end velocity are 0
492
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
493 std::string errmsg3(
494 "Error in ExactCubicVelocityConstraintsTest (2); while checking "
495
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "derivative (expected / obtained)");
496 // now check derivatives
497
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckDerivative(errmsg3, 0, 1, constraints.init_vel, &exactCubic, error);
498
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckDerivative(errmsg3, 1, 1, constraints.end_vel, &exactCubic, error);
499
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckDerivative(errmsg3, 0, 2, constraints.init_acc, &exactCubic, error);
500
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckDerivative(errmsg3, 1, 2, constraints.end_acc, &exactCubic, error);
501
502
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraints.end_vel = point_t(1, 2, 3);
503
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraints.init_vel = point_t(-1, -2, -3);
504
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraints.end_acc = point_t(4, 5, 6);
505
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 constraints.init_acc = point_t(-4, -4, -6);
506 std::string errmsg2(
507 "Error in ExactCubicVelocityConstraintsTest (3); while checking that "
508 "given wayPoints are crossed (expected / "
509
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "obtained)");
510
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Spline_t exactCubic2;
511
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 exactCubic2.createSplineFromWayPointsConstr(waypoints.begin(),
512 waypoints.end(), constraints);
513
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
514
515 std::string errmsg4(
516 "Error in ExactCubicVelocityConstraintsTest (4); while checking "
517
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 "derivative (expected / obtained)");
518 // now check derivatives
519
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckDerivative(errmsg4, 0, 1, constraints.init_vel, &exactCubic2, error);
520
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckDerivative(errmsg4, 1, 1, constraints.end_vel, &exactCubic2, error);
521
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckDerivative(errmsg4, 0, 2, constraints.init_acc, &exactCubic2, error);
522
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CheckDerivative(errmsg4, 1, 2, constraints.end_acc, &exactCubic2, error);
523 1 }
524 ✗ void CheckPointOnline(const std::string& errmsg, const point_t& A,
525 const point_t& B, const double target,
526 const Spline_t* curve, bool& error) {
527 ✗ point_t res1 = curve->operator()(target);
528 ✗ point_t ar = (res1 - A);
529 ✗ ar.normalize();
530 ✗ point_t rb = (B - res1);
531 ✗ rb.normalize();
532 ✗ if (ar.dot(rb) < 0.99999) {
533 ✗ error = true;
534 ✗ std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose()
535 ✗ << "\n ; " << target << " ; " << res1.transpose() << std::endl;
536 }
537 }
538 /*
539 void EffectorTrajectoryTest(bool& error)
540 {
541 // create arbitrary trajectory
542 T_Waypoint waypoints;
543 for(double i = 0; i <= 10; i = i + 2)
544 {
545 waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
546 }
547 helpers::Spline_t* eff_traj =
548 helpers::effector_spline(waypoints.begin(),waypoints.end(),
549 Eigen::Vector3d::UnitZ(),Eigen::Vector3d(0,0,2),
550 1,0.02,1,0.5);
551 point_t zero(0,0,0);
552 point_t off1(0,0,1);
553 point_t off2(10,10,10.02);
554 point_t end(10,10,10);
555 std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints
556 (expected / obtained)"); std::string errmsg2("Error in EffectorTrajectoryTest;
557 while checking derivative (expected / obtained)");
558 //first check start / goal positions
559 ComparePoints(zero, (*eff_traj)(0), errmsg, error);
560 ComparePoints(off1, (*eff_traj)(1), errmsg, error);
561 ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
562 ComparePoints(end , (*eff_traj)(10), errmsg, error);
563
564 //then check offset at start / goal positions
565 // now check derivatives
566 CheckDerivative(errmsg2,0,1,zero,eff_traj, error);
567 CheckDerivative(errmsg2,10,1,zero ,eff_traj, error);
568 CheckDerivative(errmsg2,0,2,zero,eff_traj, error);
569 CheckDerivative(errmsg2,10,2,zero ,eff_traj, error);
570
571 //check that end and init splines are line
572 std::string errmsg3("Error in EffectorTrajectoryTest; while checking that
573 init/end splines are line (point A/ point B, time value / point obtained)
574 \n"); for(double i = 0.1; i<1; i+=0.1)
575 {
576 CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
577 }
578
579 for(double i = 9.981; i<10; i+=0.002)
580 {
581 CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
582 }
583 delete eff_traj;
584 }
585
586 helpers::quat_t GetXRotQuat(const double theta)
587 {
588 Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX());
589 return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
590 }
591
592 double GetXRotFromQuat(helpers::quat_ref_const_t q)
593 {
594 Eigen::Quaterniond quat (q.data());
595 Eigen::AngleAxisd m (quat);
596 return m.angle() / M_PI * 180.;
597 }
598
599 void EffectorSplineRotationNoRotationTest(bool& error)
600 {
601 // create arbitrary trajectory
602 T_Waypoint waypoints;
603 for(double i = 0; i <= 10; i = i + 2)
604 {
605 waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
606 }
607 helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end());
608 helpers::config_t q_init; q_init << 0.,0.,0.,0.,0.,0.,1.;
609 helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
610 helpers::config_t q_to; q_to << 0.,0,0.02,0.,0.,0.,1.;
611 helpers::config_t q_land; q_land << 10,10, 10.02, 0, 0.,0.,1.;
612 helpers::config_t q_mod; q_mod << 6.,6.,6.,0.,0.,0.,1.;
613 std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while
614 checking waypoints (expected / obtained)"); ComparePoints(q_init ,
615 eff_traj(0), errmsg,error); ComparePoints(q_to , eff_traj(0.02),
616 errmsg,error); ComparePoints(q_land , eff_traj(9.98), errmsg,error);
617 ComparePoints(q_mod , eff_traj(6), errmsg,error);
618 ComparePoints(q_end , eff_traj(10), errmsg,error);
619 }
620
621 void EffectorSplineRotationRotationTest(bool& error)
622 {
623 // create arbitrary trajectory
624 T_Waypoint waypoints;
625 for(double i = 0; i <= 10; i = i + 2)
626 {
627 waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
628 }
629 helpers::quat_t init_quat = GetXRotQuat(M_PI);
630 helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(),
631 init_quat); helpers::config_t q_init = helpers::config_t::Zero();
632 q_init.tail<4>() = init_quat; helpers::config_t q_end; q_end
633 << 10.,10.,10.,0.,0.,0.,1.; helpers::config_t q_to = q_init; q_to(2) +=0.02;
634 helpers::config_t q_land = q_end ; q_land(2)+=0.02;
635 helpers::quat_t q_mod = GetXRotQuat(M_PI_2);;
636 std::string errmsg("Error in EffectorSplineRotationRotationTest; while
637 checking waypoints (expected / obtained)"); ComparePoints(q_init, eff_traj(0),
638 errmsg,error); ComparePoints(q_to , eff_traj(0.02), errmsg,error);
639 ComparePoints(q_land, eff_traj(9.98), errmsg,error);
640 ComparePoints(q_mod , eff_traj(5).tail<4>(), errmsg,error);
641 ComparePoints(q_end , eff_traj(10), errmsg,error);
642 }
643
644 void EffectorSplineRotationWayPointRotationTest(bool& error)
645 {
646 // create arbitrary trajectory
647 T_Waypoint waypoints;
648 for(double i = 0; i <= 10; i = i + 2)
649 {
650 waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
651 }
652 helpers::quat_t init_quat = GetXRotQuat(0);
653 helpers::t_waypoint_quat_t quat_waypoints_;
654
655
656 helpers::quat_t q_pi_0 = GetXRotQuat(0);
657 helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
658 helpers::quat_t q_pi = GetXRotQuat(M_PI);
659
660 quat_waypoints_.push_back(std::make_pair(0.4,q_pi_0));
661 quat_waypoints_.push_back(std::make_pair(6,q_pi_2));
662 quat_waypoints_.push_back(std::make_pair(8,q_pi));
663
664
665 helpers::effector_spline_rotation eff_traj(waypoints.begin(),waypoints.end(),
666 quat_waypoints_.begin(), quat_waypoints_.end());
667 helpers::config_t q_init = helpers::config_t::Zero(); q_init.tail<4>() =
668 init_quat; helpers::config_t q_end; q_end << 10.,10.,10.,0.,0.,0.,1.;
669 q_end.tail<4>() = q_pi; helpers::config_t q_mod; q_mod.head<3>() =
670 point_t(6,6,6) ; q_mod.tail<4>() = q_pi_2; helpers::config_t q_to = q_init;
671 q_to(2) +=0.02; helpers::config_t q_land = q_end ; q_land(2)+=0.02;
672 std::string errmsg("Error in EffectorSplineRotationWayPointRotationTest; while
673 checking waypoints (expected / obtained)"); ComparePoints(q_init, eff_traj(0),
674 errmsg,error); ComparePoints(q_to , eff_traj(0.02), errmsg,error);
675 ComparePoints(q_land, eff_traj(9.98), errmsg,error);
676 ComparePoints(q_mod , eff_traj(6), errmsg,error); ComparePoints(q_end ,
677 eff_traj(10), errmsg,error);
678 }
679
680 void TestReparametrization(bool& error)
681 {
682 helpers::rotation_spline s;
683 const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_;
684 if(sp.min() != 0)
685 {
686 std::cout << "in TestReparametrization; min value is not 0, got " << sp.min()
687 << std::endl; error = true;
688 }
689 if(sp.max() != 1)
690 {
691 std::cout << "in TestReparametrization; max value is not 1, got " << sp.max()
692 << std::endl; error = true;
693 }
694 if(sp(1)[0] != 1.)
695 {
696 std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0]
697 << std::endl; error = true;
698 }
699 if(sp(0)[0] != 0.)
700 {
701 std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0]
702 << std::endl; error = true;
703 }
704 for(double i =0; i<1; i+=0.002)
705 {
706 if(sp(i)[0]>sp(i+0.002)[0])
707 {
708 std::cout << "in TestReparametrization; reparametrization not monotonous " <<
709 sp.max() << std::endl; error = true;
710 }
711 }
712 }
713 */
714 1 int main(int /*argc*/, char** /*argv[]*/) {
715
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "performing tests... \n";
716 1 bool error = false;
717
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 CubicFunctionTest(error);
718
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicNoErrorTest(error);
719
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicPointsCrossedTest(
720 error); // checks that given wayPoints are crossed
721
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicTwoPointsTest(error);
722
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicOneDimTest(error);
723
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ExactCubicVelocityConstraintsTest(error);
724 // EffectorTrajectoryTest(error);
725 // EffectorSplineRotationNoRotationTest(error);
726 // EffectorSplineRotationRotationTest(error);
727 // TestReparametrization(error);
728 // EffectorSplineRotationWayPointRotationTest(error);
729 // BezierCurveTest(error);
730 // BezierDerivativeCurveTest(error);
731 // BezierDerivativeCurveConstraintTest(error);
732 // BezierCurveTestCompareHornerAndBernstein(error);
733
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (error) {
734 ✗ std::cout << "There were some errors\n";
735 ✗ return -1;
736 } else {
737
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::cout << "no errors found \n";
738 1 return 0;
739 }
740 }
741