2017年1月14日 (土)

歩行モーションの作成

先に公開したC++での歩行モーション作成プログラムを、Go Simulation上で動かしたKHR-3HVの様子です。

足裏サイズを60×46㎜に、足首4個のサーボモーター設定をKp=100とKd=1.0にしています。 ジャイロは使っていません。

2017年1月13日 (金)

歩行モーションについて

○歩行を安定させるポイントですが、

遊脚時の足首角でかなりちがいます。 特に着地前の角度は重要で、姿勢が崩れて着地タイミングが多少早まっても、ロール・ピッチ共に床と平行になっているのがよさそうです。 この角度が微妙にずれた場合制御ゲインが強いとバランスが崩れるようです。

着地位置も同様に、タイミングが多少ずれても足の位置が適正になっているのが良いはずです。 なので遊脚は着地の前からスムーズな足の運びになっている必要があります。 しかし腰の高さを一定にした歩き方ではそんな位置になりにくいのではないかなと思っています。

○加減速なんですが、歩行のタイミングを一定にしないとロール方向の計算が面倒になるため、急激な加減速には対応できていません。

○自然な歩き方ですが、着地時のロスを考慮しています。(開脚のコサイン分減速)しかし他も変わってしまうので難しくなってます。

歩行ロボットのシミュレーション設定

こんばんは。 寒いですね・・・

Go Simulation上の足首サーボの設定のことなんですが、標準のままではゲインが強すぎてぜんぜん安定しません。

Kp = 3943 ⇒ 100

Kd =    88 ⇒ 1.0   位にかなり落としています。

これで調整したモーションを移して実ロボットの方もうまく歩きました。

config変えてたのですっかり忘れてました、すみませんでした。

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

2016年12月26日 (月)

エクセルで歩行モーションを作るための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

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

 ご無沙汰しております。 歩行モーションをエクセルで作成しているのですが、モーションの確認をしたかったので、シートに作画機能を組み込みました。 かなり苦労しましたが、とても良いものが出来たのでアップします。(修正を加えました)

・今回のものは、腰を落として(ロボットのように)歩行するタイプです。 

・簡易的ですが、片脚時の重心補正や游脚軌道を自動計算しています。

・マクロで計算していますので、マクロを有効にする必要があります。

・歩幅をゼロにすると足踏みになります。加減速や旋回にも対応しています、C++コードをご覧ください。

・設定を変えれば他のロボットでも安定した歩行モーションが作れるはずです。歩幅や速さも自由に変更可能です。

・一歩24データになっていますので、12個とか8個とかを使えば充分と思われます。

・ソースコードをご覧頂けます。 なるべく分かりやすくしたつもりですが、誤り等がありましたらお教えください。

                 ダウンロード WalkExcel_1.2.xls (294.0K)

設定画面

1_4

モーションチェック

2_5

モーションデータ

3_4

歩行シュミュレーション用のC++ソースコード

こんばんは、またまたずいぶんご無沙汰しております。

ロボットの二足歩行シュミュレーション用C++ソースコードです。(修正しました)

・Microsoft Visual C++ 2010上でビルドしてGo simulation上のDLLで動かしています。

・エクセルのマクロと整合させてあります。設定値などはエクセルシートをご覧ください。

・ロボットはKHR-3HVの設定になっていて、Go simulation上で安定して歩行します。

・設定値を変えれば他のロボットでも歩行のシュミュレーションができると思います。

・今回のは「膝を曲げて歩く」タイプです。 歩幅や速さも自由に設定できます。

・前回よりの主な修正は、歩きだしの所を直しました。変数名なども見直しました。モーションを繋げる時の参考にして下さい。

・足首の踵着地や爪先の蹴りを加えました。少し旋回も設定可能にしました。

・歩隔(足の開き)は静止時の両足中心の間隔と同じです。変えると游脚が面倒なことになってしまうので変える事はできていません。

・フラグにより二歩を四分割してモーションを作っています、エクセルシートに説明を加えました。

 
	double  Hsin(double);
	double  Hcos(double);

	double    Pi = 3.141592;
	double   Deg = 180 / Pi;
	double     G = 9.8;      

	int   gImsec = 0;
	int      stc = 0;
	int        n = 0;

	//------------モーション時間設定---------------------------------
	int      dmt =  50;           //モーション間隔(ms)
	int       nc =   6;           //モーション分割回数
	int Max_step =  30;           //歩数(偶数)
	//---------------------------------------------------------------
	double    Ts = (double)dmt *  (double)nc / 1000;//一歩の半分(一歩の時間 = dmt × nc × 2 )

	//---------------ロボット設定------------------------------------
	double    Hc = 0.245;         //重心高さ(m)
	double    kh = 0.950;         //立脚相腰高さ(腰下げ)の設定
	double    Lx = 0.130;         //ピッチ側の関節間隔(m)
	double    Ly = 0.207;         //ロール側の関節間隔(m)
	double   Bsw = 0.056;         //両足の間隔(m)
	//---------------------------------------------------------------
	double    St = 0;             
	double    Sw = 0;             
	double    Uh = 0;             
	double Sw_offset;             
	double   Turning = 0;         

	void Walking(DLL_SIM_ROBOT4 *sim);


