Main.cpp 103 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

35

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

47
using namespace curves;
48

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

54
55
56
57
58
59
60
void ComparePoints(const transform_t& pt1, const transform_t& pt2, const std::string& errmsg, bool& error,
                   double prec = Eigen::NumTraits<double>::dummy_precision() ,bool notequal = false) {
  if (!pt1.isApprox(pt2,prec)  && !notequal) {
    error = true;
    std::cout << errmsg <<" translation :"<<pt1.translation() << " ; " << pt2.translation() << std::endl<<"rotation : "<<pt1.rotation() << " ; " << pt2.rotation() << std::endl;
  }
}
61
62
63
64

void ComparePoints(const Eigen::MatrixXd& pt1, const Eigen::MatrixXd& pt2, const std::string& errmsg, bool& error,
                   double prec = Eigen::NumTraits<double>::dummy_precision() ,bool notequal = false) {
  if (!pt1.isApprox(pt2,prec) && !(pt1.isZero(prec) && pt2.isZero(prec)) && !notequal) {
65
    error = true;
66
    std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
67
  }
68
69
}

Guilhem Saurel's avatar
Guilhem Saurel committed
70
template <typename curve1, typename curve2>
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())) {
75
    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
76
              << " and [" << c2.min() << "," << c2.max() << "] " << std::endl;
77
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
78
  } else {
79
    // derivative in T_min and T_max
80
81
    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);
82
    // Test values on curves
83
84
    for (double i = T_min; i <= T_max; i += 0.01) {
      ComparePoints(c1(i), c2(i), errMsg+" Curves evaluation do not match at t = "+boost::lexical_cast<std::string>(i), error,prec, false);
85
86
    }
  }
87
88
}

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
202
203
204
  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);
205
206
207
    ComparePoints(cf5_derivated.evalBernstein(d), cf5_derivated(d), errMsg2, error);
    ComparePoints(cf5_derivated.evalHorner(d), cf5_derivated(d), errMsg2, error);
    ComparePoints(cf5.derivate(d,1), cf5_derivated(d), errMsg2, error);
208

209
210
  }
  bool error_in(true);
Guilhem Saurel's avatar
Guilhem Saurel committed
211
  try {
212
    cf(-0.4);
Guilhem Saurel's avatar
Guilhem Saurel committed
213
  } catch (...) {
214
215
    error_in = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
216
  if (error_in) {
217
218
219
220
    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
221
  try {
222
    cf(1.1);
Guilhem Saurel's avatar
Guilhem Saurel committed
223
  } catch (...) {
224
225
    error_in = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
226
  if (error_in) {
227
228
229
    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
230
  if (!QuasiEqual(cf.max(), 1.0)) {
231
232
233
    error = true;
    std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
234
  if (!QuasiEqual(cf.min(), 0.0)) {
235
236
237
    error = true;
    std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
  }
238
239
}

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

  s2 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
277
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
278
279
280
281
    cf.evalHorner(*cit);
  }
  e2 = clock();
  s3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
282
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
283
284
285
    cf.evalDeCasteljau(*cit);
  }
  e3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
286
287
288
289
290
  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;
291
292
293
294
295
296
  params.push_back(d);
  params.push_back(e);
  params.push_back(f);
  params.push_back(g);
  params.push_back(h);
  params.push_back(i);
297
  bezier_t cf2(params.begin(), params.end());
298
  s1 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
299
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
300
301
302
303
    cf2.evalBernstein(*cit);
  }
  e1 = clock();
  s2 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
304
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
305
306
307
308
    cf2.evalHorner(*cit);
  }
  e2 = clock();
  s0 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
309
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
310
311
312
313
    cf2(*cit);
  }
  e0 = clock();
  s3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
314
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
315
316
317
    cf2.evalDeCasteljau(*cit);
  }
  e3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
318
319
320
321
  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;
322
323
}

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

Guilhem Saurel's avatar
Guilhem Saurel committed
338
339
340
void BezierDerivativeCurveTimeReparametrizationTest(bool& error) {
  std::string errMsg(
      "In test BezierDerivativeCurveTimeReparametrizationTest, Error While checking value of point on curve : ");
341
342
343
344
345
346
347
  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;
348
349
350
351
352
353
354
355
  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
356
  double diffT = Tmax - Tmin;
357
358
  bezier_t cf(params.begin(), params.end());
  bezier_t cfT(params.begin(), params.end(), Tmin, Tmax);
359
  ComparePoints(cf(0.5), cfT(1), errMsg, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
360
361
  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);
362
363
}

