質問編集履歴
3
クロソイドの計算の追加、キー入力の追加
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -8,46 +8,113 @@
|
|
|
8
8
|
|
|
9
9
|
|
|
10
10
|
def main():
|
|
11
|
-
fig = plt.figure()
|
|
11
|
+
fig = plt.figure(figsize=(11.0, 5.0))
|
|
12
12
|
ClickAddPoints(fig)
|
|
13
13
|
|
|
14
|
+
|
|
14
15
|
class ClickAddPoints:
|
|
15
16
|
def __init__(self, fig):
|
|
16
17
|
self.fig = fig
|
|
17
18
|
self.ax1 = fig.add_subplot(121)
|
|
18
|
-
#ax2に曲率を表示
|
|
19
|
+
# ax2に曲率を表示
|
|
19
20
|
self.ax2 = fig.add_subplot(122)
|
|
20
|
-
self.ax2.set_xticks(np.linspace(0, 6, 6))
|
|
21
|
-
self.ax2.set_yticks(np.linspace(0, 6, 6))
|
|
22
|
-
self.ax2.set_aspect('equal')
|
|
23
21
|
self.ax1.figure.canvas.mpl_connect('button_press_event', self.on_click)
|
|
24
22
|
self.ax1.figure.canvas.mpl_connect('key_press_event', self.on_key)
|
|
25
23
|
self.x = []
|
|
26
24
|
self.y = []
|
|
25
|
+
self.r = []
|
|
26
|
+
self.s = []
|
|
27
|
-
self.
|
|
27
|
+
self.x2 = []
|
|
28
|
-
self.
|
|
28
|
+
self.y2 = []
|
|
29
|
-
self.k = []
|
|
30
|
-
self.x1y1 = []
|
|
31
29
|
self.update_plot()
|
|
30
|
+
self.div = 1000 # スプラインの分割数。多いほどなめらかに描画。
|
|
32
31
|
plt.show()
|
|
33
32
|
|
|
34
33
|
def calc_spline(self, x, y, point, deg):
|
|
35
34
|
tck, u = interpolate.splprep([x, y], k=deg, s=0)
|
|
36
35
|
u = np.linspace(0, 1, num=point, endpoint=True)
|
|
37
|
-
spline = interpolate.splev(u, tck)
|
|
36
|
+
spline = interpolate.splev(u, tck, der=0)
|
|
38
37
|
return spline[0], spline[1]
|
|
39
38
|
|
|
40
39
|
def cal_curveture(self, x1, y1, x2, y2, x3, y3):
|
|
40
|
+
r = 1/(abs(x1*y2-x2*y1+x2*y3-x3*y2+x3*y1-x1*y3)/(
|
|
41
|
-
|
|
41
|
+
((x1-x2)**2+(y1-y2)**2)*((x2-x3)**2+(y2-y3)**2)*((x3-x1)**2+(y3-y1)**2))**(1/2))
|
|
42
|
-
self.k.append(k)
|
|
43
|
-
return
|
|
42
|
+
return r
|
|
44
43
|
|
|
44
|
+
def cal_curve_length(self, x1, y1, x2, y2):
|
|
45
|
+
s = np.sqrt((x2-x1)**2+(y2-y1)**2)
|
|
46
|
+
return s
|
|
47
|
+
|
|
48
|
+
def cal_clothoid(self, T, dt, V, R, _P0x, _P0y, _dθ):
|
|
49
|
+
#クロソイドの計算
|
|
50
|
+
v = (V*1000)/3600
|
|
51
|
+
L = v*T
|
|
52
|
+
A = np.sqrt(L*R)
|
|
53
|
+
dL = v*dt
|
|
54
|
+
P0x = _P0x
|
|
55
|
+
P0y = _P0y
|
|
56
|
+
Lt = 0
|
|
57
|
+
dθ = _dθ
|
|
58
|
+
dLt = dL
|
|
59
|
+
A2 = A**2
|
|
60
|
+
tsteps = np.arange(0, T, dt)
|
|
61
|
+
|
|
62
|
+
for i in tsteps:
|
|
63
|
+
x2 = P0x
|
|
64
|
+
y2 = P0y
|
|
65
|
+
|
|
66
|
+
self.x2.append(x2)
|
|
67
|
+
self.y2.append(y2)
|
|
68
|
+
|
|
69
|
+
Lt = Lt + dLt
|
|
70
|
+
Rt = A2 / Lt
|
|
71
|
+
k = 1 / Rt
|
|
72
|
+
dθ = dθ + dLt / Rt
|
|
73
|
+
dx = dL * np.cos(dθ)
|
|
74
|
+
dy = dL * np.sin(dθ)
|
|
75
|
+
P1x = P0x + dx
|
|
76
|
+
P1y = P0y + dy
|
|
77
|
+
P0x = P1x
|
|
78
|
+
P0y = P1y
|
|
79
|
+
|
|
80
|
+
return self.x2, self.y2
|
|
81
|
+
|
|
45
82
|
def update_plot(self):
|
|
46
|
-
self.ax1.set_xticks(np.linspace(0,
|
|
83
|
+
self.ax1.set_xticks(np.linspace(0, 10, 5))
|
|
47
|
-
self.ax1.set_yticks(np.linspace(0,
|
|
84
|
+
self.ax1.set_yticks(np.linspace(0, 10, 5))
|
|
48
85
|
self.ax1.set_aspect('equal')
|
|
49
86
|
self.ax1.figure.canvas.draw()
|
|
87
|
+
self.ax2.set_ylim(0, 40)
|
|
88
|
+
self.ax2.figure.canvas.draw()
|
|
50
89
|
|
|
90
|
+
def on_key(self, event):
|
|
91
|
+
# qキーで終了
|
|
92
|
+
if event.key == 'q':
|
|
93
|
+
print('Finish')
|
|
94
|
+
return
|
|
95
|
+
|
|
96
|
+
# wキーで全削除
|
|
97
|
+
if event.key == 'w':
|
|
98
|
+
self.x.clear()
|
|
99
|
+
self.y.clear()
|
|
100
|
+
self.redraw()
|
|
101
|
+
print('All Clear')
|
|
102
|
+
|
|
103
|
+
# pキーでスプライン
|
|
104
|
+
if event.key == 'p':
|
|
105
|
+
print('spline')
|
|
106
|
+
|
|
107
|
+
# cキーでクロソイド
|
|
108
|
+
elif event.key == 'c':
|
|
109
|
+
print('clothoid')
|
|
110
|
+
|
|
111
|
+
# tキーで直線
|
|
112
|
+
elif event.key == 't':
|
|
113
|
+
print('straight line')
|
|
114
|
+
|
|
115
|
+
else:
|
|
116
|
+
return
|
|
117
|
+
|
|
51
118
|
def on_click(self, event):
|
|
52
119
|
if event.button == 1:
|
|
53
120
|
# コントロール点を追加
|
|
@@ -59,8 +126,7 @@
|
|
|
59
126
|
self.x.append(event.xdata)
|
|
60
127
|
self.y.append(event.ydata)
|
|
61
128
|
self.redraw()
|
|
62
|
-
print('Added no.{} point at [{} {}]'.format(
|
|
129
|
+
print('Added no.{} point at [{} {}]'.format(len(self.x), self.x[-1], self.y[-1]))
|
|
63
|
-
len(self.x), self.x[-1], self.y[-1]))
|
|
64
130
|
elif event.button == 3:
|
|
65
131
|
# 直近のコントロール点を削除
|
|
66
132
|
if len(self.x) > 0:
|
|
@@ -71,6 +137,7 @@
|
|
|
71
137
|
|
|
72
138
|
def redraw(self):
|
|
73
139
|
self.ax1.cla()
|
|
140
|
+
self.ax2.cla()
|
|
74
141
|
count = len(self.x)
|
|
75
142
|
if count > 0:
|
|
76
143
|
# クリック点の描画
|
|
@@ -86,31 +153,26 @@
|
|
|
86
153
|
deg = 3
|
|
87
154
|
else:
|
|
88
155
|
return
|
|
89
|
-
x1, y1 = self.calc_spline(self.x, self.y, 300, deg)
|
|
90
156
|
|
|
157
|
+
x1, y1 = self.calc_spline(self.x, self.y, self.div, deg)
|
|
158
|
+
#曲線長
|
|
91
|
-
self.
|
|
159
|
+
if len(self.x) >= 2:
|
|
160
|
+
self.s = [self.cal_curve_length(x1[i],y1[i],
|
|
92
|
-
|
|
161
|
+
x1[i+1],y1[i+1])
|
|
162
|
+
for i in range(0, len(x1)-1)]
|
|
93
163
|
|
|
164
|
+
# 曲率データの更新
|
|
165
|
+
if len(self.x) > 2:
|
|
94
|
-
|
|
166
|
+
self.r = [self.cal_curveture(x1[i-1], y1[i-1],
|
|
167
|
+
x1[i], y1[i],
|
|
95
|
-
|
|
168
|
+
x1[i+1], y1[i+1])
|
|
169
|
+
for i in range(1, len(x1)-1)]
|
|
96
170
|
|
|
97
171
|
# 線の描画
|
|
98
172
|
self.ax1.plot(x1, y1)
|
|
173
|
+
self.ax2.plot(self.r)
|
|
99
174
|
self.update_plot()
|
|
100
175
|
|
|
101
176
|
|
|
102
|
-
def on_key(self, event):
|
|
103
|
-
#qキーで終了
|
|
104
|
-
if event.key == 'q':
|
|
105
|
-
print('Finish')
|
|
106
|
-
return
|
|
107
|
-
|
|
108
|
-
#wキーで全削除
|
|
109
|
-
if event.key == 'w':
|
|
110
|
-
self.x.clear()
|
|
111
|
-
self.y.clear()
|
|
112
|
-
self.redraw()
|
|
113
|
-
print('All Clear')
|
|
114
|
-
|
|
115
177
|
main()
|
|
116
178
|
```
|
2
ax2の追加
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -15,6 +15,7 @@
|
|
|
15
15
|
def __init__(self, fig):
|
|
16
16
|
self.fig = fig
|
|
17
17
|
self.ax1 = fig.add_subplot(121)
|
|
18
|
+
#ax2に曲率を表示
|
|
18
19
|
self.ax2 = fig.add_subplot(122)
|
|
19
20
|
self.ax2.set_xticks(np.linspace(0, 6, 6))
|
|
20
21
|
self.ax2.set_yticks(np.linspace(0, 6, 6))
|
1
ax2の追加
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -14,7 +14,11 @@
|
|
|
14
14
|
class ClickAddPoints:
|
|
15
15
|
def __init__(self, fig):
|
|
16
16
|
self.fig = fig
|
|
17
|
-
self.ax1 = fig.
|
|
17
|
+
self.ax1 = fig.add_subplot(121)
|
|
18
|
+
self.ax2 = fig.add_subplot(122)
|
|
19
|
+
self.ax2.set_xticks(np.linspace(0, 6, 6))
|
|
20
|
+
self.ax2.set_yticks(np.linspace(0, 6, 6))
|
|
21
|
+
self.ax2.set_aspect('equal')
|
|
18
22
|
self.ax1.figure.canvas.mpl_connect('button_press_event', self.on_click)
|
|
19
23
|
self.ax1.figure.canvas.mpl_connect('key_press_event', self.on_key)
|
|
20
24
|
self.x = []
|
|
@@ -38,8 +42,8 @@
|
|
|
38
42
|
return self.k
|
|
39
43
|
|
|
40
44
|
def update_plot(self):
|
|
41
|
-
self.ax1.set_xticks(np.linspace(0,
|
|
45
|
+
self.ax1.set_xticks(np.linspace(0, 6, 6))
|
|
42
|
-
self.ax1.set_yticks(np.linspace(0,
|
|
46
|
+
self.ax1.set_yticks(np.linspace(0, 6, 6))
|
|
43
47
|
self.ax1.set_aspect('equal')
|
|
44
48
|
self.ax1.figure.canvas.draw()
|
|
45
49
|
|