Main.cpp 90.5 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 {
Guilhem Saurel's avatar
Guilhem Saurel committed
22
23
typedef Eigen::Vector3d point_t;
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
50
bool QuasiEqual(const double a, const double b) { return std::fabs(a - b) < margin; }
bool QuasiEqual(const point_t a, const point_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

Guilhem Saurel's avatar
Guilhem Saurel committed
61
ostream& operator<<(ostream& os, const point_t& pt) {
62
63
  os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
  return os;
64
65
}

66
67
68
69
70


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) {
71
    error = true;
72
    std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
73
  }
74
75
}

Guilhem Saurel's avatar
Guilhem Saurel committed
76
template <typename curve1, typename curve2>
77
void CompareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error ,double prec = Eigen::NumTraits<double>::dummy_precision()) {
78
79
  double T_min = c1.min();
  double T_max = c1.max();
Guilhem Saurel's avatar
Guilhem Saurel committed
80
81
82
  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;
83
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
84
  } else {
85
    // derivative in T_min and T_max
86
87
    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);
88
    // Test values on curves
89
90
    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);
91
92
    }
  }
93
94
}

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

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

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

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

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

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

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

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

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

  // 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);
492
493
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
832
void BezierEvalDeCasteljau(bool& error) {
833
834
  using namespace std;
  std::vector<double> values;
Guilhem Saurel's avatar
Guilhem Saurel committed
835
836
837
838
839
840
841
842
843
844
845
846
847
  for (int i = 0; i < 100000; ++i) {
    values.push_back(rand() / RAND_MAX);
  }
  // first compare regular evaluation (low dim pol)
  point_t a(1, 2, 3);
  point_t b(2, 3, 4);
  point_t c(3, 4, 5);
  point_t d(3, 6, 7);
  point_t e(3, 61, 7);
  point_t f(3, 56, 7);
  point_t g(3, 36, 7);
  point_t h(43, 6, 7);
  point_t i(3, 6, 77);
848
849
850
851
852
853
854
  std::vector<point_t> params;
  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
855
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
856
857
858
859
860
861
862
863
864
    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
865
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
866
867
    ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
  }
868
869
870
}

/**
Guilhem Saurel's avatar
Guilhem Saurel committed
871
872
873
874
 * @brief BezierSplitCurve test the 'split' method of bezier curve
 * @param error
 */
void BezierSplitCurve(bool& error) {
875
876
877
878
879
  // 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
880
881
882
883
  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");
884
885
886
887
888
  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
889
  for (size_t i = 0; i < 1; ++i) {
890
    // build a random curve and split it at random time :
Guilhem Saurel's avatar
Guilhem Saurel committed
891
    // std::cout<<"build a random curve"<<std::endl;
892
893
    point_t a;
    std::vector<point_t> wps;
Guilhem Saurel's avatar
Guilhem Saurel committed
894
895
    for (size_t j = 0; j <= n; ++j) {
      wps.push_back(randomPoint(-10., 10.));
896
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
897
898
899
900
901
    double t0 = (rand() / (double)RAND_MAX) * (t_max - t_min) + t_min;
    double t1 = (rand() / (double)RAND_MAX) * (t_max - t0) + t0;
    double ts = (rand() / (double)RAND_MAX) * (t1 - t0) + t0;
    bezier_curve_t c(wps.begin(), wps.end(), t0, t1);
    std::pair<bezier_curve_t, bezier_curve_t> cs = c.split(ts);
902
    // test on splitted curves :
Guilhem Saurel's avatar
Guilhem Saurel committed
903
    if (!((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_))) {
904
      error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
905
      std::cout << errMsg4 << std::endl;
906
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
907
908
909
    aux0 = c.max() - c.min();
    aux1 = (cs.first.max() - cs.first.min() + cs.second.max() - cs.second.min());
    if (!QuasiEqual(aux0, aux1)) {
910
      error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
911
      std::cout << errMsg5 << std::endl;
912
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
913
    if (!QuasiEqual(cs.first.max(), ts)) {
914
      error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
915
      std::cout << errMsg0 << std::endl;
916
917
918
919
920
921
    }
    ComparePoints(c(t0), cs.first(t0), errMsg0, error);
    ComparePoints(cs.first(ts), cs.second(ts), errMsg1, error);
    ComparePoints(c(t1), cs.second(cs.second.max()), errMsg2, error);
    // check along curve :
    double ti = t0;
Guilhem Saurel's avatar
Guilhem Saurel committed
922
    while (ti <= ts) {
923
924
925
      ComparePoints(cs.first(ti), c(ti), errMsg3, error);
      ti += 0.01;
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
926
    while (ti <= t1) {
927
928
929
930
      ComparePoints(cs.second(ti), c(ti), errMsg3, error);
      ti += 0.01;
    }
    // Test extract function
Guilhem Saurel's avatar
Guilhem Saurel committed
931
932
933
    bezier_curve_t bezier_extracted0 = c.extract(t0 + 0.01, t1 - 0.01);  // T_min < t0 < t1 < T_max
    for (double t = bezier_extracted0.min(); t < bezier_extracted0.max(); t += 0.01) {
      ComparePoints(bezier_extracted0(t), c(t), errMsg6, error);
934
    }
Guilhem Saurel's avatar
Guilhem Saurel committed
935
936
937
    bezier_curve_t bezier_extracted1 = c.extract(t0, t1 - 0.01);  // T_min = t0 < t1 < T_max
    for (double t = bezier_extracted1.min(); t < bezier_extracted1.max(); t += 0.01) {
      ComparePoints(bezier_extracted1(t), c(t), errMsg6, error);