Main.cpp 25.6 KB
Newer Older
1

stonneau's avatar
stonneau committed
2 3
#include "spline/exact_cubic.h"
#include "spline/bezier_curve.h"
4
#include "spline/polynom.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
#include "spline/bezier_polynom_conversion.h"
9 10 11 12 13 14 15 16 17

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

using namespace std;

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

28 29

typedef Eigen::Matrix<double,1,1> point_one;
30
typedef polynom<double, double, 1, true, point_one> polynom_one;
31 32 33 34
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;

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

47
const double margin = 0.001;
48

49 50 51 52
} // namespace spline

using namespace spline;

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

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

/*Cubic Function tests*/

void CubicFunctionTest(bool& error)
{
72
    std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
73 74 75
	point_t a(1,2,3);
	point_t b(2,3,4);
	point_t c(3,4,5);
76
    point_t d(3,6,7);
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);
82 83 84 85
    polynom_t cf(vec.begin(), vec.end(), 0, 1);
    point_t res1;
    res1 =cf(0);
    point_t x0(1,2,3);
86
    ComparePoints(x0, res1, errMsg + "(0) ", error);
87

88 89 90
    point_t x1(9,15,19);
    res1 =cf(1);
    ComparePoints(x1, res1, errMsg + "(1) ", error);
91 92 93

    point_t x2(3.125,5.25,7.125);
    res1 =cf(0.5);
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);
101 102
    polynom_t cf2(vec, 0.5, 1);
    res1 = cf2(0.5);
103
    ComparePoints(x0, res1, errMsg + "x3 ", error);
104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139
    error = true;
    try
    {
        cf2(0.4);
    }
    catch(...)
    {
        error = false;
    }
    if(error)
    {
        std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
    }
    error = true;
    try
    {
        cf2(1.1);
    }
    catch(...)
    {
        error = false;
    }
    if(error)
    {
        std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
    }
    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";
    }
140 141
}

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

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

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

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

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

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

183 184 185 186 187
    //testing bernstein polynomes
    std::string errMsg2("In test BezierCurveTest ; Bernstein polynoms do not evaluate as analytical evaluation");
    for(double d = 0.; d <1.; d+=0.1)
    {
        ComparePoints( cf3.evalBernstein(d) , cf3 (d), errMsg2, error);
188
        ComparePoints( cf3.evalHorner(d) , cf3 (d), errMsg2, error);
189 190 191
    }

    bool error_in(true);
192
	try
193
	{
194
		cf(-0.4);
195
	}
196
	catch(...)
197
	{
198
        error_in = false;
199
	}
200
    if(error_in)
201 202
	{
		std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
203
        error = true;
204
	}
205
    error_in = true;
206 207 208 209 210 211
	try
	{
		cf(1.1);
	}
	catch(...)
	{
212
        error_in = false;
213
	}
214
    if(error_in)
215 216
	{
		std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
217
        error = true;
218
	}
stonneau's avatar
stonneau committed
219 220 221 222 223 224 225 226 227 228
	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";
	}
229 230
}

231
#include <ctime>
stevet's avatar
stevet committed
232
void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 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 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326
{
    using namespace std;
    std::vector<double> values;
    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);

    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());

    clock_t s0,e0,s1,e1,s2,e2;
    s0 = clock();
    for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
    {
        cf(*cit);
    }
    e0 = clock();

    s1 = clock();
    for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
    {
        cf.evalBernstein(*cit);
    }
    e1 = clock();

    s2 = clock();
    for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
    {
        cf.evalHorner(*cit);
    }
    e2 = clock();


    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 << "now with high order polynom "    << std::endl;


    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();
    for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
    {
        cf2.evalBernstein(*cit);
    }
    e1 = clock();

    s2 = clock();
    for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
    {
        cf2.evalHorner(*cit);
    }
    e2 = clock();

    s0 = clock();
    for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
    {
        cf2(*cit);
    }
    e0 = clock();


    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;

}

327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345
void BezierDerivativeCurveTest(bool& error)
{
    std::string errMsg("In test BezierDerivativeCurveTest ; unexpected result for x ");
    point_t a(1,2,3);
    point_t b(2,3,4);
    point_t c(3,4,5);

    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());

    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);
}

