test.py 2.54 KB
Newer Older
1
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
2
3

from numpy import matrix
4
from numpy.linalg import norm
5

6
7
__EPS = 1e-6

Steve Tonneau's avatar
Steve Tonneau committed
8
waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
9
waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose()
Steve Tonneau's avatar
Steve Tonneau committed
10
time_waypoints = matrix([0.,1.])
Steve Tonneau's avatar
Steve Tonneau committed
11

12
#testing bezier curve
13
a = bezier6(waypoints6)
14
a = bezier(waypoints, 3.)
15

Steve Tonneau's avatar
Steve Tonneau committed
16
assert(a.degree == a.nbWaypoints -1)
Steve Tonneau's avatar
Steve Tonneau committed
17
18
19
a.min()
a.max()
a(0.4)
20
21
22
assert((a.derivate(0.4,0) == a(0.4)).all())
a.derivate(0.4,2)
a = a.compute_derivate(100)
Steve Tonneau's avatar
Steve Tonneau committed
23

24
25
prim = a.compute_primitive(1)

26

27
28
29
30
31
32
33
34
35
36
37
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()

38
39
40
41
42
43
44
45
46
47
48
49
50
51
waypoints = matrix([[1.,2.,3.],[4.,5.,6.],[4.,5.,6.],[4.,5.,6.],[4.,5.,6.]]).transpose()
a0 = bezier(waypoints)
a1 = bezier(waypoints, 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)

for i in range(10):
	t = float(i) / 10.
	assert norm(a0(t) - a1(3*t))                            < __EPS
	assert norm(a0.derivate(t,1) - a1.derivate(3*t,1) * 3.) < __EPS
	assert norm(a0.derivate(t,2) - a1.derivate(3*t,2) * 9.) < __EPS
	assert norm(prim0(t) - prim1(t*3) / 3.) < __EPS
assert(prim(0) == matrix([0.,0.,0.])).all()

52

53
54
55
56
57
58
59
#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.]);

60
waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
61
62
63
64
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's avatar
Steve Tonneau committed
65

66
67
68
#testing polynom function
a = polynom(waypoints)
a = polynom(waypoints, -1., 3.)
69
70
71
a.min()
a.max()
a(0.4)
Steve Tonneau's avatar
Steve Tonneau committed
72
73
assert((a.derivate(0.4,0) == a(0.4)).all())
a.derivate(0.4,2)
Steve Tonneau's avatar
Steve Tonneau committed
74
75
76
77
78
79
80
81

#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)
82
83

#testing spline_deriv_constraints
84
c = curve_constraints();
Steve Tonneau's avatar
Steve Tonneau committed
85
c.init_vel; 
86
87
88
89
90
91
92
c.end_vel;
c.init_acc;
c.end_acc;


c.init_vel = matrix([0.,1.,1.]);
c.end_vel  = matrix([0.,1.,1.]);
93
c.init_acc = matrix([0.,1.,1.]);
94
c.end_acc  = matrix([0.,1.,1.]);
95
96
97

a = spline_deriv_constraint (waypoints, time_waypoints)
a = spline_deriv_constraint (waypoints, time_waypoints, c)
98
99
100
101
102
103

#converting bezier to polynom

a = bezier(waypoints)
a_pol = from_bezier(a)
assert norm(a(0.3) - a_pol(0.3)) < __EPS