« エクセルで歩行モーションを作るためのVBAソースコード | トップページ | 歩行ロボットのシミュレーション設定 »

2016年12月27日 (火)

エクセルで歩行モーション作り

こんにちは、多くの方に使って頂けるようシンプルにしてみました。

改良しやすいよう、歩行モーションを計算できる最低限にしました。

なので作画機能など余分なものはないです。

マクロなしダウンロード WalkExcel_1.2z.xls (170.0K)に以下のマクロをコピペしてください。

Attribute VB_Name = "Module1"
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
    '-----------------モーション作成----------------------------
    Worksheets("作成データ").Select
       Range("A5:U30").ClearContents
       Tmp = 2.356

    For stc = 1 To 4
        fp = stc And 1                 '前半・後半フラグ
        fl = Int(stc / 2) And 1        '左足・右足フラグ
        fb = fp Xor fl                 '転記用
        fc = 1 - fb                    '転記用
        'データ転記開始行-1
        If stc < 3 Then Cell_offset = 4 Else Cell_offset = 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                                        '----------------

            wrcell = Cell_offset + n + nc * (1 - fp)      'データ転記行
            Cells(wrcell, "f") = n + nc * (stc - 1)       'ナンバー
            Cells(wrcell, "g") = Ts * (a(n) + stc - 1)    '時間
            Cells(wrcell, "c") = fp
            Cells(wrcell, "d") = fl
            
            '------------立脚相の関節角度-----------------------
            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
                '右足データの転記
                wrcell = Cell_offset + n + nc * fc
                Cells(wrcell, "j") = q4 * Deg
                Cells(wrcell, "l") = q1 * Deg
                Cells(wrcell, "n") = q2 * Deg
                Cells(wrcell, "p") = q3 * Deg
                Cells(wrcell, "r") = q5 * Deg
                Cells(wrcell, "t") = q6 * Deg
            End If
            If fl = 1 Then
                '左足データの転記
                wrcell = Cell_offset + n + nc * fb
                Cells(wrcell, "k") = -q4 * Deg
                Cells(wrcell, "m") = q1 * Deg
                Cells(wrcell, "o") = q2 * Deg
                Cells(wrcell, "q") = q3 * Deg
                Cells(wrcell, "s") = -q5 * Deg
                Cells(wrcell, "u") = q6 * Deg
            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
                '左足データの転記
                wrcell = Cell_offset + n + nc * fc
                Cells(wrcell, "k") = q4 * Deg
                Cells(wrcell, "m") = q1 * Deg
                Cells(wrcell, "o") = q2 * Deg
                Cells(wrcell, "q") = q3 * Deg
                Cells(wrcell, "s") = q5 * Deg
                Cells(wrcell, "u") = q6 * Deg
            End If
            If fl = 1 Then
                '右足データの転記
                wrcell = Cell_offset + n + nc * fb
                Cells(wrcell, "j") = -q4 * Deg
                Cells(wrcell, "l") = q1 * Deg
                Cells(wrcell, "n") = q2 * Deg
                Cells(wrcell, "p") = q3 * Deg
                Cells(wrcell, "r") = -q5 * Deg
                Cells(wrcell, "t") = q6 * Deg
            End If
        Next n
    Next stc
    Worksheets("設定シート").Select
End Sub
'
Function HSin(x)
    HSin = (Exp(x) - Exp(-x)) / 2
End Function
'
Function HCos(x)
    HCos = (Exp(x) + Exp(-x)) / 2
End Function
'
Function Arcsin(x)
    Arcsin = Atn(x / Sqr(-x * x + 1))
End Function
'
Function Arccos(x)
    If x < 1 Then
        Arccos = Atn(-x / Sqr(-x * x + 1)) + 2 * Atn(1)
    Else
        Arccos = 0
    End If
End Function

« エクセルで歩行モーションを作るためのVBAソースコード | トップページ | 歩行ロボットのシミュレーション設定 »

コメント

コメントを書く

(ウェブ上には掲載しません)

« エクセルで歩行モーションを作るためのVBAソースコード | トップページ | 歩行ロボットのシミュレーション設定 »