346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371
void BezierDerivativeCurveTimeReparametrizationTest(bool& error)
{
    std::string errMsg("In test BezierDerivativeCurveTimeReparametrizationTest ; unexpected result for x ");
    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);

    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 T = 2.;
    bezier_curve_t cf(params.begin(), params.end());
    bezier_curve_t cfT(params.begin(), params.end(),T);

    ComparePoints(cf(0.5), cfT(1), errMsg, error);
    ComparePoints(cf.derivate(0.5,1), cfT.derivate(1,1) * T, errMsg, error);
    ComparePoints(cf.derivate(0.5,2), cfT.derivate(1,2) * T*T, errMsg, error);
}

372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402
void BezierDerivativeCurveConstraintTest(bool& error)
{
    std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
    point_t a(1,2,3);
    point_t b(2,3,4);
    point_t c(3,4,5);

    bezier_curve_t::curve_constraints_t constraints;
    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);

    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(), constraints);

    assert(cf3.degree_ == params.size() + 3);
    assert(cf3.size_   == params.size() + 4);

    ComparePoints(a, cf3(0), errMsg, error);
    ComparePoints(c, cf3(1), errMsg, error);
    ComparePoints(constraints.init_vel, cf3.derivate(0.,1), errMsg, error);
    ComparePoints(constraints.end_vel , cf3.derivate(1.,1), errMsg, error);
    ComparePoints(constraints.init_acc, cf3.derivate(0.,2), errMsg, error);
    ComparePoints(constraints.end_vel, cf3.derivate(1.,1), errMsg, error);
    ComparePoints(constraints.end_acc, cf3.derivate(1.,2), errMsg, error);
}
403 404


405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437
void BezierToPolynomConversionTest(bool& error)
{
    std::string errMsg("In test BezierToPolynomConversionTest ; unexpected result for x ");
    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);

    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);
    params.push_back(g);
    params.push_back(h);
    params.push_back(i);

    bezier_curve_t cf(params.begin(), params.end());
    polynom_t pol =from_bezier<bezier_curve_t, polynom_t>(cf);
    for(double i =0.; i<1.; i+=0.01)
    {
        ComparePoints(cf(i),pol(i),errMsg, error, true);
        ComparePoints(cf(i),pol(i),errMsg, error, false);
    }
}

438 439 440 441
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
	spline::T_Waypoint waypoints;
442
	for(double i = 0; i <= 1; i = i + 0.2)
443
	{
444
		waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
445
	}
446 447 448
	exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
	point_t res1;
	try
449
	{
450 451
		exactCubic(0);
		exactCubic(1);
452
	}
453
	catch(...)
454 455
	{
		error = true;
456
		std::cout << "Evaluation of ExactCubicNoErrorTest error\n";
457
	}
458 459
	error = true;
	try
460
	{
461 462 463 464 465 466 467 468 469
		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";
470
	}
stonneau's avatar
stonneau committed
471
	if(exactCubic.max() != 1)
472 473 474 475
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
	}
stonneau's avatar
stonneau committed
476
	if(exactCubic.min() != 0)
477 478 479 480
	{
		error = true;
		std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
	}
481 482
}

483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520

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

stevet's avatar
stevet committed
521
void CheckWayPointConstraint(const std::string& errmsg, const double step, const spline::T_Waypoint& /*wayPoints*/, const exact_cubic_t* curve, bool& error )
522 523 524 525 526 527 528 529 530 531 532
{
    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 )
{
533
    point_t res1 = curve->derivate(eval_point,order);
534 535 536 537
    ComparePoints(target, res1, errmsg, error);
}


538 539 540
void ExactCubicPointsCrossedTest(bool& error)
{
	spline::T_Waypoint waypoints;
541
	for(double i = 0; i <= 1; i = i + 0.2)
542
	{
543
		waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
544
	}
545 546 547 548
    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);

549 550
}

551 552 553 554 555 556 557 558 559
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
560
    spline_deriv_constraint_t exactCubic(waypoints.begin(), waypoints.end());
561 562 563 564 565 566
    // 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);
