Main.cpp 107 KB
Newer Older
1
#include "curves/fwd.h"
2
3
#include "curves/exact_cubic.h"
#include "curves/bezier_curve.h"
4
#include "curves/polynomial.h"
5
6
#include "curves/helpers/effector_spline.h"
#include "curves/helpers/effector_spline_rotation.h"
7
#include "curves/curve_conversion.h"
8
#include "curves/cubic_hermite_spline.h"
9
#include "curves/piecewise_curve.h"
10
11
#include "curves/optimization/definitions.h"
#include "load_problem.h"
12
#include "curves/so3_linear.h"
13
#include "curves/se3_curve.h"
14
15
16
#include <string>
#include <iostream>
#include <cmath>
17
#include <ctime>
18
#include <boost/smart_ptr/shared_ptr.hpp>
19
20
21

using namespace std;

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
22
23
namespace curves {
typedef exact_cubic<double, double, true, Eigen::Matrix<double, 1, 1> > exact_cubic_one;
Guilhem Saurel's avatar
Guilhem Saurel committed
24
typedef exact_cubic_t::spline_constraints spline_constraints_t;
25

Guilhem Saurel's avatar
Guilhem Saurel committed
26
27
typedef std::pair<double, pointX_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
28
typedef Eigen::Matrix<double, 1, 1> point_one;
Guilhem Saurel's avatar
Guilhem Saurel committed
29
30
31
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
typedef std::pair<pointX_t, pointX_t> pair_point_tangent_t;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
32
typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
33

Guilhem Saurel's avatar
Guilhem Saurel committed
34
const double margin = 1e-3;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
35
bool QuasiEqual(const double a, const double b) { return std::fabs(a - b) < margin; }
36
bool QuasiEqual(const point3_t a, const point3_t b) {
Guilhem Saurel's avatar
Guilhem Saurel committed
37
  bool equal = true;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
38
39
  for (size_t i = 0; i < 3; ++i) {
    equal = equal && QuasiEqual(a[i], b[i]);
Guilhem Saurel's avatar
Guilhem Saurel committed
40
41
42
  }
  return equal;
}
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
43
}  // End namespace curves
44

45
using namespace curves;
46

47
ostream& operator<<(ostream& os, const point3_t& pt) {
48
49
  os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
  return os;
50
51
}

52
void ComparePoints(const transform_t& pt1, const transform_t& pt2, const std::string& errmsg, bool& error,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
53
54
                   double prec = Eigen::NumTraits<double>::dummy_precision(), bool notequal = false) {
  if (!pt1.isApprox(pt2, prec) && !notequal) {
55
    error = true;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
56
57
    std::cout << errmsg << " translation :" << pt1.translation() << " ; " << pt2.translation() << std::endl
              << "rotation : " << pt1.rotation() << " ; " << pt2.rotation() << std::endl;
58
59
  }
}
60
61

void ComparePoints(const Eigen::MatrixXd& pt1, const Eigen::MatrixXd& pt2, const std::string& errmsg, bool& error,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
62
63
                   double prec = Eigen::NumTraits<double>::dummy_precision(), bool notequal = false) {
  if (!pt1.isApprox(pt2, prec) && !(pt1.isZero(prec) && pt2.isZero(prec)) && !notequal) {
64
    error = true;
65
    std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
66
  }
67
68
}

Guilhem Saurel's avatar
Guilhem Saurel committed
69
template <typename curve1, typename curve2>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
70
71
void CompareCurves(const curve1& c1, const curve2& c2, const std::string& errMsg, bool& error,
                   double prec = Eigen::NumTraits<double>::dummy_precision()) {
72
73
  double T_min = c1.min();
  double T_max = c1.max();
Guilhem Saurel's avatar
Guilhem Saurel committed
74
  if (!QuasiEqual(T_min, c2.min()) || !QuasiEqual(T_max, c2.max())) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
75
76
    std::cout << errMsg << "CompareCurves, ERROR, time min and max of curves do not match [" << T_min << "," << T_max
              << "] "
Guilhem Saurel's avatar
Guilhem Saurel committed
77
              << " and [" << c2.min() << "," << c2.max() << "] " << std::endl;
78
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
79
  } else {
80
    // derivative in T_min and T_max
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
81
82
83
84
    ComparePoints(c1.derivate(T_min, 1), c2.derivate(T_min, 1), errMsg + " Derivates at tMin do not match.", error,
                  prec, false);
    ComparePoints(c1.derivate(T_max, 1), c2.derivate(T_max, 1), errMsg + " Derivates at tMax do not match.", error,
                  prec, false);
85
    // Test values on curves
86
    for (double i = T_min; i <= T_max; i += 0.01) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
87
88
89
      ComparePoints(c1(i), c2(i),
                    errMsg + " Curves evaluation do not match at t = " + boost::lexical_cast<std::string>(i), error,
                    prec, false);
90
91
    }
  }
92
93
}

94
/*Cubic Function tests*/
Guilhem Saurel's avatar
Guilhem Saurel committed
95
void PolynomialCubicFunctionTest(bool& error) {
96
  std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
97
98
99
100
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 6, 7);
101
  t_pointX_t vec;
102
103
104
105
106
  vec.push_back(a);
  vec.push_back(b);
  vec.push_back(c);
  vec.push_back(d);
  polynomial_t cf(vec.begin(), vec.end(), 0, 1);
