Main.cpp 15.7 KB
Newer Older
1

stonneau's avatar
stonneau committed
2
3
#include "spline/exact_cubic.h"
#include "spline/bezier_curve.h"
4
#include "spline/spline_curve.h"
Steve Tonneau's avatar
Steve Tonneau committed
5
#include "spline/spline_deriv_constraint.h"
6
#include "spline/helpers/effector_spline.h"
7
#include "spline/helpers/effector_spline_rotation.h"
8
9
10
11
12
13
14
15
16

#include <string>
#include <iostream>
#include <cmath>

using namespace std;

namespace spline
{
17
typedef Eigen::Vector3d point_t;
18
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> >  t_point_t;
19
typedef spline_curve  <double, double, 3, true, point_t, t_point_t> cubic_function_t;
20
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
Steve Tonneau's avatar
Steve Tonneau committed
21
typedef spline_deriv_constraint <double, double, 3, true, point_t> spline_deriv_constraint_t;
22
typedef bezier_curve  <double, double, 3, true, point_t> bezier_curve_t;
Steve Tonneau's avatar
Steve Tonneau committed
23
typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
steve tonneau's avatar
steve tonneau committed
24
25
typedef std::pair<double, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
26

27
28

typedef Eigen::Matrix<double,1,1> point_one;
29
typedef spline_curve<double, double, 1, true, point_one> cubic_function_one;
30
31
32
33
typedef exact_cubic   <double, double, 1, true, point_one> exact_cubic_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;

34
bool QuasiEqual(const double a, const double b, const float margin)
35
36
37
38
39
40
41
42
43
44
45
{
	if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
	{
		return (abs(a-b)) <= margin;
	}
	else
	{
		return abs(a) + abs(b) <= margin;
	}
}

46
const double margin = 0.001;
47

48
49
50
51
} // namespace spline

using namespace spline;

52
ostream& operator<<(ostream& os, const point_t& pt)
53
54
55
56
57
{
    os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
    return os;
}

58
void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error)
59
{
60
    if((pt1-pt2).norm() > margin)
61
62
	{
		error = true;
63
        std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
64
65
66
67
68
69
70
71
	}
}

/*Cubic Function tests*/

void CubicFunctionTest(bool& error)
{
	std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
72
73
74
75
	point_t a(1,2,3);
	point_t b(2,3,4);
	point_t c(3,4,5);
	point_t d(3,6,7);
76
77
78
79
80
81
    t_point_t vec;
    vec.push_back(a);
    vec.push_back(b);
    vec.push_back(c);
    vec.push_back(d);
    cubic_function_t cf(vec.begin(), vec.end(), 0, 1);
82
83
84
	point_t res1;
	res1 =cf(0); 
	point_t x0(1,2,3);
85
    ComparePoints(x0, res1, errMsg + "(0) ", error);
86
	
87
88
89
    point_t x1(9,15,19);
    res1 =cf(1);
    ComparePoints(x1, res1, errMsg + "(1) ", error);
90
	
91
92
	point_t x2(3.125,5.25,7.125);
	res1 =cf(0.5);
93
94
95
96
97
98
99
100
    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);
    cubic_function_t cf2(vec, 0.5, 1);
101
	res1 = cf2(0.5); 
102
    ComparePoints(x0, res1, errMsg + "x3 ", error);
103
104
105
106
107
108
109
110
111
112
	error = true;	
	try
	{
		cf2(0.4);
	}
	catch(...)
	{
		error = false;
	}
	if(error)
113
114
115
	{
		std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
	}
116
117
118
119
120
121
122
123
124
125
	error = true;	
	try
	{
		cf2(1.1);
	}
	catch(...)
	{
		error = false;
	}
	if(error)
126
127
128
	{
		std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
	}
stonneau's avatar
stonneau committed
129
130
131
132
133
134
135
136
137
138
	if(cf.max() != 1)
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
	}
	if(cf.min() != 0)
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
	}
139
140
}

141
/*bezier_curve Function tests*/
142
143
144
145

void BezierCurveTest(bool& error)
{
	std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
146
147
148
149
	point_t a(1,2,3);
	point_t b(2,3,4);
	point_t c(3,4,5);
	point_t d(3,6,7);
150

151
	std::vector<point_t> params;
152
153
154
155
	params.push_back(a);
	params.push_back(b);

	// 2d curve
156
157
158
159
	bezier_curve_t cf(params.begin(), params.end());
	point_t res1;
	res1 = cf(0); 
	point_t x20 = a ;
160
    ComparePoints(x20, res1, errMsg + "2(0) ", error);
161
	
162
163
	point_t x21 = b;
	res1 = cf(1); 
164
    ComparePoints(x21, res1, errMsg + "2(1) ", error);
165
166
167
	
	//3d curve
	params.push_back(c);
168
169
	bezier_curve_t cf3(params.begin(), params.end());
	res1 = cf3(0); 
170
    ComparePoints(a, res1, errMsg + "3(0) ", error);
171

172
	res1 = cf3(1); 
173
    ComparePoints(c, res1, errMsg + "3(1) ", error);
174
175
176

	//4d curve
	params.push_back(d);
177
178
	bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2);
	res1 = cf4(0.4); 
