test.py 1.82 KB
 Steve Tonneau committed Apr 05, 2017 1 ``````from spline import bezier, bezier6, spline, exact_cubic, curve_constraints, spline_deriv_constraint `````` Steve Tonneau committed Mar 13, 2017 2 3 `````` from numpy import matrix `````` Steve Tonneau committed Apr 05, 2017 4 ``````from numpy.linalg import norm `````` Steve Tonneau committed Mar 13, 2017 5 `````` `````` Steve Tonneau committed Mar 14, 2017 6 ``````waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose() `````` Steve Tonneau committed Apr 05, 2017 7 ``````waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose() `````` Steve Tonneau committed Mar 14, 2017 8 ``````time_waypoints = matrix([0.,1.]) `````` Steve Tonneau committed Mar 14, 2017 9 `````` `````` Steve Tonneau committed Mar 13, 2017 10 ``````#testing bezier curve `````` Steve Tonneau committed Apr 05, 2017 11 ``````a = bezier6(waypoints6) `````` Steve Tonneau committed Mar 14, 2017 12 ``````a = bezier(waypoints, -1., 3.) `````` Steve Tonneau committed Apr 04, 2017 13 `````` `````` Steve Tonneau committed Apr 05, 2017 14 ``````assert(a.degree == a.nbWaypoints -1) `````` Steve Tonneau committed Mar 14, 2017 15 16 17 ``````a.min() a.max() a(0.4) `````` Steve Tonneau committed Mar 14, 2017 18 19 20 ``````assert((a.derivate(0.4,0) == a(0.4)).all()) a.derivate(0.4,2) a = a.compute_derivate(100) `````` Steve Tonneau committed Mar 14, 2017 21 `````` `````` Steve Tonneau committed Apr 04, 2017 22 23 24 25 26 27 28 29 30 31 32 33 34 35 ``````prim = a.compute_primitive(1) for i in range(10): t = float(i) / 10. assert(a(t) == prim.derivate(t,1)).all() assert(prim(0) == matrix([0.,0.,0.])).all() prim = a.compute_primitive(2) for i in range(10): t = float(i) / 10. assert(a(t) == prim.derivate(t,2)).all() assert(prim(0) == matrix([0.,0.,0.])).all() `````` Steve Tonneau committed Apr 05, 2017 36 37 38 39 40 41 42 43 44 45 46 47 ``````#testing bezier with constraints c = curve_constraints(); c.init_vel = matrix([0.,1.,1.]); c.end_vel = matrix([0.,1.,1.]); c.init_acc = matrix([0.,1.,-1.]); c.end_acc = matrix([0.,100.,1.]); a = bezier(waypoints,c) assert norm(a.derivate(0,1) - c.init_vel) < 1e-10 assert norm(a.derivate(1,2) - c.end_acc) < 1e-10 `````` Steve Tonneau committed Mar 14, 2017 48 49 50 51 `````` #testing spline function a = spline(waypoints) a = spline(waypoints, -1., 3.) `````` Steve Tonneau committed Mar 13, 2017 52 53 54 ``````a.min() a.max() a(0.4) `````` Steve Tonneau committed Mar 14, 2017 55 56 ``````assert((a.derivate(0.4,0) == a(0.4)).all()) a.derivate(0.4,2) `````` Steve Tonneau committed Mar 14, 2017 57 58 59 60 61 62 63 64 `````` #testing exact_cubic function a = exact_cubic(waypoints, time_waypoints) a.min() a.max() a(0.4) assert((a.derivate(0.4,0) == a(0.4)).all()) a.derivate(0.4,2) `````` Steve Tonneau committed Mar 14, 2017 65 66 `````` #testing spline_deriv_constraints `````` Steve Tonneau committed Apr 05, 2017 67 ``````c = curve_constraints(); `````` Steve Tonneau committed Mar 14, 2017 68 ``````c.init_vel; `````` Steve Tonneau committed Mar 14, 2017 69 70 71 72 73 74 75 ``````c.end_vel; c.init_acc; c.end_acc; c.init_vel = matrix([0.,1.,1.]); c.end_vel = matrix([0.,1.,1.]); `````` Steve Tonneau committed Mar 14, 2017 76 ``````c.init_acc = matrix([0.,1.,1.]); `````` Steve Tonneau committed Mar 14, 2017 77 ``````c.end_acc = matrix([0.,1.,1.]); `````` Steve Tonneau committed Mar 14, 2017 78 79 80 `````` a = spline_deriv_constraint (waypoints, time_waypoints) a = spline_deriv_constraint (waypoints, time_waypoints, c)``````