test.py 1.07 KB
Newer Older
1
from spline import bezier, spline, exact_cubic, spline_constraints, spline_constraints, spline_deriv_constraint
2
3
4

from numpy import matrix

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

8
#testing bezier curve
Steve Tonneau's avatar
Steve Tonneau committed
9
10
11
12
13
a = bezier(waypoints)
a = bezier(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
14
15
16
17
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
18
19
20
21
22


#testing spline function
a = spline(waypoints)
a = spline(waypoints, -1., 3.)
23
24
25
a.min()
a.max()
a(0.4)
Steve Tonneau's avatar
Steve Tonneau committed
26
27
assert((a.derivate(0.4,0) == a(0.4)).all())
a.derivate(0.4,2)
Steve Tonneau's avatar
Steve Tonneau committed
28
29
30
31
32
33
34
35

#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)
36
37
38

#testing spline_deriv_constraints
c = spline_constraints();
Steve Tonneau's avatar
Steve Tonneau committed
39
c.init_vel; 
40
41
42
43
44
45
46
c.end_vel;
c.init_acc;
c.end_acc;


c.init_vel = matrix([0.,1.,1.]);
c.end_vel  = matrix([0.,1.,1.]);
47
c.init_acc = matrix([0.,1.,1.]);
48
c.end_acc  = matrix([0.,1.,1.]);
49
50
51

a = spline_deriv_constraint (waypoints, time_waypoints)
a = spline_deriv_constraint (waypoints, time_waypoints, c)