567 568
    CheckDerivative(errmsg3,0,2,constraints.init_acc,&exactCubic, error);
    CheckDerivative(errmsg3,1,2,constraints.end_acc,&exactCubic, error);
569 570 571

    constraints.end_vel = point_t(1,2,3);
    constraints.init_vel = point_t(-1,-2,-3);
572 573
    constraints.end_acc = point_t(4,5,6);
    constraints.init_acc = point_t(-4,-4,-6);
574
    std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
Steve Tonneau's avatar
Steve Tonneau committed
575
    spline_deriv_constraint_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
576 577 578 579 580
    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);
581 582 583
    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);
584 585
}

586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608
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),
609
                                                               1,0.02,1,0.5);
610 611
    point_t zero(0,0,0);
    point_t off1(0,0,1);
612
    point_t off2(10,10,10.02);
613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635
    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);
    }

636
    for(double i = 9.981; i<10; i+=0.002)
637 638 639
    {
        CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
    }
640 641
    delete eff_traj;
}
642

643 644 645 646
helpers::quat_t GetXRotQuat(const double theta)
{
    Eigen::AngleAxisd m (theta, Eigen::Vector3d::UnitX());
    return helpers::quat_t(Eigen::Quaterniond(m).coeffs().data());
647 648
}

649 650 651 652 653 654 655
double GetXRotFromQuat(helpers::quat_ref_const_t q)
{
    Eigen::Quaterniond quat (q.data());
    Eigen::AngleAxisd m (quat);
    return m.angle() / M_PI * 180.;
}

656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676
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);
}
677

678 679 680 681 682 683 684 685 686 687 688 689 690 691 692
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);;
693
    std::string errmsg("Error in EffectorSplineRotationRotationTest; while checking waypoints (expected / obtained)");
694 695 696 697 698 699 700
    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);
}

701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736
void EffectorSplineRotationWayPointRotationTest(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(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);
    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);
}

737 738
void TestReparametrization(bool& error)
{
739 740
    helpers::rotation_spline s;
    const helpers::spline_deriv_constraint_one_dim& sp = s.time_reparam_;
741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770
    if(sp.min() != 0)
    {
        std::cout << "in TestReparametrization; min value is not 0, got " << sp.min() << std::endl;
        error = true;
    }
    if(sp.max() != 1)
    {
        std::cout << "in TestReparametrization; max value is not 1, got " << sp.max() << std::endl;
        error = true;
    }
    if(sp(1)[0] != 1.)
    {
        std::cout << "in TestReparametrization; end value is not 1, got " << sp(1)[0] << std::endl;
        error = true;
    }
    if(sp(0)[0] != 0.)
    {
        std::cout << "in TestReparametrization; init value is not 0, got " << sp(0)[0] << std::endl;
        error = true;
    }
    for(double i =0; i<1; i+=0.002)
    {
        if(sp(i)[0]>sp(i+0.002)[0])
        {
            std::cout << "in TestReparametrization; reparametrization not monotonous " << sp.max() << std::endl;
            error = true;
        }
    }
}

771
int main(int /*argc*/, char** /*argv[]*/)
772
{
773 774
    std::cout << "performing tests... \n";
    bool error = false;
775
    CubicFunctionTest(error);
776 777 778
    ExactCubicNoErrorTest(error);
    ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
    ExactCubicTwoPointsTest(error);
779 780
    ExactCubicOneDimTest(error);
    ExactCubicVelocityConstraintsTest(error);
781
    EffectorTrajectoryTest(error);
782
    EffectorSplineRotationNoRotationTest(error);
783
    EffectorSplineRotationRotationTest(error);
784
    TestReparametrization(error);
785
    EffectorSplineRotationWayPointRotationTest(error);
786
    BezierCurveTest(error);
787
    BezierDerivativeCurveTest(error);
788
    BezierDerivativeCurveConstraintTest(error);
789 790
    BezierCurveTestCompareHornerAndBernstein(error);
    BezierDerivativeCurveTimeReparametrizationTest(error);
791
    BezierToPolynomConversionTest(error);
792 793
	if(error)
	{
794
        std::cout << "There were some errors\n";
795 796 797 798 799 800 801 802
		return -1;
	}
	else
	{
		std::cout << "no errors found \n";
		return 0;
	}
}
803