forked from realkushagrakhare/3D_Path_Planning
-
Notifications
You must be signed in to change notification settings - Fork 0
/
SplineInterpolation-test-3.py
74 lines (67 loc) · 1.88 KB
/
SplineInterpolation-test-3.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
import numpy as np
from scipy.interpolate import splprep, splev
#import matplotlib.pyplot as plt
def fitSpline(points):
pts = np.array(points)
tck, u = splprep(pts.T, u=None, s=0.0, per=1)
u_new = np.linspace(u.min(), u.max(), 1000)
x_new, y_new = splev(u_new, tck, der=0)
return list(zip(x_new,y_new))
pts = np.array([[ 6.55525 , 3.05472 ],
[ 6.17284 , 2.802609],
[ 5.53946 , 2.649209],
[ 4.93053 , 2.444444],
[ 4.32544 , 2.318749],
[ 3.90982 , 2.2875 ],
[ 3.51294 , 2.221875],
[ 3.09107 , 2.29375 ],
[ 2.64013 , 2.4375 ],
[ 2.275444, 2.653124],
[ 2.137945, 3.26562 ],
[ 2.15982 , 3.84375 ],
[ 2.20982 , 4.31562 ],
[ 2.334704, 4.87873 ],
[ 2.314264, 5.5047 ],
[ 2.311709, 5.9135 ],
[ 2.29638 , 6.42961 ],
[ 2.619374, 6.75021 ],
[ 3.32448 , 6.66353 ],
[ 3.31582 , 5.68866 ],
[ 3.35159 , 5.17255 ],
[ 3.48482 , 4.73125 ],
[ 3.70669 , 4.51875 ],
[ 4.23639 , 4.58968 ],
[ 4.39592 , 4.94615 ],
[ 4.33527 , 5.33862 ],
[ 3.95968 , 5.61967 ],
[ 3.56366 , 5.73976 ],
[ 3.78818 , 6.55292 ],
[ 4.27712 , 6.8283 ],
[ 4.89532 , 6.78615 ],
[ 5.35334 , 6.72433 ],
[ 5.71583 , 6.54449 ],
[ 6.13452 , 6.46019 ],
[ 6.54478 , 6.26068 ],
[ 6.7873 , 5.74615 ],
[ 6.64086 , 5.25269 ],
[ 6.45649 , 4.86206 ],
[ 6.41586 , 4.46519 ],
[ 5.44711 , 4.26519 ],
[ 5.04087 , 4.10581 ],
[ 4.70013 , 3.67405 ],
[ 4.83482 , 3.4375 ],
[ 5.34086 , 3.43394 ],
[ 5.76392 , 3.55156 ],
[ 6.37056 , 3.8778 ],
[ 6.53116 , 3.47228 ]])
tck, u = splprep(pts.T, u=None, s=0.0, per=1) # Here pts.T means Transpose of the pts.
#print(u.min())
#print(u.max())
#print(tck)
u_new = np.linspace(u.min(), u.max(), 1000)
x_new, y_new = splev(u_new, tck, der=0)
x = zip(x_new,y_new)
print(list(x))
#plt.plot(pts[:,0], pts[:,1], 'ro')
#plt.plot(x_new, y_new, 'b--')
#plt.show()