//-----------------------------------------------------------------------
void __stdcall Dll4_Run(DLL_SIM_ROBOT4 *sim) {

	//Time count up
	int idt = (int)(sim->dt / 0.001);
	gImsec += idt;

	if ((gImsec % dmt) == 0) {
		stc =   gImsec /(dmt  * nc) + 1;//歩数カウンタ
		  n = ((gImsec / dmt) % nc) + 1;//モーションカウンタ

		St  = 0.100;                    //歩幅設定(m)
		Sw  = Bsw  ;                    //歩隔設定(m)
		Sw_offset = Bsw / 2;            //重心~足の横オフセット(m)

		//歩き始めの歩幅設定例//
		if(stc <=  6)            { St = 0.070;}
		if(stc <=  4)            { St = 0.030;}
		if(stc ==  2)            { St = 0;     Sw = Bsw * 0.5;}
		if(stc ==  1)            { St = 0;     Sw = Bsw * 0.5; Sw_offset = 0.0;}
		//止まる時の歩幅設定例//
		if(stc >= (Max_step - 5)){ St = 0.070;}
		if(stc >= (Max_step - 3)){ St = 0.030;}
		if(stc == (Max_step - 1)){ St = 0;     Sw = Bsw * 0.5;}
		if(stc == (Max_step - 0)){ St = 0;     Sw = Bsw * 0.5; Sw_offset = 0.0;}

		Turning = 0.0/ Deg;             //旋回角の設定(度)
		     Uh = St * 0.15;             //遊脚相足上げ高さの設定

		//DLL終了
		if (stc == Max_step && n==nc){
			sim->bFinish = true;//DLL finish flag
		}

		Walking(sim);                   //歩行モーションへ→
	}
	return;
}

//-----------------------------------------------------------------------
// 歩行モーション
//-----------------------------------------------------------------------
void Walking(DLL_SIM_ROBOT4 *sim){

	//計算用比率
	int i;
	double a[100];
	double b[100];
	for (i = 0; i <= nc; i++) {
		a[i] =               (double)i  / (double)nc;
		b[i] = ((double)nc - (double)i) / (double)nc;
	}

	double  Px;         //立脚足位置ピッチ
	double  Py;         // 〃  ロール
	double  Ux;         //遊脚足位置ピッチ
	double  Uy;         // 〃  ロール
	double  Uref;       //遊脚軌道

	double  toe;        //足首角
	double  Tmp = 2.356;
	double  t;
    double  angle;

	//関節角
	double  c1;  
	double  cf;
	double  q1;         //股ピッチ
	double  q2;         //膝
	double  q3;         //足ピッチ
	double  q4;         //股ロール
	double  q5;         //足ロール
	double  q6;         //腕

	        St = St / 2;
	        Sw = Sw / 2;
	double  Tc = sqrt(Hc / G );
	double   C = Hcos(Ts / Tc);
	double   S = Hsin(Ts / Tc);
	double  Vx = St / ( Tc * S );         //歩速
	double  Zx = sqrt(Lx*Lx - St*St) * kh;//立脚高さピッチ側
	double  Zy = Ly - (Lx - Zx);          //立脚高さロール側
	double  Zu;                           //遊脚高さ
	double  Pl;                           //立脚長
	double  Ul;                           //遊脚長

	int     fp =  stc     & 1;            //前半・後半フラグ
	int     fl = (stc>>1) & 1;            //左足・右足フラグ

	double servoref[20];//関節制御角
	for (i = 1; i <= 18; i++){ servoref[i] = 0; }

	//-----------------------位置の計算-------------------------
	if (fp == 1) {
		 t = (double)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(pow((a[n] / 2 + 0.50), 0.75) * Pi);
		angle = Turning * a[n];
		toe = asin(Uref * Uh / 0.120) * a[n];//踵から着地
	}
	else {
		 t = -(double)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(pow((a[n] / 2), 0.75) * Pi);
		angle = -Turning * b[n];
		toe = -asin(Uref * Uh / 0.120) * b[n];//爪先蹴り
	}

	//-------------------------立脚相の関節角-------------------
	Pl =  sqrt(Zx*Zx + Px*Px);
	   if(Pl > Lx){Pl = Lx;}
	c1 =  acos(Pl / Lx);
	cf = -atan(Px / Zx);
	q1 = cf + c1;
	q2 = c1 * 2;
	q3 = c1 - cf;
	q4 = atan((Py - Sw_offset) / Zy);
	q5 = q4;
	q6 = Px / Zx;

	if (fl == 0) {
		//---RFoot-------------
		servoref[ 1] =  angle * Deg;
		servoref[ 5] =  q1 * Deg;
		servoref[ 7] =  q2 * Deg;
		servoref[ 9] =  q3 * Deg;
		servoref[ 3] =  q4 * Deg;
		servoref[11] =  q5 * Deg;
		servoref[17] =  q6 * Deg;
	}
	else {
		//---LFoot-------------
		servoref[ 2] =  angle * Deg;
		servoref[ 6] =  q1 * Deg;
		servoref[ 8] =  q2 * Deg;
		servoref[10] =  q3 * Deg;
		servoref[ 4] = -q4 * Deg;
		servoref[12] = -q5 * Deg;
		servoref[18] =  q6 * Deg;
	}

	//------------------------游脚相の関節角--------------------
	Zu =  Zx - Uref * Uh;
	Ul =  sqrt(Zu*Zu + Ux*Ux);
	   if(Ul > Lx){Ul = Lx;}
	c1 =  acos(Ul / Lx);
	cf = -atan(Ux / Zu);
	q1 = cf + c1;
	q2 = c1 * 2;
	q3 = c1 - cf + toe;
	q4 = -atan((Uy + Sw_offset) / (Zy - Uref * Uh));
	q5 = q4 * 0.9;
	q6 = -Px / Zx;

	if (fl == 0) {
		//---LFoot-------------
		servoref[ 2] =  -angle * Deg;
		servoref[ 6] =  q1 * Deg;
		servoref[ 8] =  q2 * Deg;
		servoref[10] =  q3 * Deg;
		servoref[ 4] =  q4 * Deg;
		servoref[12] =  q5 * Deg;
		servoref[18] =  q6 * Deg;
	}
	else {
		//---RFoot-------------
		servoref[ 1] =  -angle * Deg;
		servoref[ 5] =  q1 * Deg;
		servoref[ 7] =  q2 * Deg;
		servoref[ 9] =  q3 * Deg;
		servoref[ 3] = -q4 * Deg;
		servoref[11] = -q5 * Deg;
		servoref[17] =  q6 * Deg;
	}

	//---------------脚部角度制御------------------------------------
	for (i = 1; i <=18; i++) { sim->ServoRefDeg[i] = servoref[i]; }
	//---------------------------------------------------------------

}

