Main.cpp 91.1 KB
Newer Older
1

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
19
20

using namespace std;

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
21
namespace curves {
22
typedef Eigen::Vector3d point3_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
23
typedef Eigen::VectorXd pointX_t;
24
typedef Eigen::Quaternion<double> quaternion_t;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
25
26
27
28
29
30
31
32
33
34
35
typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
typedef curve_abc<double, double, true, pointX_t> curve_abc_t;
typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
typedef exact_cubic<double, double, true, pointX_t> exact_cubic_t;
typedef exact_cubic<double, double, true, Eigen::Matrix<double, 1, 1> > exact_cubic_one;
typedef bezier_curve<double, double, true, pointX_t> bezier_curve_t;
typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, bezier_curve_t> piecewise_bezier_curve_t;
typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, cubic_hermite_spline_t>
    piecewise_cubic_hermite_curve_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
36
37
38
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef std::pair<double, pointX_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
39
typedef Eigen::Matrix<double, 1, 1> point_one;
Guilhem Saurel's avatar
Guilhem Saurel committed
40
41
42
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
43
44
45
46
typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
typedef SO3Linear<double, double, true> SO3Linear_t;
typedef SE3Curve<double, double, true> SE3Curve_t;
typedef Eigen::Transform<double, 3, Eigen::Affine> transform_t;
Guilhem Saurel's avatar
Guilhem Saurel committed
47
48

const double margin = 1e-3;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
49
bool QuasiEqual(const double a, const double b) { return std::fabs(a - b) < margin; }
50
bool QuasiEqual(const point3_t a, const point3_t b) {
Guilhem Saurel's avatar
Guilhem Saurel committed
51
  bool equal = true;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
52
53
  for (size_t i = 0; i < 3; ++i) {
    equal = equal && QuasiEqual(a[i], b[i]);
Guilhem Saurel's avatar
Guilhem Saurel committed
54
55
56
  }
  return equal;
}
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
57
}  // End namespace curves
58

59
using namespace curves;
60

61
ostream& operator<<(ostream& os, const point3_t& pt) {
62
63
  os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
  return os;
64
65
}

66
67
68
69
70
71
72
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;
  }
}
73
74
75
76

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) {
77
    error = true;
78
    std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
79
  }
80
81
}

Guilhem Saurel's avatar
Guilhem Saurel committed
82
template <typename curve1, typename curve2>
83
void CompareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error ,double prec = Eigen::NumTraits<double>::dummy_precision()) {
84
85
  double T_min = c1.min();
  double T_max = c1.max();
Guilhem Saurel's avatar
Guilhem Saurel committed
86
87
88
  if (!QuasiEqual(T_min, c2.min()) || !QuasiEqual(T_max, c2.max())) {
    std::cout << "CompareCurves, ERROR, time min and max of curves do not match [" << T_min << "," << T_max << "] "
              << " and [" << c2.min() << "," << c2.max() << "] " << std::endl;
89
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
90
  } else {
91
    // derivative in T_min and T_max
92
93
    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);
94
    // Test values on curves
95
96
    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);
97
98
    }
  }
99
100
}

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

/*bezier_curve Function tests*/
Guilhem Saurel's avatar
Guilhem Saurel committed
174
void BezierCurveTest(bool& error) {
175
  std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
176
177
178
179
180
  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;
181
182
183
  params.push_back(a);
  // 1d curve in [0,1]
  bezier_curve_t cf1(params.begin(), params.end());
184
  point3_t res1;
185
  res1 = cf1(0);
186
  point3_t x10 = a;
187
  ComparePoints(x10, res1, errMsg + "1(0) ", error);
Guilhem Saurel's avatar
Guilhem Saurel committed
188
  res1 = cf1(1);
189
190
191
192
193
  ComparePoints(x10, res1, errMsg + "1(1) ", error);
  // 2d curve in [0,1]
  params.push_back(b);
  bezier_curve_t cf(params.begin(), params.end());
  res1 = cf(0);
194
  point3_t x20 = a;
195
  ComparePoints(x20, res1, errMsg + "2(0) ", error);
196
  point3_t x21 = b;
197
198
  res1 = cf(1);
  ComparePoints(x21, res1, errMsg + "2(1) ", error);
Guilhem Saurel's avatar
Guilhem Saurel committed
199
  // 3d curve in [0,1]
200
201
202
203
204
205
  params.push_back(c);
  bezier_curve_t cf3(params.begin(), params.end());
  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
206
  // 4d curve in [1,2]
207
208
  params.push_back(d);
  bezier_curve_t cf4(params.begin(), params.end(), 1., 2.);
Guilhem Saurel's avatar
Guilhem Saurel committed
209
210
  // testing bernstein polynomials
  bezier_curve_t cf5(params.begin(), params.end(), 1., 2.);
211
  std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
Guilhem Saurel's avatar
Guilhem Saurel committed
212
213
214
215
216
  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);
    ComparePoints(cf5.compute_derivate(1).evalBernstein(d), cf5.compute_derivate(1)(d), errMsg2, error);
    ComparePoints(cf5.compute_derivate(1).evalHorner(d), cf5.compute_derivate(1)(d), errMsg2, error);