179
    ComparePoints(a, res1, errMsg + "3(0) ", error);
180
181
	
	res1 = cf4(2); 
182
    ComparePoints(d, res1, errMsg + "3(1) ", error);
183

184
	try
185
	{
186
		cf(-0.4);
187
	}
188
	catch(...)
189
	{
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
		error = false;
	}
	if(error)
	{
		std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
	}
	error = true;	
	try
	{
		cf(1.1);
	}
	catch(...)
	{
		error = false;
	}
	if(error)
	{
		std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
208
	}
stonneau's avatar
stonneau committed
209
210
211
212
213
214
215
216
217
218
	if(cf.max() != 1)
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
	}
	if(cf.min() != 0)
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
	}
219
220
}

221
222
223
224
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
	spline::T_Waypoint waypoints;
225
	for(double i = 0; i <= 1; i = i + 0.2)
226
	{
227
		waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
228
	}
229
230
231
	exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
	point_t res1;
	try
232
	{
233
234
		exactCubic(0);
		exactCubic(1);
235
	}
236
	catch(...)
237
238
	{
		error = true;
239
		std::cout << "Evaluation of ExactCubicNoErrorTest error\n";
240
	}
241
242
	error = true;
	try
243
	{
244
245
246
247
248
249
250
251
252
		exactCubic(1.2);
	}
	catch(...)
	{
		error = false;
	}
	if(error)
	{
		std::cout << "Evaluation of exactCubic cf error, 1.2 should be an out of range value\n";
253
	}
stonneau's avatar
stonneau committed
254
	if(exactCubic.max() != 1)
255
256
257
258
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
	}
stonneau's avatar
stonneau committed
259
	if(exactCubic.min() != 0)
260
261
262
263
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
	}
264
265
}

266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303

/*Exact Cubic Function tests*/
void ExactCubicTwoPointsTest(bool& error)
{
	spline::T_Waypoint waypoints;
	for(double i = 0; i < 2; ++i)
	{
		waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
	}
	exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
	
	point_t res1 = exactCubic(0);
	std::string errmsg("in ExactCubic 2 points Error While checking that given wayPoints  are crossed (expected / obtained)");
	ComparePoints(point_t(0,0,0), res1, errmsg, error);
		
	res1 = exactCubic(1);
	ComparePoints(point_t(1,1,1), res1, errmsg, error);
}

void ExactCubicOneDimTest(bool& error)
{
	spline::T_WaypointOne waypoints;
	point_one zero; zero(0,0) = 9;
	point_one one; one(0,0) = 14;
	point_one two; two(0,0) = 25;
	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);
	std::string errmsg("in ExactCubicOneDim Error While checking that given wayPoints  are crossed (expected / obtained)");
	ComparePoints(zero, res1, errmsg, error);
		
	res1 = exactCubic(1);
	ComparePoints(one, res1, errmsg, error);
}

304
305
306
307
308
309
310
311
312
313
314
315
void CheckWayPointConstraint(const std::string& errmsg, const double step, const spline::T_Waypoint& wayPoints, const exact_cubic_t* curve, bool& error )
{
    point_t res1;
    for(double i = 0; i <= 1; i = i + step)
    {
        res1 = (*curve)(i);
        ComparePoints(point_t(i,i,i), res1, errmsg, error);
    }
}

void CheckDerivative(const std::string& errmsg, const double eval_point, const std::size_t order, const point_t& target, const exact_cubic_t* curve, bool& error )
{
316
    point_t res1 = curve->derivate(eval_point,order);
317
318
319
320
    ComparePoints(target, res1, errmsg, error);
}


321
322
323
void ExactCubicPointsCrossedTest(bool& error)
{
	spline::T_Waypoint waypoints;
324
	for(double i = 0; i <= 1; i = i + 0.2)
325
	{
326
		waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
327
	}
328
329
330
331
    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);

332
333
}