double  Hsin(double x)
{
	return (exp(x) - exp(-x)) / 2;
}
double Hcos(double x)
{
	return (exp(x) + exp(-x)) / 2;
}

2016年2月26日 (金)

自然な歩行をするKHR-3HV用ロボットモーション

バージョンアップしたので、新しいページをご覧ください。

2016年1月22日 (金)

自然に歩くKHR-3HV用のモーションデータ

 二足歩行ロボットKHR-3HV用の、自然に歩くモーションを作り直しました。 KHR-3HVの足裏の改造のみで、とても自然に歩くモーションですのでぜひお試しください。

  前回のモーションに比べて加減速が安定しています。それと少しゆっくりにしました。(0.45s⇒0.60s)

   足踏み「foot2.0.xml」をダウンロード

   歩行 「walk2.0.xml」をダウンロード 右クリックして「対象をファイルに保存」を選択してください。

  動画はこちら・・・
                 足踏み動画はhttps://youtu.be/9fD4OZ8anrA

               歩行の動画はhttps://youtu.be/TO__fDun_js

調整の手順は、

  ①直立姿勢で足がまっすぐになるように脚部のトリムを調整する。

  ②直立姿勢で前後・左右に揺らしてみて、ジャイロの効果により安定しているか確認する。

  ③足踏モーションが安定するように足首のトリムを調整する。

 ・フレーム周期は15msです。ジャイロはKHR-3のマニュアル通り足首のみに効かせます。

 ・足首トリムの調整には根気が必要です。直立姿勢で、荷重が足裏全体に掛かっているようになるのが理想です。(ZMPを足裏中心にする)

 ・足裏のサイズは60×46です、厚さ5ミリの木片を付けてあります。(今回、足裏接地面はフラットです)

 ・無改造の状態では試していませんが、ソールに厚さ数ミリの板を張り付けるか、足首サーボモーターのトルクを下げれば良いかなと思います。

 ・Go Simulation上で足裏のサイズを60×46にし、ジャイロ無しで安定して歩けるように調整してあるモーションです。 

2015年7月 1日 (水)

自然に歩くKHR-3HV(go simulation用)のモーションデータ

こんばんは、ずいぶんご無沙汰してしまいました。

Go Simulation上のKHR-3HVで、自然な歩行を可能にするモーションデータです。

  ・足のサイズは60×46で調整してあります。

  ・関節角度は100倍になっているので、このままGo Simulation上のデータにコピペしてお試しください。

   データはこちらです「KHR-3_Walk-01.txt」をダウンロード

   動画はこちらですhttps://youtu.be/7b6QJxbmOdk

 実ロボットの方は、データを変換後に動作が確認できましたらアップしようと思っていますのでよろしくお願いします。