Main.cpp 89.9 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, error,prec, false);
    ComparePoints(c1.derivate(T_max, 1), c2.derivate(T_max, 1), errMsg, error,prec, false);
88
    // Test values on curves
Guilhem Saurel's avatar
Guilhem Saurel committed
89
    for (double i = T_min; i < T_max; i += 0.02) {
90
91
      ComparePoints(c1(i), c2(i), errMsg, error,prec, false);
      ComparePoints(c1(i), c2(i), errMsg, error,prec, false);
92
93
    }
  }
94
95
}

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
243
void BezierCurveTestCompareHornerAndBernstein(bool&)  // error
244
{
245
246
  using namespace std;
  std::vector<double> values;
Guilhem Saurel's avatar
Guilhem Saurel committed
247
248
249
250
251
252
253
254
255
256
257
258
259
  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);
260
261
262
263
264
  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
265
  bezier_curve_t cf(params.begin(), params.end());  // defined in [0,1]
266
  // Check all evaluation of bezier curve
Guilhem Saurel's avatar
Guilhem Saurel committed
267
  clock_t s0, e0, s1, e1, s2, e2, s3, e3;
268
  s0 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
269
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
270
271
272
273
    cf(*cit);
  }
  e0 = clock();
  s1 = 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
279
    cf.evalBernstein(*cit);
  }
  e1 = clock();

  s2 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
280
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
281
282
283
284
    cf.evalHorner(*cit);
  }
  e2 = clock();
  s3 = 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
    cf.evalDeCasteljau(*cit);
  }
  e3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
289
290
291
292
293
  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;
294
295
296
297
298
299
300
301
  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
302
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
303
304
305
306
    cf2.evalBernstein(*cit);
  }
  e1 = clock();
  s2 = 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.evalHorner(*cit);
  }
  e2 = clock();
  s0 = 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(*cit);
  }
  e0 = clock();
  s3 = 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
    cf2.evalDeCasteljau(*cit);
  }
  e3 = clock();
Guilhem Saurel's avatar
Guilhem Saurel committed
321
322
323
324
  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;
325
326
}

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

Guilhem Saurel's avatar
Guilhem Saurel committed
342
343
344
345
346
347
348
349
350
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);
351
352
353
354
355
356
357
358
359
  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
360
  double diffT = Tmax - Tmin;
361
  bezier_curve_t cf(params.begin(), params.end());
Guilhem Saurel's avatar
Guilhem Saurel committed
362
  bezier_curve_t cfT(params.begin(), params.end(), Tmin, Tmax);
363
  ComparePoints(cf(0.5), cfT(1), errMsg, error);
Guilhem Saurel's avatar
Guilhem Saurel committed
364
365
  ComparePoints(cf.derivate(0.5, 1), cfT.derivate(1, 1) * (diffT), errMsg, error);
  ComparePoints(cf.derivate(0.5, 2), cfT.derivate(1, 2) * diffT * diffT, errMsg, error);
366
367
}

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

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

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

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

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

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

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

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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
647
648
649
650
651
652
653
654
655
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) {
656
    error = true;
Guilhem Saurel's avatar
Guilhem Saurel committed
657
658
    std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " << target << " ; "
              << res1.transpose() << std::endl;
659
  }
660
661
}

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

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

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

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

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

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

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

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

Guilhem Saurel's avatar
Guilhem Saurel committed
833
void BezierEvalDeCasteljau(bool& error) {
834
835
  using namespace std;
  std::vector<double> values;
Guilhem Saurel's avatar
Guilhem Saurel committed
836
837
838
839
840
841
842
843
844
845
846
847
848
  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);
849
850
851
852
853
854
855
  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
856
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
857
858
859
860
861
862
863
864
865
    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
866
  for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
867
868
    ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
  }
869
870
871
}

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