107
  point3_t res1;
Guilhem Saurel's avatar
Guilhem Saurel committed
108
  res1 = cf(0);
109
  point3_t x0(1, 2, 3);
110
  ComparePoints(x0, res1, errMsg + "(0) ", error);
111
  point3_t x1(9, 15, 19);
Guilhem Saurel's avatar
Guilhem Saurel committed
112
  res1 = cf(1);
113
  ComparePoints(x1, res1, errMsg + "(1) ", error);
114
  point3_t x2(3.125, 5.25, 7.125);
Guilhem Saurel's avatar
Guilhem Saurel committed
115
  res1 = cf(0.5);
116
117
118
119
120
121
122
123
124
125
  ComparePoints(x2, res1, errMsg + "(0.5) ", error);
  vec.clear();
  vec.push_back(a);
  vec.push_back(b);
  vec.push_back(c);
  vec.push_back(d);
  polynomial_t cf2(vec, 0.5, 1);
  res1 = cf2(0.5);
  ComparePoints(x0, res1, errMsg + "x3 ", error);
  error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
126
  try {
127
    cf2(0.4);
Guilhem Saurel's avatar
Guilhem Saurel committed
128
  } catch (...) {
129
130
    error = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
131
  if (error) {
132
133
134
    std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
  }
  error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
135
  try {
136
    cf2(1.1);
Guilhem Saurel's avatar
Guilhem Saurel committed
137
  } catch (...) {
138
139
    error = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
140
  if (error) {
141
142
    std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
143
  if (!QuasiEqual(cf.max(), 1.0)) {
144
    error = true;
145
146
    std::cout << "Evaluation of cubic cf error, MaxBound should be equal to 1\n";
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
147
  if (!QuasiEqual(cf.min(), 0.0)) {
148
    error = true;
149
150
    std::cout << "Evaluation of cubic cf error, MinBound should be equal to 1\n";
  }
151
152
  // Test derivate and compute_derivative
  // Order 1
153
  curve_abc_t* cf_derivated = cf.compute_derivate_ptr(1);
154
155
156
157
  ComparePoints(cf.derivate(0, 1), (*cf_derivated)(0), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(0.3, 1), (*cf_derivated)(0.3), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(0.5, 1), (*cf_derivated)(0.5), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(1, 1), (*cf_derivated)(1), errMsg + " - derivate order 1 : ", error);
158
159
  // Order 2
  polynomial_t cf_derivated_2 = cf.compute_derivate(2);
160
161
162
163
  ComparePoints(cf.derivate(0, 2), (cf_derivated_2)(0), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(0.3, 2), (cf_derivated_2)(0.3), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(0.5, 2), (cf_derivated_2)(0.5), errMsg + " - derivate order 1 : ", error);
  ComparePoints(cf.derivate(1, 2), (cf_derivated_2)(1), errMsg + " - derivate order 1 : ", error);
164
165
166
}

/*bezier_curve Function tests*/
Guilhem Saurel's avatar
Guilhem Saurel committed
167
void BezierCurveTest(bool& error) {
168
  std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
169
170
171
172
173
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 6, 7);
  std::vector<point3_t> params;
174
175
  params.push_back(a);
  // 1d curve in [0,1]
176
  bezier_t cf1(params.begin(), params.end());
177
  point3_t res1;
178
  res1 = cf1(0);
179
  point3_t x10 = a;
180
  ComparePoints(x10, res1, errMsg + "1(0) ", error);
Guilhem Saurel's avatar
Guilhem Saurel committed
181
  res1 = cf1(1);
182
183
184
  ComparePoints(x10, res1, errMsg + "1(1) ", error);
  // 2d curve in [0,1]
  params.push_back(b);
185
  bezier_t cf(params.begin(), params.end());
186
  res1 = cf(0);
187
  point3_t x20 = a;
188
  ComparePoints(x20, res1, errMsg + "2(0) ", error);
189
  point3_t x21 = b;
190
191
  res1 = cf(1);
  ComparePoints(x21, res1, errMsg + "2(1) ", error);
Guilhem Saurel's avatar
Guilhem Saurel committed
192
  // 3d curve in [0,1]
193
  params.push_back(c);
194
  bezier_t cf3(params.begin(), params.end());
195
196
197
198
  res1 = cf3(0);
  ComparePoints(a, res1, errMsg + "3(0) ", error);
  res1 = cf3(1);
  ComparePoints(c, res1, errMsg + "3(1) ", error);
Guilhem Saurel's avatar
Guilhem Saurel committed
199
  // 4d curve in [1,2]
200
  params.push_back(d);
201
  bezier_t cf4(params.begin(), params.end(), 1., 2.);
Guilhem Saurel's avatar
Guilhem Saurel committed
202
  // testing bernstein polynomials
203
  bezier_t cf5(params.begin(), params.end(), 1., 2.);
204
  std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
205
  bezier_t cf5_derivated = cf5.compute_derivate(1);
206

Guilhem Saurel's avatar
Guilhem Saurel committed
207
208
209
  for (double d = 1.; d < 2.; d += 0.1) {
    ComparePoints(cf5.evalBernstein(d), cf5(d), errMsg2, error);
    ComparePoints(cf5.evalHorner(d), cf5(d), errMsg2, error);
210
211
    ComparePoints(cf5_derivated.evalBernstein(d), cf5_derivated(d), errMsg2, error);
    ComparePoints(cf5_derivated.evalHorner(d), cf5_derivated(d), errMsg2, error);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
212
    ComparePoints(cf5.derivate(d, 1), cf5_derivated(d), errMsg2, error);
213
214
  }
  bool error_in(true);
Guilhem Saurel's avatar
Guilhem Saurel committed
215
  try {
216
    cf(-0.4);
Guilhem Saurel's avatar
Guilhem Saurel committed
217
  } catch (...) {
218
219
    error_in = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
220
  if (error_in) {
221
222
223
224
    std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
    error = true;
  }
  error_in = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
225
  try {
226
    cf(1.1);
Guilhem Saurel's avatar
Guilhem Saurel committed
227
  } catch (...) {
228
229
    error_in = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
230
  if (error_in) {
231
232
233
    std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
    error = true;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
234
  if (!QuasiEqual(cf.max(), 1.0)) {
235
236
237
    error = true;
    std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
238
  if (!QuasiEqual(cf.min(), 0.0)) {
239
240
241
    error = true;
    std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
  }
242
243
}

Guilhem Saurel's avatar
Guilhem Saurel committed
244
void BezierCurveTestCompareHornerAndBernstein(bool&)  // error
245
{
246
247
  using namespace std;
  std::vector<double> values;
Guilhem Saurel's avatar
Guilhem Saurel committed
248
249
250
251
  for (int i = 0; i < 100000; ++i) {
    values.push_back(rand() / RAND_MAX);
  }
  // first compare regular evaluation (low dim pol)
252
253
254
255
256
257
258
259
260
261
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 6, 7);
  point3_t e(3, 61, 7);
  point3_t f(3, 56, 7);
  point3_t g(3, 36, 7);
  point3_t h(43, 6, 7);
  point3_t i(3, 6, 77);
  std::vector<point3_t> params;
262
263
264
265
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
  // 3d curve
266
  bezier_t cf(params.begin(), params.end());  // defined in [0,1]
267
  // Check all evaluation of bezier curve
Guilhem Saurel's avatar
Guilhem Saurel committed
268
  clock_t s0, e0, s1, e1, s2, e2, s3, e3;
269
  s0 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
270
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
271
272
273
274
    cf(*cit);
  }
  e0 = clock();
  s1 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
275
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
276
277
278
279
280
    cf.evalBernstein(*cit);
  }
  e1 = clock();

  s2 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
281
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
282
283
284
285
    cf.evalHorner(*cit);
  }
  e2 = clock();
  s3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
286
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
287
288
289
    cf.evalDeCasteljau(*cit);
  }
  e3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
290
291
292
293
294
  std::cout << "time for analytical eval  " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for bernstein eval   " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for horner eval      " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
  std::cout << "now with high order polynomial " << std::endl;
295
296
297
298
299
300
  params.push_back(d);
  params.push_back(e);
  params.push_back(f);
  params.push_back(g);
  params.push_back(h);
  params.push_back(i);
301
  bezier_t cf2(params.begin(), params.end());
302
  s1 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
303
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
304
305
306
307
    cf2.evalBernstein(*cit);
  }
  e1 = clock();
  s2 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
308
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
309
310
311
312
    cf2.evalHorner(*cit);
  }
  e2 = clock();
  s0 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