217
218
  }
  bool error_in(true);
Guilhem Saurel's avatar
Guilhem Saurel committed
219
  try {
220
    cf(-0.4);
Guilhem Saurel's avatar
Guilhem Saurel committed
221
  } catch (...) {
222
223
    error_in = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
224
  if (error_in) {
225
226
227
228
    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
229
  try {
230
    cf(1.1);
Guilhem Saurel's avatar
Guilhem Saurel committed
231
  } catch (...) {
232
233
    error_in = false;
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
234
  if (error_in) {
235
236
237
    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
238
  if (!QuasiEqual(cf.max(), 1.0)) {
239
240
241
    error = true;
    std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
242
  if (!QuasiEqual(cf.min(), 0.0)) {
243
244
245
    error = true;
    std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
  }
246
247
}

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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
347
348
349
void BezierDerivativeCurveTimeReparametrizationTest(bool& error) {
  std::string errMsg(
      "In test BezierDerivativeCurveTimeReparametrizationTest, Error While checking value of point on curve : ");
350
351
352
353
354
355
356
  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;
357
358
359
360
361
362
363
364
  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
365
  double diffT = Tmax - Tmin;
366
  bezier_curve_t cf(params.begin(), params.end());
Guilhem Saurel's avatar
Guilhem Saurel committed
367
  bezier_curve_t cfT(params.begin(), params.end(), Tmin, Tmax);
368
  ComparePoints(cf(0.5), cfT(1), errMsg, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
369
370
  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);
371
372
}

Guilhem Saurel's avatar
Guilhem Saurel committed
373
void BezierDerivativeCurveConstraintTest(bool& error) {
374
  std::string errMsg0("In test BezierDerivativeCurveConstraintTest, Error While checking value of point on curve : ");
375
376
377
  point3_t a(1, 2, 3);
  point3_t b(2, 3, 4);
  point3_t c(3, 4, 5);
stevet's avatar
stevet committed
378
  bezier_curve_t::curve_constraints_t constraints(3);
379
380
381
382
383
  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;
384
385
386
387
388
389
390
391
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
  bezier_curve_t::num_t T_min = 1.0;
  bezier_curve_t::num_t T_max = 3.0;
  bezier_curve_t cf(params.begin(), params.end(), constraints, T_min, T_max);
  ComparePoints(a, cf(T_min), errMsg0, error);
  ComparePoints(c, cf(T_max), errMsg0, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
392
393
394
395
396
397
398
399
400
401
  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) {
402
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
403
    std::cout << errMsg1 << cf.degree_ << " ; " << params.size() + 3 << std::endl;
404
  }
Guilhem Saurel's avatar
Guilhem Saurel committed
405
  if (cf.size_ != params.size() + 4) {
406
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
407
    std::cout << errMsg2 << cf.size_ << " ; " << params.size() + 4 << std::endl;
408
  }
409
410
}

Guilhem Saurel's avatar
Guilhem Saurel committed
411
void toPolynomialConversionTest(bool& error) {
412
413
  // bezier to polynomial
  std::string errMsg("In test BezierToPolynomialConversionTest, Error While checking value of point on curve : ");
414
415
416
417
418
419
420
421
422
423
  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;
424
425
426
427
428
429
430
431
432
433
434
  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);
  bezier_curve_t::num_t T_min = 1.0;
  bezier_curve_t::num_t T_max = 3.0;
Guilhem Saurel's avatar
Guilhem Saurel committed
435
  bezier_curve_t bc(control_points.begin(), control_points.end(), T_min, T_max);
436
437
  polynomial_t pol = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc);
  CompareCurves<polynomial_t, bezier_curve_t>(pol, bc, errMsg, error);
438
439
}

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

  // Test : compute derivative of bezier => Convert it to polynomial
  bezier_curve_t bc_der = bc0.compute_derivate(1);
  polynomial_t pol_test = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc_der);
  CompareCurves<bezier_curve_t, polynomial_t>(bc_der, pol_test, errMsg1, error);
498
499
}

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
572
void ExactCubicOneDimTest(bool& error) {
573
  curves::T_WaypointOne waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
574
575
576
577
578
579
  point_one zero;
  zero(0, 0) = 9;
  point_one one;
  one(0, 0) = 14;
  point_one two;
  two(0, 0) = 25;
580
581
582
583
584
  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
585
586
  std::string errmsg(
      "in ExactCubicOneDim Error While checking that given wayPoints  are crossed (expected / obtained)");
587
588
589
  ComparePoints(zero, res1, errmsg, error);
  res1 = exactCubic(1);
  ComparePoints(one, res1, errmsg, error);
590
591
}

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

