エクセルで歩行モーションを作るための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 '作画用
'データ転記開始行－１
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 が参考になります。
参考にした文献はたくさんあって・・・足位置の計算は「ヒューマノイドロボット」の４章かな・・・。
投稿： | 2016年12月26日 (月) 23時44分