質問編集履歴
3
クロソイドの計算の追加、キー入力の追加
test
CHANGED
|
File without changes
|
test
CHANGED
|
@@ -18,12 +18,14 @@
|
|
|
18
18
|
|
|
19
19
|
def main():
|
|
20
20
|
|
|
21
|
-
fig = plt.figure()
|
|
21
|
+
fig = plt.figure(figsize=(11.0, 5.0))
|
|
22
22
|
|
|
23
23
|
ClickAddPoints(fig)
|
|
24
24
|
|
|
25
25
|
|
|
26
26
|
|
|
27
|
+
|
|
28
|
+
|
|
27
29
|
class ClickAddPoints:
|
|
28
30
|
|
|
29
31
|
def __init__(self, fig):
|
|
@@ -32,16 +34,10 @@
|
|
|
32
34
|
|
|
33
35
|
self.ax1 = fig.add_subplot(121)
|
|
34
36
|
|
|
35
|
-
#ax2に曲率を表示
|
|
37
|
+
# ax2に曲率を表示
|
|
36
38
|
|
|
37
39
|
self.ax2 = fig.add_subplot(122)
|
|
38
40
|
|
|
39
|
-
self.ax2.set_xticks(np.linspace(0, 6, 6))
|
|
40
|
-
|
|
41
|
-
self.ax2.set_yticks(np.linspace(0, 6, 6))
|
|
42
|
-
|
|
43
|
-
self.ax2.set_aspect('equal')
|
|
44
|
-
|
|
45
41
|
self.ax1.figure.canvas.mpl_connect('button_press_event', self.on_click)
|
|
46
42
|
|
|
47
43
|
self.ax1.figure.canvas.mpl_connect('key_press_event', self.on_key)
|
|
@@ -50,16 +46,18 @@
|
|
|
50
46
|
|
|
51
47
|
self.y = []
|
|
52
48
|
|
|
49
|
+
self.r = []
|
|
50
|
+
|
|
51
|
+
self.s = []
|
|
52
|
+
|
|
53
|
-
self.x
|
|
53
|
+
self.x2 = []
|
|
54
|
-
|
|
54
|
+
|
|
55
|
-
self.y
|
|
55
|
+
self.y2 = []
|
|
56
|
-
|
|
57
|
-
self.k = []
|
|
58
|
-
|
|
59
|
-
self.x1y1 = []
|
|
60
56
|
|
|
61
57
|
self.update_plot()
|
|
62
58
|
|
|
59
|
+
self.div = 1000 # スプラインの分割数。多いほどなめらかに描画。
|
|
60
|
+
|
|
63
61
|
plt.show()
|
|
64
62
|
|
|
65
63
|
|
|
@@ -70,7 +68,7 @@
|
|
|
70
68
|
|
|
71
69
|
u = np.linspace(0, 1, num=point, endpoint=True)
|
|
72
70
|
|
|
73
|
-
spline = interpolate.splev(u, tck)
|
|
71
|
+
spline = interpolate.splev(u, tck, der=0)
|
|
74
72
|
|
|
75
73
|
return spline[0], spline[1]
|
|
76
74
|
|
|
@@ -78,24 +76,160 @@
|
|
|
78
76
|
|
|
79
77
|
def cal_curveture(self, x1, y1, x2, y2, x3, y3):
|
|
80
78
|
|
|
79
|
+
r = 1/(abs(x1*y2-x2*y1+x2*y3-x3*y2+x3*y1-x1*y3)/(
|
|
80
|
+
|
|
81
|
-
|
|
81
|
+
((x1-x2)**2+(y1-y2)**2)*((x2-x3)**2+(y2-y3)**2)*((x3-x1)**2+(y3-y1)**2))**(1/2))
|
|
82
|
+
|
|
82
|
-
|
|
83
|
+
return r
|
|
84
|
+
|
|
85
|
+
|
|
86
|
+
|
|
87
|
+
def cal_curve_length(self, x1, y1, x2, y2):
|
|
88
|
+
|
|
89
|
+
s = np.sqrt((x2-x1)**2+(y2-y1)**2)
|
|
90
|
+
|
|
91
|
+
return s
|
|
92
|
+
|
|
93
|
+
|
|
94
|
+
|
|
95
|
+
def cal_clothoid(self, T, dt, V, R, _P0x, _P0y, _dθ):
|
|
96
|
+
|
|
97
|
+
#クロソイドの計算
|
|
98
|
+
|
|
99
|
+
v = (V*1000)/3600
|
|
100
|
+
|
|
101
|
+
L = v*T
|
|
102
|
+
|
|
103
|
+
A = np.sqrt(L*R)
|
|
104
|
+
|
|
105
|
+
dL = v*dt
|
|
106
|
+
|
|
107
|
+
P0x = _P0x
|
|
108
|
+
|
|
109
|
+
P0y = _P0y
|
|
110
|
+
|
|
111
|
+
Lt = 0
|
|
112
|
+
|
|
113
|
+
dθ = _dθ
|
|
114
|
+
|
|
115
|
+
dLt = dL
|
|
116
|
+
|
|
117
|
+
A2 = A**2
|
|
118
|
+
|
|
119
|
+
tsteps = np.arange(0, T, dt)
|
|
120
|
+
|
|
121
|
+
|
|
122
|
+
|
|
123
|
+
for i in tsteps:
|
|
124
|
+
|
|
125
|
+
x2 = P0x
|
|
126
|
+
|
|
127
|
+
y2 = P0y
|
|
128
|
+
|
|
129
|
+
|
|
130
|
+
|
|
83
|
-
self.
|
|
131
|
+
self.x2.append(x2)
|
|
132
|
+
|
|
84
|
-
|
|
133
|
+
self.y2.append(y2)
|
|
134
|
+
|
|
135
|
+
|
|
136
|
+
|
|
137
|
+
Lt = Lt + dLt
|
|
138
|
+
|
|
139
|
+
Rt = A2 / Lt
|
|
140
|
+
|
|
141
|
+
k = 1 / Rt
|
|
142
|
+
|
|
143
|
+
dθ = dθ + dLt / Rt
|
|
144
|
+
|
|
145
|
+
dx = dL * np.cos(dθ)
|
|
146
|
+
|
|
147
|
+
dy = dL * np.sin(dθ)
|
|
148
|
+
|
|
149
|
+
P1x = P0x + dx
|
|
150
|
+
|
|
151
|
+
P1y = P0y + dy
|
|
152
|
+
|
|
153
|
+
P0x = P1x
|
|
154
|
+
|
|
155
|
+
P0y = P1y
|
|
156
|
+
|
|
157
|
+
|
|
158
|
+
|
|
85
|
-
return self.
|
|
159
|
+
return self.x2, self.y2
|
|
86
160
|
|
|
87
161
|
|
|
88
162
|
|
|
89
163
|
def update_plot(self):
|
|
90
164
|
|
|
91
|
-
self.ax1.set_xticks(np.linspace(0,
|
|
165
|
+
self.ax1.set_xticks(np.linspace(0, 10, 5))
|
|
92
|
-
|
|
166
|
+
|
|
93
|
-
self.ax1.set_yticks(np.linspace(0,
|
|
167
|
+
self.ax1.set_yticks(np.linspace(0, 10, 5))
|
|
94
168
|
|
|
95
169
|
self.ax1.set_aspect('equal')
|
|
96
170
|
|
|
97
171
|
self.ax1.figure.canvas.draw()
|
|
98
172
|
|
|
173
|
+
self.ax2.set_ylim(0, 40)
|
|
174
|
+
|
|
175
|
+
self.ax2.figure.canvas.draw()
|
|
176
|
+
|
|
177
|
+
|
|
178
|
+
|
|
179
|
+
def on_key(self, event):
|
|
180
|
+
|
|
181
|
+
# qキーで終了
|
|
182
|
+
|
|
183
|
+
if event.key == 'q':
|
|
184
|
+
|
|
185
|
+
print('Finish')
|
|
186
|
+
|
|
187
|
+
return
|
|
188
|
+
|
|
189
|
+
|
|
190
|
+
|
|
191
|
+
# wキーで全削除
|
|
192
|
+
|
|
193
|
+
if event.key == 'w':
|
|
194
|
+
|
|
195
|
+
self.x.clear()
|
|
196
|
+
|
|
197
|
+
self.y.clear()
|
|
198
|
+
|
|
199
|
+
self.redraw()
|
|
200
|
+
|
|
201
|
+
print('All Clear')
|
|
202
|
+
|
|
203
|
+
|
|
204
|
+
|
|
205
|
+
# pキーでスプライン
|
|
206
|
+
|
|
207
|
+
if event.key == 'p':
|
|
208
|
+
|
|
209
|
+
print('spline')
|
|
210
|
+
|
|
211
|
+
|
|
212
|
+
|
|
213
|
+
# cキーでクロソイド
|
|
214
|
+
|
|
215
|
+
elif event.key == 'c':
|
|
216
|
+
|
|
217
|
+
print('clothoid')
|
|
218
|
+
|
|
219
|
+
|
|
220
|
+
|
|
221
|
+
# tキーで直線
|
|
222
|
+
|
|
223
|
+
elif event.key == 't':
|
|
224
|
+
|
|
225
|
+
print('straight line')
|
|
226
|
+
|
|
227
|
+
|
|
228
|
+
|
|
229
|
+
else:
|
|
230
|
+
|
|
231
|
+
return
|
|
232
|
+
|
|
99
233
|
|
|
100
234
|
|
|
101
235
|
def on_click(self, event):
|
|
@@ -120,9 +254,7 @@
|
|
|
120
254
|
|
|
121
255
|
self.redraw()
|
|
122
256
|
|
|
123
|
-
print('Added no.{} point at [{} {}]'.format(
|
|
257
|
+
print('Added no.{} point at [{} {}]'.format(len(self.x), self.x[-1], self.y[-1]))
|
|
124
|
-
|
|
125
|
-
len(self.x), self.x[-1], self.y[-1]))
|
|
126
258
|
|
|
127
259
|
elif event.button == 3:
|
|
128
260
|
|
|
@@ -144,6 +276,8 @@
|
|
|
144
276
|
|
|
145
277
|
self.ax1.cla()
|
|
146
278
|
|
|
279
|
+
self.ax2.cla()
|
|
280
|
+
|
|
147
281
|
count = len(self.x)
|
|
148
282
|
|
|
149
283
|
if count > 0:
|
|
@@ -174,19 +308,33 @@
|
|
|
174
308
|
|
|
175
309
|
return
|
|
176
310
|
|
|
311
|
+
|
|
312
|
+
|
|
177
|
-
x1, y1 = self.calc_spline(self.x, self.y,
|
|
313
|
+
x1, y1 = self.calc_spline(self.x, self.y, self.div, deg)
|
|
314
|
+
|
|
178
|
-
|
|
315
|
+
#曲線長
|
|
179
|
-
|
|
180
|
-
|
|
316
|
+
|
|
181
|
-
self.x
|
|
317
|
+
if len(self.x) >= 2:
|
|
318
|
+
|
|
182
|
-
|
|
319
|
+
self.s = [self.cal_curve_length(x1[i],y1[i],
|
|
320
|
+
|
|
183
|
-
|
|
321
|
+
x1[i+1],y1[i+1])
|
|
322
|
+
|
|
184
|
-
|
|
323
|
+
for i in range(0, len(x1)-1)]
|
|
324
|
+
|
|
325
|
+
|
|
326
|
+
|
|
185
|
-
|
|
327
|
+
# 曲率データの更新
|
|
328
|
+
|
|
186
|
-
|
|
329
|
+
if len(self.x) > 2:
|
|
330
|
+
|
|
187
|
-
|
|
331
|
+
self.r = [self.cal_curveture(x1[i-1], y1[i-1],
|
|
332
|
+
|
|
188
|
-
|
|
333
|
+
x1[i], y1[i],
|
|
334
|
+
|
|
189
|
-
|
|
335
|
+
x1[i+1], y1[i+1])
|
|
336
|
+
|
|
337
|
+
for i in range(1, len(x1)-1)]
|
|
190
338
|
|
|
191
339
|
|
|
192
340
|
|
|
@@ -194,38 +342,14 @@
|
|
|
194
342
|
|
|
195
343
|
self.ax1.plot(x1, y1)
|
|
196
344
|
|
|
345
|
+
self.ax2.plot(self.r)
|
|
346
|
+
|
|
197
347
|
self.update_plot()
|
|
198
348
|
|
|
199
349
|
|
|
200
350
|
|
|
201
351
|
|
|
202
352
|
|
|
203
|
-
def on_key(self, event):
|
|
204
|
-
|
|
205
|
-
#qキーで終了
|
|
206
|
-
|
|
207
|
-
if event.key == 'q':
|
|
208
|
-
|
|
209
|
-
print('Finish')
|
|
210
|
-
|
|
211
|
-
return
|
|
212
|
-
|
|
213
|
-
|
|
214
|
-
|
|
215
|
-
#wキーで全削除
|
|
216
|
-
|
|
217
|
-
if event.key == 'w':
|
|
218
|
-
|
|
219
|
-
self.x.clear()
|
|
220
|
-
|
|
221
|
-
self.y.clear()
|
|
222
|
-
|
|
223
|
-
self.redraw()
|
|
224
|
-
|
|
225
|
-
print('All Clear')
|
|
226
|
-
|
|
227
|
-
|
|
228
|
-
|
|
229
353
|
main()
|
|
230
354
|
|
|
231
355
|
```
|
2
ax2の追加
test
CHANGED
|
File without changes
|
test
CHANGED
|
@@ -32,6 +32,8 @@
|
|
|
32
32
|
|
|
33
33
|
self.ax1 = fig.add_subplot(121)
|
|
34
34
|
|
|
35
|
+
#ax2に曲率を表示
|
|
36
|
+
|
|
35
37
|
self.ax2 = fig.add_subplot(122)
|
|
36
38
|
|
|
37
39
|
self.ax2.set_xticks(np.linspace(0, 6, 6))
|
1
ax2の追加
test
CHANGED
|
File without changes
|
test
CHANGED
|
@@ -30,7 +30,15 @@
|
|
|
30
30
|
|
|
31
31
|
self.fig = fig
|
|
32
32
|
|
|
33
|
-
self.ax1 = fig.add_
|
|
33
|
+
self.ax1 = fig.add_subplot(121)
|
|
34
|
+
|
|
35
|
+
self.ax2 = fig.add_subplot(122)
|
|
36
|
+
|
|
37
|
+
self.ax2.set_xticks(np.linspace(0, 6, 6))
|
|
38
|
+
|
|
39
|
+
self.ax2.set_yticks(np.linspace(0, 6, 6))
|
|
40
|
+
|
|
41
|
+
self.ax2.set_aspect('equal')
|
|
34
42
|
|
|
35
43
|
self.ax1.figure.canvas.mpl_connect('button_press_event', self.on_click)
|
|
36
44
|
|
|
@@ -78,9 +86,9 @@
|
|
|
78
86
|
|
|
79
87
|
def update_plot(self):
|
|
80
88
|
|
|
81
|
-
self.ax1.set_xticks(np.linspace(0,
|
|
89
|
+
self.ax1.set_xticks(np.linspace(0, 6, 6))
|
|
82
|
-
|
|
90
|
+
|
|
83
|
-
self.ax1.set_yticks(np.linspace(0,
|
|
91
|
+
self.ax1.set_yticks(np.linspace(0, 6, 6))
|
|
84
92
|
|
|
85
93
|
self.ax1.set_aspect('equal')
|
|
86
94
|
|