334
335
336
337
338
339
340
341
342
void ExactCubicVelocityConstraintsTest(bool& error)
{
    spline::T_Waypoint waypoints;
    for(double i = 0; i <= 1; i = i + 0.2)
    {
        waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
    }
    std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
    spline_constraints_t constraints;
Steve Tonneau's avatar
Steve Tonneau committed
343
    spline_deriv_constraint_t exactCubic(waypoints.begin(), waypoints.end());
344
345
346
347
348
349
    // now check that init and end velocity are 0
    CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
    std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
    // now check derivatives
    CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
    CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
350
351
    CheckDerivative(errmsg3,0,2,constraints.init_acc,&exactCubic, error);
    CheckDerivative(errmsg3,1,2,constraints.end_acc,&exactCubic, error);
352
353
354

    constraints.end_vel = point_t(1,2,3);
    constraints.init_vel = point_t(-1,-2,-3);
355
356
    constraints.end_acc = point_t(4,5,6);
    constraints.init_acc = point_t(-4,-4,-6);
357
    std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
Steve Tonneau's avatar
Steve Tonneau committed
358
    spline_deriv_constraint_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
359
360
361
362
363
    CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);

    std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
    // now check derivatives
    CheckDerivative(errmsg4,0,1,constraints.init_vel,&exactCubic2, error);
364
365
366
    CheckDerivative(errmsg4,1,1,constraints.end_vel ,&exactCubic2, error);
    CheckDerivative(errmsg4,0,2,constraints.init_acc,&exactCubic2, error);
    CheckDerivative(errmsg4,1,2,constraints.end_acc ,&exactCubic2, error);
367
368
}

369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
void CheckPointOnline(const std::string& errmsg, const point_t& A, const point_t& B, const double target, const exact_cubic_t* 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)
    {
        error = true;
        std::cout << errmsg << " ; " << A.transpose() << "\n ; " << B.transpose() << "\n ; " <<
                     target << " ; " << res1.transpose() <<  std::endl;
    }
}

void EffectorTrajectoryTest(bool& error)
{
    // create arbitrary trajectory
    spline::T_Waypoint waypoints;
    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),
392
                                                               1,0.02,1,0.5);
393
394
    point_t zero(0,0,0);
    point_t off1(0,0,1);
395
    point_t off2(10,10,10.02);
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
    point_t end(10,10,10);
    std::string errmsg("Error in EffectorTrajectoryTest; while checking waypoints (expected / obtained)");
    std::string errmsg2("Error in EffectorTrajectoryTest; while checking derivative (expected / obtained)");
    //first check start / goal positions
    ComparePoints(zero, (*eff_traj)(0), errmsg, error);
    ComparePoints(off1, (*eff_traj)(1), errmsg, error);
    ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
    ComparePoints(end , (*eff_traj)(10), errmsg, error);

    //then check offset at start / goal positions
    // now check derivatives
    CheckDerivative(errmsg2,0,1,zero,eff_traj, error);
    CheckDerivative(errmsg2,10,1,zero ,eff_traj, error);
    CheckDerivative(errmsg2,0,2,zero,eff_traj, error);
    CheckDerivative(errmsg2,10,2,zero ,eff_traj, 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(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
    }

419
    for(double i = 9.981; i<10; i+=0.002)
420
421
422
    {
        CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
    }
423
424
    delete eff_traj;
}
425

426
427
428
429
helpers::quat_t GetXRotQuat(const double theta)
{
    Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX());
    return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
430
431
}

432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
void EffectorSplineRotationNoRotationTest(bool& error)
{
    // create arbitrary trajectory
    spline::T_Waypoint waypoints;
    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.;
    std::string errmsg("Error in EffectorSplineRotationNoRotationTest; 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);
}
453

454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
void EffectorSplineRotationRotationTest(bool& error)
{
    // create arbitrary trajectory
    spline::T_Waypoint waypoints;
    for(double i = 0; i <= 10; i = i + 2)
    {
        waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
    }
    helpers::quat_t init_quat = GetXRotQuat(M_PI);
    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);;
    std::string errmsg("Error in EffectorSplineRotationNoRotationTest; 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(5).tail<4>(), errmsg,error);
    ComparePoints(q_end , eff_traj(10),          errmsg,error);
}

477
int main(int /*argc*/, char** /*argv[]*/)
478
{
479
480
    std::cout << "performing tests... \n";
    bool error = false;
481
    CubicFunctionTest(error);
482
483
484
    ExactCubicNoErrorTest(error);
    ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
    ExactCubicTwoPointsTest(error);
485
486
    ExactCubicOneDimTest(error);
    ExactCubicVelocityConstraintsTest(error);
487
    EffectorTrajectoryTest(error);
488
    EffectorSplineRotationNoRotationTest(error);
489
490
    EffectorSplineRotationRotationTest(error);
    BezierCurveTest(error);
491
492
	if(error)
	{
493
        std::cout << "There were some errors\n";
494
495
496
497
498
499
500
501
		return -1;
	}
	else
	{
		std::cout << "no errors found \n";
		return 0;
	}
}
502