-
Notifications
You must be signed in to change notification settings - Fork 1
/
test_spline_order_converter.py
83 lines (72 loc) · 3.6 KB
/
test_spline_order_converter.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
import numpy as np
import matplotlib.pyplot as plt
from bsplinegenerator.bsplines import BsplineEvaluation
from path_generation.spline_order_converter import SmoothingSpline
import time
num_data_points_per_interval = 1000
order = 2
control_points = np.array([[-3, -4, -2, -.5, 1 , 0, 2, 3.5, 3],
[.5, 3.5, 6, 5.5, 3.7, 2, -1, 2, 5]])
dimension = 2
num_control_points = order + 4
# control_points = np.random.randint(10, size=(dimension,num_control_points)) # random
num_points = 1000
scale_factor = 1
bspline = BsplineEvaluation(control_points, order, 0, scale_factor)
spline_data, time_data = bspline.get_spline_data(num_data_points_per_interval)
spline_velocity_data, time_data = bspline.get_derivative_magnitude_data(num_data_points_per_interval,1)
spline_acceleration_data, time_data = bspline.get_derivative_magnitude_data(num_data_points_per_interval,2)
curvature_data, time_data = bspline.get_spline_curvature_data(num_data_points_per_interval)
ang_rate_data, time_data = bspline.get_angular_rate_data(num_data_points_per_interval)
centripetal_acceleration_data, time_data = bspline.get_centripetal_acceleration_data(num_data_points_per_interval)
new_order = 3
dimension = 2
smoother = SmoothingSpline(new_order, dimension, 1000)
start_time = time.time()
new_control_points, new_scale_factor = smoother.generate_new_control_points(control_points,scale_factor,order)
end_time = time.time()
print("elapsed time: " , end_time - start_time)
new_bspline = BsplineEvaluation(new_control_points, new_order, 0, new_scale_factor)
new_spline_data, new_time_data = new_bspline.get_spline_data(num_data_points_per_interval)
new_spline_velocity_data, new_time_data = new_bspline.get_derivative_magnitude_data(num_data_points_per_interval,1)
new_spline_acceleration_data, new_time_data = new_bspline.get_derivative_magnitude_data(num_data_points_per_interval,2)
new_curvature_data, new_time_data = new_bspline.get_spline_curvature_data(num_data_points_per_interval)
new_ang_rate_data, new_time_data = new_bspline.get_angular_rate_data(num_data_points_per_interval)
new_centripetal_acceleration_data, new_time_data = new_bspline.get_centripetal_acceleration_data(num_data_points_per_interval)
new_initial_control_points = smoother.create_initial_control_points(control_points, order, np.shape(new_control_points)[1])
plt.figure()
plt.plot(spline_data[0,:],spline_data[1,:],label="3rd order spline")
plt.scatter(control_points[0,:],control_points[1,:])
# plt.plot(control_points[0,:],control_points[1,:],color="tab:blue")
plt.plot(new_spline_data[0,:],new_spline_data[1,:],'--',label="4th order spline")
plt.scatter(new_control_points[0,:],new_control_points[1,:])
# plt.scatter(new_initial_control_points[0,:],new_initial_control_points[1,:],facecolors='none', edgecolors='tab:orange')
# plt.plot(new_initial_control_points[0,:],new_initial_control_points[1,:],color='tab:orange')
plt.title("spline")
plt.legend()
plt.show()
plt.figure()
plt.plot(time_data, spline_velocity_data)
plt.plot(new_time_data, new_spline_velocity_data,'--')
plt.title("velocity")
plt.show()
plt.figure()
plt.plot(time_data, spline_acceleration_data)
plt.plot(new_time_data, new_spline_acceleration_data,'--')
plt.title("acceleration")
plt.show()
plt.figure()
plt.plot(time_data, curvature_data)
plt.plot(new_time_data, new_curvature_data,'--')
plt.title("curvature")
plt.show()
plt.figure()
plt.plot(time_data, ang_rate_data)
plt.plot(new_time_data, new_ang_rate_data,'--')
plt.title("angular_rate")
plt.show()
plt.figure()
plt.plot(time_data, centripetal_acceleration_data)
plt.plot(new_time_data, new_centripetal_acceleration_data,'--')
plt.title("cent_accel")
plt.show()