teratail
回答率
85.48%
質問するログイン新規登録
トップに関する質問Scipyのsplprepによるスプライン補間について

編集履歴

質問編集履歴

3

クロソイドの計算の追加、キー入力の追加

2020/07/02 08:42

投稿

takes.it.easy
takes.it.easy

スコア18

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.x1 = []
53
+ self.x2 = []
54
-
54
+
55
- self.y1 = []
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
- k = abs(x1*y2-x2*y1+x2*y3-x3*y2+x3*y1-x1*y3)/(((x1-x2)**2+(y1-y2)**2)*((x2-x3)**2+(y2-y3)**2)*((x3-x1)**2+(y3-y1)**2))**(1/2)
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.k.append(k)
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.k
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, 6, 6))
165
+ self.ax1.set_xticks(np.linspace(0, 10, 5))
92
-
166
+
93
- self.ax1.set_yticks(np.linspace(0, 6, 6))
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, 300, deg)
313
+ x1, y1 = self.calc_spline(self.x, self.y, self.div, deg)
314
+
178
-
315
+ #曲線長
179
-
180
-
316
+
181
- self.x1.append(x1)
317
+ if len(self.x) >= 2:
318
+
182
-
319
+ self.s = [self.cal_curve_length(x1[i],y1[i],
320
+
183
- self.y1.append(y1)
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
- #k = self.cal_curveture(x1[i],y1[i],x1[i+1],y1[i+1],x1[i+2],y1[i+2])
331
+ self.r = [self.cal_curveture(x1[i-1], y1[i-1],
332
+
188
-
333
+ x1[i], y1[i],
334
+
189
- #print(k)
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の追加

2020/07/02 08:42

投稿

takes.it.easy
takes.it.easy

スコア18

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の追加

2020/07/01 09:13

投稿

takes.it.easy
takes.it.easy

スコア18

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_axes([0.1, 0.1, 0.8, 0.8])
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, 4, 5))
89
+ self.ax1.set_xticks(np.linspace(0, 6, 6))
82
-
90
+
83
- self.ax1.set_yticks(np.linspace(0, 4, 5))
91
+ self.ax1.set_yticks(np.linspace(0, 6, 6))
84
92
 
85
93
  self.ax1.set_aspect('equal')
86
94