Guilhem Saurel's avatar
Guilhem Saurel committed
364
void BezierDerivativeCurveConstraintTest(bool& error) {
365
  std::string errMsg0("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
366
367
368
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
369
  bezier_t::curve_constraints_t constraints(3);
370
371
372
373
374
  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;
375
376
377
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
378
379
380
  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);
381
382
  ComparePoints(a, cf(T_min), errMsg0, error);
  ComparePoints(c, cf(T_max), errMsg0, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
383
384
385
386
387
388
389
390
391
392
  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) {
393
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
394
    std::cout << errMsg1 << cf.degree_ << " ; " << params.size() + 3 << std::endl;
395
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
396
  if (cf.size_ != params.size() + 4) {
397
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
398
    std::cout << errMsg2 << cf.size_ << " ; " << params.size() + 4 << std::endl;
399
  }
400
401
}

Guilhem Saurel's avatar
Guilhem Saurel committed
402
void toPolynomialConversionTest(bool& error) {
403
404
  // bezier to polynomial
  std::string errMsg("In test BezierToPolynomialConversionTest, Error While checking value of point on curve : ");
405
406
407
408
409
410
411
412
413
414
  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;
415
416
417
418
419
420
421
422
423
  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);
424
425
426
  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);
427
  polynomial_t pol = polynomial_from_curve<polynomial_t>(bc);
428
  CompareCurves<polynomial_t, bezier_t>(pol, bc, errMsg, error);
429
430
}

Guilhem Saurel's avatar
Guilhem Saurel committed
431
432
433
434
435
436
437
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 : ");
438
  // Create cubic hermite spline : Test hermite to bezier/polynomial
439
440
441
442
  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
443
444
  pair_point_tangent_t pair0(p0, m0);
  pair_point_tangent_t pair1(p1, m1);
445
446
447
  t_pair_point_tangent_t control_points;
  control_points.push_back(pair0);
  control_points.push_back(pair1);
Guilhem Saurel's avatar
Guilhem Saurel committed
448
  std::vector<double> time_control_points;
449
450
451
452
453
454
  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
455
456
  // std::cout<<"======================= \n";
  // std::cout<<"hermite to bezier \n";
457
458
  bezier_t bc0 = bezier_from_curve<bezier_t>(chs0);
  CompareCurves<cubic_hermite_spline_t, bezier_t>(chs0, bc0, errMsg0, error);
459
  // hermite to pol
Guilhem Saurel's avatar
Guilhem Saurel committed
460
461
  // std::cout<<"======================= \n";
  // std::cout<<"hermite to polynomial \n";
462
  polynomial_t pol0 = polynomial_from_curve<polynomial_t>(chs0);
463
464
  CompareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
  // pol to hermite
Guilhem Saurel's avatar
Guilhem Saurel committed
465
466
  // std::cout<<"======================= \n";
  // std::cout<<"polynomial to hermite \n";
467
  cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t>(pol0);
Guilhem Saurel's avatar
Guilhem Saurel committed
468
  CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol0, chs1, errMsg2, error);
469
  // pol to bezier
Guilhem Saurel's avatar
Guilhem Saurel committed
470
471
  // std::cout<<"======================= \n";
  // std::cout<<"polynomial to bezier \n";
472
473
  bezier_t bc1 = bezier_from_curve<bezier_t>(pol0);
  CompareCurves<bezier_t, polynomial_t>(bc1, pol0, errMsg2, error);
474
  // Bezier to pol
Guilhem Saurel's avatar
Guilhem Saurel committed
475
476
  // std::cout<<"======================= \n";
  // std::cout<<"bezier to polynomial \n";
477
  polynomial_t pol1 = polynomial_from_curve<polynomial_t>(bc0);
478
  CompareCurves<bezier_t, polynomial_t>(bc0, pol1, errMsg1, error);
479
  // bezier => hermite
Guilhem Saurel's avatar
Guilhem Saurel committed
480
481
  // std::cout<<"======================= \n";
  // std::cout<<"bezier to hermite \n";
482
  cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t>(bc0);
483
  CompareCurves<bezier_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
484
485

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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
563
void ExactCubicOneDimTest(bool& error) {
564
  curves::T_WaypointOne waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
565
566
567
568
569
570
  point_one zero;
  zero(0, 0) = 9;
  point_one one;
  one(0, 0) = 14;
  point_one two;
  two(0, 0) = 25;
571
572
573
574
575
  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
576
577
  std::string errmsg(
      "in ExactCubicOneDim Error While checking that given wayPoints  are crossed (expected / obtained)");
578
579
580
  ComparePoints(zero, res1, errmsg, error);
  res1 = exactCubic(1);
  ComparePoints(one, res1, errmsg, error);
581
582
}

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