Guilhem Saurel's avatar
Guilhem Saurel committed
601
void ExactCubicPointsCrossedTest(bool& error) {
602
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
603
  for (double i = 0; i <= 1; i = i + 0.2) {
604
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
605
606
607
608
  }
  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);
609
610
}

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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
704
705
helpers::quat_t GetXRotQuat(const double theta) {
  Eigen::AngleAxisd m(theta, Eigen::Vector3d::UnitX());
706
  return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
707
708
}

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

Guilhem Saurel's avatar
Guilhem Saurel committed
715
void EffectorSplineRotationNoRotationTest(bool& error) {
716
717
  // create arbitrary trajectory
  curves::T_Waypoint waypoints;
Guilhem Saurel's avatar
Guilhem Saurel committed
718
  for (double i = 0; i <= 10; i = i + 2) {
719
    waypoints.push_back(std::make_pair(i, point3_t(i, i, i)));
Guilhem Saurel's avatar
Guilhem Saurel committed
720
721
722
723
724
725
726
727
728
729
730
731
  }
  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.;
732
  std::string errmsg("Error in EffectorSplineRotationNoRotationTest; while checking waypoints (expected / obtained)");
Guilhem Saurel's avatar
Guilhem Saurel committed
733
734
735
736
737
  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);
738
739
}

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

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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
838
void BezierEvalDeCasteljau(bool& error) {
839
840
  using namespace std;
  std::vector<double> values;
Guilhem Saurel's avatar
Guilhem Saurel committed
841
842
843
844
  for (int i = 0; i < 100000; ++i) {
    values.push_back(rand() / RAND_MAX);
  }
  // first compare regular evaluation (low dim pol)
845
846
847
848
849
850
851
852
853
854
  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;
855
856
857
858
859
860
  params.push_back(a);
  params.push_back(b);
  params.push_back(c);
  // 3d curve
  bezier_curve_t cf(params.begin(), params.end());
  std::string errmsg("Error in BezierEvalDeCasteljau; while comparing actual bezier evaluation and de Casteljau : ");
Guilhem Saurel's avatar
Guilhem Saurel committed
861
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
862
863
864
865
866
867
868
869
870
    ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
  }
  params.push_back(d);
  params.push_back(e);
  params.push_back(f);
  params.push_back(g);
  params.push_back(h);
  params.push_back(i);
  bezier_curve_t cf2(params.begin(), params.end());
Guilhem Saurel's avatar
Guilhem Saurel committed
871
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
872
873
    ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
  }
874
875
876
}

/**
Guilhem Saurel's avatar
Guilhem Saurel committed
877
878
879
880
 * @brief BezierSplitCurve test the 'split' method of bezier curve
 * @param error
 */
void BezierSplitCurve(bool& error) {
881
882
883
884
885
  // test for degree 5
  size_t n = 5;
  double t_min = 0.2;
  double t_max = 10;
  double aux0, aux1;
Guilhem Saurel's avatar
Guilhem Saurel committed
886
887
888
889
  std::string errMsg0(
      "BezierSplitCurve, ERROR initial point of the splitted curve doesn't correspond to the original");
  std::string errMsg1(
      "BezierSplitCurve, ERROR splitting point of the splitted curve doesn't correspond to the original");
890
891
892
893
894
  std::string errMsg2("BezierSplitCurve, ERROR final point of the splitted curve doesn't correspond to the original");
  std::string errMsg3("BezierSplitCurve, ERROR while checking value on curve and curves splitted");
  std::string errMsg4("BezierSplitCurve, ERROR Degree of the splitted curve are not the same as the original curve");
  std::string errMsg5("BezierSplitCurve, ERROR duration of the splitted curve doesn't correspond to the original");
  std::string errMsg6("BezierSplitCurve, ERROR while checking value on curve extracted");
Guilhem Saurel's avatar
Guilhem Saurel committed
895
  for (size_t i = 0; i < 1; ++i) {