エクセルで歩行モーションを作るためのVBAソースコード
マクロVBAコードです。(修正しました)
安全のためマクロ無しにしたシートダウンロード WalkExcel_1.2x.xls (268.0K)
(ご自分で以下のマクロをコピペしてください)
Public q1, q2, q3, q4, q5 Sub main() Worksheets("設定シート").Select Pi = 3.141592 Deg = 180 / Pi G = 9.8 '----------モーション時間設定----------- nc = Int(Range("d04") / 2) 'モーション数(一歩の半分) Ts = Range("d03") / 2 '一歩の半分 '-----------ロボット設定------------ Hc = Range("e19") / 1000 '重心高さ kh = Range("e24") '立脚相腰高さ(腰下げ)の設定 Lx = Range("b28") / 1000 'ピッチ側の関節間隔 Ly = Range("j28") / 1000 'ロール側の関節間隔 St = Range("f9") / 2000 '歩幅(半分) Sw = Range("c9") / 2000 '歩隔(半分)= 両足の間隔(半分) Sw_offset = Sw '重心~足のオフセット = 歩隔(半分) Uh = Range("e34") * St * 2 '遊脚相足上げ高さの設定 '----------------------------------- Tc = Sqr(Hc / G) C = HCos(Ts / Tc) S = HSin(Ts / Tc) Vx = St / (Tc * S) '歩速 Zx = Sqr(Lx * Lx - St * St) * kh '立脚高さピッチ側 Zy = Ly - (Lx - Zx) '立脚高さロール側 '計算用 Dim a(24) Dim b(24) For n = 0 To nc a(n) = n / nc b(n) = (nc - n) / nc Next n '作画用足部 Ax1 = Range("d64") / Deg Ax2 = Range("d65") / Deg Ax3 = Range("e64") / 1000 / Hc Ax4 = Range("e65") / 1000 / Hc Ay1 = Range("i64") / Deg Ay2 = Range("i65") / Deg Ay3 = Range("j64") / 1000 / Hc Ay4 = Range("j65") / 1000 / Hc '-----------------モーション作成---------------------------- Worksheets("作成データ").Select Range("A5:U250").ClearContents Tmp = 2.356 For stc = 1 To 4 fp = stc And 1 '前半・後半フラグ fl = Int(stc / 2) And 1 '左足・右足フラグ fb = Int(stc * 1.5) And 1 '作画用 fc = 1 - fb '作画用 'データ転記開始行-1 If stc < 3 Then offcell = 4 Else offcell = 5 + nc * 2 For n = 1 To nc '------------位置の計算----------------------------- If fp = 1 Then t = Ts / Tc * a(n) '立脚 Px = Vx * Tc * HSin(t) Py = Sw / C * HCos(t) '游脚 Ux = Sin(a(n) * Tmp) / Sin(Tmp) * -St Uy = -Py Uref = Sin((a(n) / 2 + 0.5) ^ 0.75 * Pi) toe = Arcsin(Uref * Uh / 0.12) * a(n) '踵で着地 Else '立脚 t = -Ts / Tc * b(n) Px = Vx * Tc * HSin(t) Py = Sw / C * HCos(t) '游脚 Ux = Sin(b(n) * Tmp) / Sin(Tmp) * St Uy = -Py Uref = Sin((a(n) / 2) ^ 0.75 * Pi) toe = -Arcsin(Uref * Uh / 0.12) * b(n) '爪先で蹴り End If dcell = offcell + n + nc * (1 - fp) Cells(dcell, "f") = n + nc * (stc - 1) Cells(dcell, "g") = Ts * (a(n) + stc - 1) '------------立脚相の関節角度----------------------- Pl = Sqr(Zx ^ 2 + Px ^ 2) c1 = Arccos(Pl / Lx) cf = -Atn(Px / Zx) q1 = cf + c1 q2 = c1 * 2 q3 = c1 - cf q4 = Atn((Py - Sw_offset) / Zy) q5 = q4 q6 = Px / Zx If fl = 0 Then 'RFoot dcell = offcell + n + nc * fc Cells(dcell, "j") = q4 * Deg Cells(dcell, "l") = q1 * Deg Cells(dcell, "n") = q2 * Deg Cells(dcell, "p") = q3 * Deg Cells(dcell, "r") = q5 * Deg Cells(dcell, "t") = q6 * Deg '散布図データ scell = nc * 6 * (3 * fc) x = Plot(n, scell, q1, q2, q3, -q4, q5, Hc / Lx, Hc / Ly, -Ay1, -Ay2, Ay3, Ay4, -Sw / Hc, Ax1, Ax2, Ax3, Ax4) End If If fl = 1 Then 'LFoot dcell = offcell + n + nc * fb Cells(dcell, "k") = -q4 * Deg Cells(dcell, "m") = q1 * Deg Cells(dcell, "o") = q2 * Deg Cells(dcell, "q") = q3 * Deg Cells(dcell, "s") = -q5 * Deg Cells(dcell, "u") = q6 * Deg '散布図データ scell = nc * 6 * (3 * fb) x = Plot(n, scell, q1, q2, q3, q4, -q5, Hc / Lx, Hc / Ly, Ay1, Ay2, Ay3, Ay4, Sw / Hc, Ax1, Ax2, Ax3, Ax4) End If '------------游脚相の関節角度----------------------- Zu = Zx - Uref * Uh Ul = Sqr(Zu ^ 2 + Ux ^ 2) c1 = Arccos(Ul / Lx) cf = -Atn(Ux / Zu) q1 = cf + c1 q2 = c1 * 2 q3 = c1 - cf + toe q4 = -Atn((Uy + Sw_offset) / (Zy - Uref * Uh)) q5 = q4 * 0.9 q6 = -Px / Zx If fl = 0 Then 'LFoot dcell = offcell + n + nc * fc Cells(dcell, "k") = q4 * Deg Cells(dcell, "m") = q1 * Deg Cells(dcell, "o") = q2 * Deg Cells(dcell, "q") = q3 * Deg Cells(dcell, "s") = q5 * Deg Cells(dcell, "u") = q6 * Deg '散布図データ scell = nc * 6 * (1 + fb) x = Plot(n, scell, q1, q2, q3, -q4, q5, Hc / Lx, Hc / Ly, Ay1, Ay2, Ay3, Ay4, Sw / Hc, Ax1, Ax2, Ax3, Ax4) End If If fl = 1 Then 'RFoot dcell = offcell + n + nc * fb Cells(dcell, "j") = -q4 * Deg Cells(dcell, "l") = q1 * Deg Cells(dcell, "n") = q2 * Deg Cells(dcell, "p") = q3 * Deg Cells(dcell, "r") = -q5 * Deg Cells(dcell, "t") = q6 * Deg '散布図データ scell = nc * 6 * (1 + fc) x = Plot(n, scell, q1, q2, q3, q4, -q5, Hc / Lx, Hc / Ly, -Ay1, -Ay2, Ay3, Ay4, -Sw / Hc, Ax1, Ax2, Ax3, Ax4) End If Next n Next stc Worksheets("設定シート").Select End Sub '作画用データ Function Plot(n, scell, q1, q2, q3, q4, q5, kx, ky, Ay1, Ay2, Ay3, Ay4, xy, Ax1, Ax2, Ax3, Ax4) rad = 0.0174533 x1 = Sin(q1) / kx / 2 y1 = Cos(q1) / kx / 2 x2 = x1 + Sin(q1 - q2) / kx / 2 y2 = y1 + Cos(q1 - q2) / kx / 2 x3 = x2 + Sin(q1 - q2 + q3 + Ax1) * Ax3 y3 = y2 + Cos(q1 - q2 + q3 + Ax1) * Ax3 x4 = x3 + Sin(q1 - q2 + q3 + Ax2) * Ax4 y4 = y3 + Cos(q1 - q2 + q3 + Ax2) * Ax4 Cells(scell + n * 6 + 0, "a") = 0 Cells(scell + n * 6 + 0, "b") = 0 Cells(scell + n * 6 + 1, "a") = x1 Cells(scell + n * 6 + 1, "b") = -y1 Cells(scell + n * 6 + 2, "a") = x2 Cells(scell + n * 6 + 2, "b") = -y2 Cells(scell + n * 6 + 3, "a") = x3 Cells(scell + n * 6 + 3, "b") = -y3 Cells(scell + n * 6 + 4, "a") = x4 Cells(scell + n * 6 + 4, "b") = -y4 x1 = xy y1 = 0 x2 = x1 + Sin(q4) / ky y2 = y1 + Cos(q4) / ky x3 = x2 + Sin(q4 + q5 + Ay1) * Ay3 y3 = y2 + Cos(q4 + q5 + Ay1) * Ay3 x4 = x2 + Sin(q4 + q5 + Ay2) * Ay4 y4 = y2 + Cos(q4 + q5 + Ay2) * Ay4 Cells(scell + n * 6 + 0, "c") = 0 Cells(scell + n * 6 + 0, "d") = 0 Cells(scell + n * 6 + 1, "c") = x1 Cells(scell + n * 6 + 1, "d") = -y1 Cells(scell + n * 6 + 2, "c") = x2 Cells(scell + n * 6 + 2, "d") = -y2 Cells(scell + n * 6 + 3, "c") = x3 Cells(scell + n * 6 + 3, "d") = -y3 Cells(scell + n * 6 + 4, "c") = x4 Cells(scell + n * 6 + 4, "d") = -y4 Plot = 1 End Function ' Function HSin(x) HSin = (Exp(x) - Exp(-x)) / 2 End Function ' Function HCos(x) HCos = (Exp(x) + Exp(-x)) / 2 End Function ' Public Function Arcsin(x) Arcsin = Atn(x / Sqr(-x * x + 1)) End Function ' Public Function Arccos(x) If x < 1 Then Arccos = Atn(-x / Sqr(-x * x + 1)) + 2 * Atn(1) Else Arccos = 0 End If End Function
« エクセルで歩行モーションの作成 | トップページ | エクセルで歩行モーション作り »
こんにちは。こちらのプログラムを膝ダブルサーボのロボットに適用するには理論式のどこを変更したらよろしいでしょうか...?また、理論で参考にした文献など教えて頂けるとありがたいです。もしよろしければご返答お願いします。
投稿: 名無し | 2016年12月11日 (日) 23時38分
こんにちは、膝ダブルサーボへの適用ですが、脚の関節角度は位置Px,長さLx,高さZxから求めています。
みっちーさんの http://dream-drive.net が参考になります。
参考にした文献はたくさんあって・・・足位置の計算は「ヒューマノイドロボット」の4章かな・・・。
投稿: | 2016年12月26日 (月) 23時44分