313
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
314
315
316
317
    cf2(*cit);
  }
  e0 = clock();
  s3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
318
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
319
320
321
    cf2.evalDeCasteljau(*cit);
  }
  e3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
322
323
324
325
  std::cout << "time for analytical eval  " << double(e0 - s0) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for bernstein eval   " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for horner eval      " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
  std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
326
327
}

Guilhem Saurel's avatar
Guilhem Saurel committed
328
void BezierDerivativeCurveTest(bool& error) {
329
  std::string errMsg("In test BezierDerivativeCurveTest ;, Error While checking value of point on curve : ");
330
331
332
333
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  std::vector<point3_t> params;
334
335
336
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
337
  bezier_t cf3(params.begin(), params.end());
Guilhem Saurel's avatar
Guilhem Saurel committed
338
  ComparePoints(cf3(0), cf3.derivate(0., 1), errMsg, error, true);
339
  ComparePoints(point3_t::Zero(), cf3.derivate(0., 100), errMsg, error);
340
341
}

Guilhem Saurel's avatar
Guilhem Saurel committed
342
343
344
void BezierDerivativeCurveTimeReparametrizationTest(bool& error) {
  std::string errMsg(
      "In test BezierDerivativeCurveTimeReparametrizationTest, Error While checking value of point on curve : ");
345
346
347
348
349
350
351
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 4, 5);
  point3_t e(3, 4, 5);
  point3_t f(3, 4, 5);
  std::vector<point3_t> params;
352
353
354
355
356
357
358
359
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
  params.push_back(d);
  params.push_back(e);
  params.push_back(f);
  double Tmin = 0.;
  double Tmax = 2.;
Guilhem Saurel's avatar
Guilhem Saurel committed
360
  double diffT = Tmax - Tmin;
361
362
  bezier_t cf(params.begin(), params.end());
  bezier_t cfT(params.begin(), params.end(), Tmin, Tmax);
