二次元の逆運動学 | Python シミュレーション

二次元の逆運動学はPythonで実装してみました。本記事でアルゴリズムとプログラムを解説します。
#####依存

• Python
• Numpy
• Matplotlib

以下プログラムの流れを図で発表しています。最初は目標点(上記絵の赤い点)とジョイト角度を初期する。

# 極座標で目標点を初期
pr = 1
p_th = np.pi
Dp = np.array([pr*np.cos(p_th), pr*np.sin(p_th)]) 

関節角度を順運動学関数（forw_kin）に入力すると終点の位置が出る。目標点と終点位置で絶対誤差をとってｋパラメータエラー(Perror)と定義された閾値と比べる。閾値と比べてエラーが大きい場合逆ヤコビ行列とエラーの乗算でジョイトの回転速度を与える。次は更新節角度の為前の角度と回転速度を組み合わせてまた順運動学をやってエラーを見る。

# 順運動学関数：入力（節角度　th と　ジョイトの間の距離 l）　
def forw_kin(th,l):
    l1 = l[0]
    l2 = l[1]
    th1 = th[0]
    pos = np.array([l2*np.cos(th.sum())+l1*np.cos(th1),l2*np.sin(th.sum())+l1*np.sin(th1)])
    return pos

# ヤコビ行列関数：入力（節角度　th と　ジョイトイの間の距離 l）　
def Jacob(th,l):

    l1 = l[0]
    l2 = l[1]
    th1 = th[0]
    th2 = th[1]

    J = np.array([[-l1*np.sin(th1)-l2*np.sin(th1+th2), -l2*np.sin(th1+th2)],
        [l1*np.cos(th1)+l2*np.cos(th1+th2), l2*np.cos(th1+th2)]])
    
    return J

#####注意
時間パラメータ（dt）とエラーパラメータ（ｋ）を十分に小さい値で定義しないと逆ヤコビ行列が発散することが起きる。

全てのシミュレーションプログラムをGithubで投稿しているので興味があれば是非下のリンクを見て下さい。
--- https://github.com/mymultiverse/robot/tree/master/python