Guilhem Saurel's avatar
Guilhem Saurel committed
592
void ExactCubicPointsCrossedTest(bool& error) {
593
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
594
  for (double i = 0; i <= 1; i = i + 0.2) {
595
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
596
597
598
599
  }
  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);
600
601
}

Guilhem Saurel's avatar
Guilhem Saurel committed
602
void ExactCubicVelocityConstraintsTest(bool& error) {
603
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
604
  for (double i = 0; i <= 1; i = i + 0.2) {
605
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
606
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
607
608
609
  std::string errmsg(
      "Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / "
      "obtained)");
stevet's avatar
stevet committed
610
  spline_constraints_t constraints(3);
611
612
613
614
  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);
615
616
617
  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
618
619
  std::string errmsg3(
      "Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
620
  // now check derivatives
621
622
623
624
  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);
625
626
627
628
  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
629
630
631
632
  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);
633
  CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error,1e-10);
Guilhem Saurel's avatar
Guilhem Saurel committed
634
635
  std::string errmsg4(
      "Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
636
  // now check derivatives
637
638
639
640
  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);
641
642
}

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

Guilhem Saurel's avatar
Guilhem Saurel committed
658
void EffectorTrajectoryTest(bool& error) {
659
660
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
661
  for (double i = 0; i <= 10; i = i + 2) {
662
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
663
664
665
  }
  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);
666
667
668
669
  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);
670
671
  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
672
  // first check start / goal positions
673
674
675
  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
676
  ComparePoints(end, (*eff_traj)(10), errmsg, error);
677
  // now check derivatives
Guilhem Saurel's avatar
Guilhem Saurel committed
678
679
680
681
682
683
684
685
686
687
688
689
690
  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);
691
692
  }
  delete eff_traj;
693
694
}

Guilhem Saurel's avatar
Guilhem Saurel committed
695
696
helpers::quat_t GetXRotQuat(const double theta) {
  Eigen::AngleAxisd m(theta, Eigen::Vector3d::UnitX());
697
  return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
698
699
}

Guilhem Saurel's avatar
Guilhem Saurel committed
700
701
702
double GetXRotFromQuat(helpers::quat_ref_const_t q) {
  Eigen::Quaterniond quat(q.data());
  Eigen::AngleAxisd m(quat);
703
  return m.angle() / M_PI * 180.;
704
705
}

Guilhem Saurel's avatar
Guilhem Saurel committed
706
void EffectorSplineRotationNoRotationTest(bool& error) {
707
708
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
709
  for (double i = 0; i <= 10; i = i + 2) {
710
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
711
712
713
714
715
716
717
718
719
720
721
722
  }
  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.;
723
  std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
Guilhem Saurel's avatar
Guilhem Saurel committed
724
725
726
727
728
  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);
729
730
}

Guilhem Saurel's avatar
Guilhem Saurel committed
731
void EffectorSplineRotationRotationTest(bool& error) {
732
733
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
734
  for (double i = 0; i <= 10; i = i + 2) {
735
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
736
737
  }
  helpers::quat_t init_quat = GetXRotQuat(M_PI);
Guilhem Saurel's avatar
Guilhem Saurel committed
738
739
740
741
742
743
744
745
746
747
748
  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);
  ;
749
  std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
Guilhem Saurel's avatar
Guilhem Saurel committed
750
751
752
753
754
  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);
755
756
}

Guilhem Saurel's avatar
Guilhem Saurel committed
757
void EffectorSplineRotationWayPointRotationTest(bool& error) {
758
759
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
760
  for (double i = 0; i <= 10; i = i + 2) {
761
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
762
763
764
765
766
  }
  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
767
768
769
770
771
772
773
774
775
776
777
778
  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;
779
  q_mod.head<3>() = point3_t(6, 6, 6);
Guilhem Saurel's avatar
Guilhem Saurel committed
780
781
782
783
784
785
786
787
788
789
790
791
  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);
792
793
}

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

821
822
point3_t randomPoint(const double min, const double max) {
  point3_t p;
Guilhem Saurel's avatar
Guilhem Saurel committed
823
824
  for (size_t i = 0; i < 3; ++i) {
    p[i] = (rand() / (double)RAND_MAX) * (max - min) + min;
825
826
  }
  return p;
827
828
}

Guilhem Saurel's avatar
Guilhem Saurel committed
829
void BezierEvalDeCasteljau(bool& error) {
830
831
  using namespace std;
  std::vector<double> values;
Guilhem Saurel's avatar
Guilhem Saurel committed
832
833
834
835
  for (int i = 0; i < 100000; ++i) {
    values.push_back(rand() / RAND_MAX);
  }
  // first compare regular evaluation (low dim pol)
836
837
838
839
840
841
842
843
844
845
  point3_t a(1, 2, 3);
  point3_t b