363
  ComparePoints(cf(0.5), cfT(1), errMsg, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
364
365
  ComparePoints(cf.derivate(0.5, 1), cfT.derivate(1, 1) * (diffT), errMsg, error);
  ComparePoints(cf.derivate(0.5, 2), cfT.derivate(1, 2) * diffT * diffT, errMsg, error);
366
367
}

Guilhem Saurel's avatar
Guilhem Saurel committed
368
void BezierDerivativeCurveConstraintTest(bool& error) {
369
  std::string errMsg0("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
370
371
372
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
373
  bezier_t::curve_constraints_t constraints(3);
374
375
376
377
378
  constraints.init_vel = point3_t(-1, -1, -1);
  constraints.init_acc = point3_t(-2, -2, -2);
  constraints.end_vel = point3_t(-10, -10, -10);
  constraints.end_acc = point3_t(-20, -20, -20);
  std::vector<point3_t> params;
379
380
381
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
382
383
384
  bezier_t::num_t T_min = 1.0;
  bezier_t::num_t T_max = 3.0;
  bezier_t cf(params.begin(), params.end(), constraints, T_min, T_max);
385
386
  ComparePoints(a, cf(T_min), errMsg0, error);
  ComparePoints(c, cf(T_max), errMsg0, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
387
388
389
390
391
392
393
394
395
396
  ComparePoints(constraints.init_vel, cf.derivate(T_min, 1), errMsg0, error);
  ComparePoints(constraints.end_vel, cf.derivate(T_max, 1), errMsg0, error);
  ComparePoints(constraints.init_acc, cf.derivate(T_min, 2), errMsg0, error);
  ComparePoints(constraints.end_vel, cf.derivate(T_max, 1), errMsg0, error);
  ComparePoints(constraints.end_acc, cf.derivate(T_max, 2), errMsg0, error);
  std::string errMsg1(
      "In test BezierDerivativeCurveConstraintTest, Error While checking checking degree of bezier curve :");
  std::string errMsg2(
      "In test BezierDerivativeCurveConstraintTest, Error While checking checking size of bezier curve :");
  if (cf.degree_ != params.size() + 3) {
397
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
398
    std::cout << errMsg1 << cf.degree_ << " ; " << params.size() + 3 << std::endl;
399
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
400
  if (cf.size_ != params.size() + 4) {
401
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
402
    std::cout << errMsg2 << cf.size_ << " ; " << params.size() + 4 << std::endl;
403
  }
404
405
}

Guilhem Saurel's avatar
Guilhem Saurel committed
406
void toPolynomialConversionTest(bool& error) {
407
408
  // bezier to polynomial
  std::string errMsg("In test BezierToPolynomialConversionTest, Error While checking value of point on curve : ");
409
410
411
412
413
414
415
416
417
418
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
  point3_t d(3, 6, 7);
  point3_t e(3, 61, 7);
  point3_t f(3, 56, 7);
  point3_t g(3, 36, 7);
  point3_t h(43, 6, 7);
  point3_t i(3, 6, 77);
  std::vector<point3_t> control_points;
419
420
421
422
423
424
425
426
427
  control_points.push_back(a);
  control_points.push_back(b);
  control_points.push_back(c);
  control_points.push_back(d);
  control_points.push_back(e);
  control_points.push_back(f);
  control_points.push_back(g);
  control_points.push_back(h);
  control_points.push_back(i);
428
429
430
  bezier_t::num_t T_min = 1.0;
  bezier_t::num_t T_max = 3.0;
  bezier_t bc(control_points.begin(), control_points.end(), T_min, T_max);
431
  polynomial_t pol = polynomial_from_curve<polynomial_t>(bc);
432
  CompareCurves<polynomial_t, bezier_t>(pol, bc, errMsg, error);
433
434
}

Guilhem Saurel's avatar
Guilhem Saurel committed
435
436
437
438
439
440
441
void cubicConversionTest(bool& error) {
  std::string errMsg0(
      "In test CubicConversionTest - convert hermite to, Error While checking value of point on curve : ");
  std::string errMsg1(
      "In test CubicConversionTest - convert bezier to, Error While checking value of point on curve : ");
  std::string errMsg2(
      "In test CubicConversionTest - convert polynomial to, Error While checking value of point on curve : ");
442
  // Create cubic hermite spline : Test hermite to bezier/polynomial
443
444
445
446
  point3_t p0(1, 2, 3);
  point3_t m0(2, 3, 4);
  point3_t p1(3, 4, 5);
  point3_t m1(3, 6, 7);
Guilhem Saurel's avatar
Guilhem Saurel committed
447
448
  pair_point_tangent_t pair0(p0, m0);
  pair_point_tangent_t pair1(p1, m1);
449
450
451
  t_pair_point_tangent_t control_points;
  control_points.push_back(pair0);
  control_points.push_back(pair1);
Guilhem Saurel's avatar
Guilhem Saurel committed
452
  std::vector<double> time_control_points;
453
454
455
456
457
458
  polynomial_t::num_t T_min = 1.0;
  polynomial_t::num_t T_max = 3.0;
  time_control_points.push_back(T_min);
  time_control_points.push_back(T_max);
  cubic_hermite_spline_t chs0(control_points.begin(), control_points.end(), time_control_points);
  // hermite to bezier
Guilhem Saurel's avatar
Guilhem Saurel committed
459
460
  // std::cout<<"======================= \n";
  // std::cout<<"hermite to bezier \n";
461
462
  bezier_t bc0 = bezier_from_curve<bezier_t>(chs0);
  CompareCurves<cubic_hermite_spline_t, bezier_t>(chs0, bc0, errMsg0, error);
463
  // hermite to pol
Guilhem Saurel's avatar
Guilhem Saurel committed
464
465
  // std::cout<<"======================= \n";
  // std::cout<<"hermite to polynomial \n";
466
  polynomial_t pol0 = polynomial_from_curve<polynomial_t>(chs0);
467
468
  CompareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
  // pol to hermite
Guilhem Saurel's avatar
Guilhem Saurel committed
469
470
  // std::cout<<"======================= \n";
  // std::cout<<"polynomial to hermite \n";
471
  cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t>(pol0);
Guilhem Saurel's avatar
Guilhem Saurel committed
472
  CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol0, chs1, errMsg2, error);
473
  // pol to bezier
Guilhem Saurel's avatar
Guilhem Saurel committed
474
475
  // std::cout<<"======================= \n";
  // std::cout<<"polynomial to bezier \n";
476
477
  bezier_t bc1 = bezier_from_curve<bezier_t>(pol0);
  CompareCurves<bezier_t, polynomial_t>(bc1, pol0, errMsg2, error);
478
  // Bezier to pol
Guilhem Saurel's avatar
Guilhem Saurel committed
479
480
  // std::cout<<"======================= \n";
  // std::cout<<"bezier to polynomial \n";
481
  polynomial_t pol1 = polynomial_from_curve<polynomial_t>(bc0);
482
  CompareCurves<bezier_t, polynomial_t>(bc0, pol1, errMsg1, error);
483
  // bezier => hermite
Guilhem Saurel's avatar
Guilhem Saurel committed
484
485
  // std::cout<<"======================= \n";
  // std::cout<<"bezier to hermite \n";
486
  cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t>(bc0);
487
  CompareCurves<bezier_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
488
489

  // Test : compute derivative of bezier => Convert it to polynomial
490
  curve_abc_t* bc_der = bc0.compute_derivate_ptr(1);
491
492
  polynomial_t pol_test = polynomial_from_curve<polynomial_t>(*bc_der);
  CompareCurves<curve_abc_t, polynomial_t>(*bc_der, pol_test, errMsg1, error);
493
494
}

495
/*Exact Cubic Function tests*/
Guilhem Saurel's avatar
Guilhem Saurel committed
496
void ExactCubicNoErrorTest(bool& error) {
497
498
  // Create an exact cubic spline with 7 waypoints => 6 polynomials defined in [0.0,3.0]
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
499
  for (double i = 0.0; i <= 3.0; i = i + 0.5) {
500
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
501
502
503
504
  }
  exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
  // Test number of polynomials in exact cubic
  std::size_t numberSegments = exactCubic.getNumberSplines();
Guilhem Saurel's avatar
Guilhem Saurel committed
505
  if (numberSegments != 6) {
JasonChmn's avatar
JasonChmn committed
506
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
507
508
    std::cout << "In ExactCubicNoErrorTest, Error While checking number of splines" << numberSegments << " ; " << 6
              << std::endl;
509
510
  }
  // Test getSplineAt function
Guilhem Saurel's avatar
Guilhem Saurel committed
511
  for (std::size_t i = 0; i < numberSegments; i++) {
512
513
514
    exactCubic.getSplineAt(i);
  }
  // Other tests
Guilhem Saurel's avatar
Guilhem Saurel committed
515
  try {
516
517
    exactCubic(0.0);
    exactCubic(3.0);
Guilhem Saurel's avatar
Guilhem Saurel committed
518
  } catch (...) {
519
520
521
522
    error = true;
    std::cout << "Evaluation of ExactCubicNoErrorTest error when testing value on bounds\n";
  }
  error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
523
  try {
524
    exactCubic(3.2);
Guilhem Saurel's avatar
Guilhem Saurel committed
525
  } catch (...) {
526
527
    error = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
528
  if (error) {
529
530
    std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of range value\n";
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
531
  if (!QuasiEqual(exactCubic.max(), 3.0)) {
532
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
533
    std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 3 but is : " << exactCubic.max() << "\n";
534
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
535
  if (!QuasiEqual(exactCubic.min(), 0.0)) {
536
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
537
    std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0 but is : " << exactCubic.min() << "\n";
538
  }
539
540
541
}

/*Exact Cubic Function tests*/
Guilhem Saurel's avatar
Guilhem Saurel committed
542
void ExactCubicTwoPointsTest(bool& error) {
543
544
  // Create an exact cubic spline with 2 waypoints => 1 polynomial defined in [0.0,1.0]
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
545
  for (double i = 0.0; i < 2.0; ++i) {
546
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
547
548
  }
  exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
549
  point3_t res1 = exactCubic(0);
Guilhem Saurel's avatar
Guilhem Saurel committed
550
551
  std::string errmsg0(
      "in ExactCubicTwoPointsTest, Error While checking that given wayPoints  are crossed (expected / obtained)");
552
  ComparePoints(point3_t(0, 0, 0), res1, errmsg0, error);
553
  res1 = exactCubic(1);
554
  ComparePoints(point3_t(1, 1, 1), res1, errmsg0, error);
555
556
  // Test number of polynomials in exact cubic
  std::size_t numberSegments = exactCubic.getNumberSplines();
Guilhem Saurel's avatar
Guilhem Saurel committed
557
  if (numberSegments != 1) {
558
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
559
560
    std::cout << "In ExactCubicTwoPointsTest, Error While checking number of splines" << numberSegments << " ; " << 1
              << std::endl;
561
562
563
564
  }
  // Test getSplineAt
  std::string errmsg1("in ExactCubicTwoPointsTest, Error While checking value on curve");
  ComparePoints(exactCubic(0.5), (exactCubic.getSplineAt(0))(0.5), errmsg1, error);
565
566
}

Guilhem Saurel's avatar
Guilhem Saurel committed
567
void ExactCubicOneDimTest(bool& error) {
568
  curves::T_WaypointOne waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
569
570
571
572
573
574
  point_one zero;
  zero(0, 0) = 9;
  point_one one;
  one(0, 0) = 14;
  point_one two;
  two(0, 0) = 25;
575
576
577
578
579
  waypoints.push_back(std::make_pair(0., zero));
  waypoints.push_back(std::make_pair(1., one));
  waypoints.push_back(std::make_pair(2., two));
  exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
  point_one res1 = exactCubic(0);
Guilhem Saurel's avatar
Guilhem Saurel committed
580
581
  std::string errmsg(
      "in ExactCubicOneDim Error While checking that given wayPoints  are crossed (expected / obtained)");
582
583
584
  ComparePoints(zero, res1, errmsg, error);
  res1 = exactCubic(1);
  ComparePoints(one, res1, errmsg, error);
585
586
}

Guilhem Saurel's avatar
Guilhem Saurel committed
587
void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint&,
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
588
589
                             const exact_cubic_t* curve, bool& error,
                             double prec = Eigen::NumTraits<double>::dummy_precision()) {
590
  point3_t res1;
Guilhem Saurel's avatar
Guilhem Saurel committed
591
  for (double i = 0; i <= 1; i = i + step) {
592
    res1 = (*curve)(i);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
593
    ComparePoints(point3_t(i, i, i), res1, errmsg, error, prec);
594
  }
595
596
}

Guilhem Saurel's avatar
Guilhem Saurel committed
597
void ExactCubicPointsCrossedTest(bool& error) {
598
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
599
  for (double i = 0; i <= 1; i = i + 0.2) {
600
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
601
602
603
604
  }
  exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
  std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
  CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
605
606
}

Guilhem Saurel's avatar
Guilhem Saurel committed
607
void ExactCubicVelocityConstraintsTest(bool& error) {
608
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
609
  for (double i = 0; i <= 1; i = i + 0.2) {
610
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
611
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
612
613
614
  std::string errmsg(
      "Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / "
      "obtained)");
stevet's avatar
stevet committed
615
  spline_constraints_t constraints(3);
616
617
618
619
  constraints.end_vel = point3_t(0, 0, 0);
  constraints.init_vel = point3_t(0, 0, 0);
  constraints.end_acc = point3_t(0, 0, 0);
  constraints.init_acc = point3_t(0, 0, 0);
620
621
622
  exact_cubic_t exactCubic(waypoints.begin(), waypoints.end(), constraints);
  // now check that init and end velocity are 0
  CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
623
624
  std::string errmsg3(
      "Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
625
  // now check derivatives
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
626
627
628
629
  ComparePoints(constraints.init_vel, exactCubic.derivate(0, 1), errmsg3, error, 1e-10);
  ComparePoints(constraints.end_vel, exactCubic.derivate(1, 1), errmsg3, error, 1e-10);
  ComparePoints(constraints.init_acc, exactCubic.derivate(0, 2), errmsg3, error, 1e-10);
  ComparePoints(constraints.end_acc, exactCubic.derivate(1, 2), errmsg3, error, 1e-10);
630
631
632
633
  constraints.end_vel = point3_t(1, 2, 3);
  constraints.init_vel = point3_t(-1, -2, -3);
  constraints.end_acc = point3_t(4, 5, 6);
  constraints.init_acc = point3_t(-4, -4, -6);
Guilhem Saurel's avatar
Guilhem Saurel committed
634
635
636
637
  std::string errmsg2(
      "Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / "
      "obtained)");
  exact_cubic_t exactCubic2(waypoints.begin(), waypoints.end(), constraints);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
638
  CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error, 1e-10);
Guilhem Saurel's avatar
Guilhem Saurel committed
639
640
  std::string errmsg4(
      "Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
641
  // now check derivatives
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
642
643
644
645
  ComparePoints(constraints.init_vel, exactCubic2.derivate(0, 1), errmsg4, error, 1e-10);
  ComparePoints(constraints.end_vel, exactCubic2.derivate(1, 1), errmsg4, error, 1e-10);
  ComparePoints(constraints.init_acc, exactCubic2.derivate(0, 2), errmsg4, error, 1e-10);
  ComparePoints(constraints.end_acc, exactCubic2.derivate(1, 2), errmsg4, error, 1e-10);
646
647
}

Guilhem Saurel's avatar
Guilhem Saurel committed
648
template <typename CurveType>
649
void CheckPointOnline(const std::string& errmsg, const point3_t& A, const point3_t& B, const double target,
Guilhem Saurel's avatar
Guilhem Saurel committed
650
                      const CurveType* curve, bool& error) {
651
652
  point3_t res1 = curve->operator()(target);
  point3_t ar = (res1 - A);
Guilhem Saurel's avatar
Guilhem Saurel committed
653
  ar.normalize();
654
  point3_t rb = (B - res1);
Guilhem Saurel's avatar
Guilhem Saurel committed
655
656
  rb.normalize();
  if (ar.dot(rb) < 0.99999) {
657
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
658
659
    std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " << target << " ; "
              << res1.transpose() << std::endl;
660
  }
661
662
}

Guilhem Saurel's avatar
Guilhem Saurel committed
663
void EffectorTrajectoryTest(bool& error) {
664
665
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
666
  for (double i = 0; i <= 10; i = i + 2) {
667
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
668
669
670
  }
  helpers::exact_cubic_t* eff_traj = helpers::effector_spline(
      waypoints.begin(), waypoints.end(), Eigen::Vector3d::UnitZ(), Eigen::Vector3d(0, 0, 2), 1, 0.02, 1, 0.5);
671
672
673
674
  point3_t zero(0, 0, 0);
  point3_t off1(0, 0, 1);
  point3_t off2(10, 10, 10.02);
  point3_t end(10, 10, 10);
675
676
  std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)");
  std::string errmsg2("Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)");
Guilhem Saurel's avatar
Guilhem Saurel committed
677
  // first check start / goal positions
678
679
680
  ComparePoints(zero, (*eff_traj)(0), errmsg, error);
  ComparePoints(off1, (*eff_traj)(1), errmsg, error);
  ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
681
  ComparePoints(end, (*eff_traj)(10), errmsg, error);
682
  // now check derivatives
Guilhem Saurel's avatar
Guilhem Saurel committed
683
684
685
686
687
688
689
690
691
692
693
694
695
  ComparePoints(zero, (*eff_traj).derivate(0, 1), errmsg2, error);
  ComparePoints(zero, (*eff_traj).derivate(10, 1), errmsg2, error);
  ComparePoints(zero, (*eff_traj).derivate(0, 2), errmsg2, error);
  ComparePoints(zero, (*eff_traj).derivate(10, 2), errmsg2, error);
  // check that end and init splines are line
  std::string errmsg3(
      "Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / "
      "point obtained) \n");
  for (double i = 0.1; i < 1; i += 0.1) {
    CheckPointOnline<helpers::exact_cubic_t>(errmsg3, (*eff_traj)(0), (*eff_traj)(1), i, eff_traj, error);
  }
  for (double i = 9.981; i < 10; i += 0.002) {
    CheckPointOnline<helpers::exact_cubic_t>(errmsg3, (*eff_traj)(9.5), (*eff_traj)(10), i, eff_traj, error);
696
697
  }
  delete eff_traj;
698
699
}

Guilhem Saurel's avatar
Guilhem Saurel committed
700
701
helpers::quat_t GetXRotQuat(const double theta) {
  Eigen::AngleAxisd m(theta, Eigen::Vector3d::UnitX());
702
  return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
703
704
}

Guilhem Saurel's avatar
Guilhem Saurel committed
705
706
707
double GetXRotFromQuat(helpers::quat_ref_const_t q) {
  Eigen::Quaterniond quat(q.data());
  Eigen::AngleAxisd m(quat);
708
  return m.angle() / M_PI * 180.;
709
710
}

Guilhem Saurel's avatar
Guilhem Saurel committed
711
void EffectorSplineRotationNoRotationTest(bool& error) {
712
713
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
714
  for (double i = 0; i <= 10; i = i + 2) {
715
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
716
717
718
719
720
721
722
723
724
725
726
727
  }
  helpers::effector_spline_rotation eff_traj(waypoints.begin(), waypoints.end());
  helpers::config_t q_init;
  q_init << 0., 0., 0., 0., 0., 0., 1.;
  helpers::config_t q_end;
  q_end << 10., 10., 10., 0., 0., 0., 1.;
  helpers::config_t q_to;
  q_to << 0., 0, 0.02, 0., 0., 0., 1.;
  helpers::config_t q_land;
  q_land << 10, 10, 10.02, 0, 0., 0., 1.;
  helpers::config_t q_mod;
  q_mod << 6., 6., 6., 0., 0., 0., 1.;
728
  std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
Guilhem Saurel's avatar
Guilhem Saurel committed
729
730
731
732
733
  ComparePoints(q_init, eff_traj(0), errmsg, error);
  ComparePoints(q_to, eff_traj(0.02), errmsg, error);
  ComparePoints(q_land, eff_traj(9.98), errmsg, error);
  ComparePoints(q_mod, eff_traj(6), errmsg, error);
  ComparePoints(q_end, eff_traj(10), errmsg, error);
734
735
}

Guilhem Saurel's avatar
Guilhem Saurel committed
736
void EffectorSplineRotationRotationTest(bool& error) {
737
738
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
739
  for (double i = 0; i <= 10; i = i + 2) {
740
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
741
742
  }
  helpers::quat_t init_quat = GetXRotQuat(M_PI);
Guilhem Saurel's avatar
Guilhem Saurel committed
743
744
745
746
747
748
749
750
751
752
753
  helpers::effector_spline_rotation eff_traj(waypoints.begin(), waypoints.end(), init_quat);
  helpers::config_t q_init = helpers::config_t::Zero();
  q_init.tail<4>() = init_quat;
  helpers::config_t q_end;
  q_end << 10., 10., 10., 0., 0., 0., 1.;
  helpers::config_t q_to = q_init;
  q_to(2) += 0.02;
  helpers::config_t q_land = q_end;
  q_land(2) += 0.02;
  helpers::quat_t q_mod = GetXRotQuat(M_PI_2);
  ;
754
  std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
Guilhem Saurel's avatar
Guilhem Saurel committed
755
756
757
758
759
  ComparePoints(q_init, eff_traj(0), errmsg, error);
  ComparePoints(q_to, eff_traj(0.02), errmsg, error);
  ComparePoints(q_land, eff_traj(9.98), errmsg, error);
  ComparePoints(q_mod, eff_traj(5).tail<4>(), errmsg, error);
  ComparePoints(q_end, eff_traj(10), errmsg, error);
760
761
}

Guilhem Saurel's avatar
Guilhem Saurel committed
762
void EffectorSplineRotationWayPointRotationTest(bool& error) {
763
764
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
765
  for (double i = 0; i <= 10; i = i + 2) {
766
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
767
768
769
770
771
  }
  helpers::quat_t init_quat = GetXRotQuat(0);
  helpers::t_waypoint_quat_t quat_waypoints_;
  helpers::quat_t q_pi_0 = GetXRotQuat(0);
  helpers::quat_t q_pi_2 = GetXRotQuat(M_PI_2);
Guilhem Saurel's avatar
Guilhem Saurel committed
772
773
774
775
776
777
778
779
780
781
782
783
  helpers::quat_t q_pi = GetXRotQuat(M_PI);
  quat_waypoints_.push_back(std::make_pair(0.4, q_pi_0));
  quat_waypoints_.push_back(std::make_pair(6, q_pi_2));
  quat_waypoints_.push_back(std::make_pair(8, q_pi));
  helpers::effector_spline_rotation eff_traj(waypoints.begin(), waypoints.end(), quat_waypoints_.begin(),
                                             quat_waypoints_.end());
  helpers::config_t q_init = helpers::config_t::Zero();
  q_init.tail<4>() = init_quat;
  helpers::config_t q_end;
  q_end << 10., 10., 10., 0., 0., 0., 1.;
  q_end.tail<4>() = q_pi;
  helpers::config_t q_mod;
784
  q_mod.head<3>() = point3_t(6, 6, 6);
Guilhem Saurel's avatar
Guilhem Saurel committed
785
786
787
788
789
790
791
792
793
794
795
796
  q_mod.tail<4>() = q_pi_2;
  helpers::config_t q_to = q_init;
  q_to(2) += 0.02;
  helpers::config_t q_land = q_end;
  q_land(2) += 0.02;
  std::string errmsg(
      "Error in EffectorSplineRotationWayPointRotationTest; while checking waypoints (expected / obtained)");
  ComparePoints(q_init, eff_traj(0), errmsg, error);
  ComparePoints(q_to, eff_traj(0.02), errmsg, error);
  ComparePoints(q_land, eff_traj(9.98), errmsg, error);
  ComparePoints(q_mod, eff_traj(6), errmsg, error);
  ComparePoints(q_end, eff_traj(10), errmsg, error);
797
798
}

Guilhem Saurel's avatar
Guilhem Saurel committed
799
void TestReparametrization(bool& error) {
800
801
  helpers::rotation_spline s;
  const helpers::exact_cubic_constraint_one_dim& sp = s.time_reparam_;
Guilhem Saurel's avatar
Guilhem Saurel committed
802
  if (!QuasiEqual(sp.min(), 0.0)) {
803
804
805
    std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
    error = true;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
806
  if (!QuasiEqual(sp.max(), 1.0)) {
807
808
809
    std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
    error = true;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
810
  if (!QuasiEqual(sp(1)[0], 1.0)) {
811
812
813
    std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
    error = true;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
814
  if (!QuasiEqual(sp(0)[0], 0.0)) {
815
816
817
    std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
    error = true;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
818
819
  for (double i = 0; i < 1; i += 0.002) {
    if (sp(i)[0] > sp(i + 0.002)[0]) {
820
821
      std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
      error = true;
822
    }
823
  }
824
825
}

826
827
point3_t randomPoint(const double min, const double max) {
  point3_t p;
Guilhem Saurel's avatar
Guilhem Saurel committed
828
829
  for (size_t i = 0; i < 3; ++i) {
    p[i] = (rand() / (double)RAND_MAX) * (max - min) + min;
830
831
  }
  return p;
832
833
}

Guilhem Saurel's avatar
Guilhem Saurel committed
834
void BezierEvalDeCasteljau(bool& error) {
835
836
  using namespace std;
  std::vector<double> values;
Guilhem Saurel's avatar
Guilhem Saurel committed
837
838
839
840
  for (int i = 0; i < 100000; ++i) {
    values.push_back(rand() / RAND_MAX);
  }
  // first compare regular evaluation (low dim pol)
841
842
843