事前に必要な知識（見ておいた方がいい記事）

お疲れ様です。秋並です。

この記事では、「ヤコビ行列を用いた数値解法による逆運動学」について解説します。

はじめに

前回は、解析的解法による逆運動学について紹介しました。

解析的解法は、手先の座標を代入するだけで一瞬で解が求められ、高速に動作します。

一方で、ロボットアームの構造が複雑になると、数式が存在しないようなパターンも存在します。

そこで、今回はヤコビ行列を用いた数値解法による逆運動学について解説します。

本記事内のコードはすべてgoogle colabratory上でも動作します（2024/2/24現在）。

⚠️

本記事に出てくるコードはnotebook上で動作させる（「ipynb」ファイル）ことを前提としているため「py」ファイルでは一部のコードは動作しないのでご注意ください（主に描画関連）

ヤコビ行列による逆運動学のイメージ

🔍

本記事では、順運動学により「関節角度」→「手先位置」を導出する方法がわかっている前提で話をすすめます。順運動学については、以下記事を参照ください。

【図解】順運動学【概要】

【図解】順運動学（三角関数）【pythonコード付き】

【図解】順運動学（同次変換行列）【pythonコード付き】

ヤコビ行列による逆運動学は、（2次元平面上の2軸ロボットを例にすると）下記のような手順になります

目標手先 $\vec{P}_{\mathrm{goal}} = \begin{bmatrix}x_{\mathrm{goal}} \\ y_{\mathrm{goal}} \end{bmatrix}$ を設定
現在手先 $\vec{P}_{\mathrm{current}} = \begin{bmatrix}x_{\mathrm{current}} \\ y_{\mathrm{current}} \end{bmatrix}$ が、目標手先 $\vec{P}_{\mathrm{goal}}$ 方向に $\Delta\vec{P} = \begin{bmatrix}\Delta x \\ \Delta y \end{bmatrix}$ だけ移動するように、現在関度 $\vec{Q}_{\mathrm{current}} = \begin{bmatrix}\theta_{1_\mathrm{current}} \\ \theta_{2_\mathrm{current}} \end{bmatrix}$ を $\Delta\vec{Q} = \begin{bmatrix}\Delta\theta_{1} \\ \Delta\theta_{2} \end{bmatrix}$ だけ動かす
2.の手順を繰り返し、現在手先 $\vec{P}_{\mathrm{current}}$ が目標手先 $\vec{P}_{\mathrm{goal}}$ に十分近づいたら終了

⚠️

上動画は、説明のために動画として描画していますが、実際はコンピュータ上で高速に計算するため、描画をしなければもっと高速に解が求まります。

ここで、それぞれの変数について考えてみましょう。

まず、目標手先 $\vec{P}_{\mathrm{goal}}$ は自身で設定するため、既知であることは明らかです。

次に、現在角度 $\vec{Q}_{\mathrm{current}}$ は一般に各関節のエンコーダから取得することができるので、こちらも既知です。

現在手先 $\vec{P}_{\mathrm{current}}$ は、現在角度 $\vec{Q}_{\mathrm{current}}$ が分かっていれば、順運動学によって求めることができます。

$\Delta\vec{P}$ も、現在手先 $\vec{P}_{\mathrm{current}}$ と目標手先 $\vec{P}_{\mathrm{goal}}$ が分かっていれば、式(1)で求めることができます。

\Delta\vec{P} = P_{\mathrm{param}}(\vec{P}_{\mathrm{goal}} - \vec{P}_{\mathrm{current}} ) \tag{1}

ここで $P_{\mathrm{param}}$ は、「どの程度微小に手先を動かすか」を決めるパラメータで「0.01～0.1」付近の値にすることが多いです。

最後に $\Delta\vec{Q}$ ですが、これは上記の値ほど簡単に求めることができません。

微小角度 $\Delta\vec{Q}$ を求める

ここで、 $\Delta\vec{Q}$ と $\Delta\vec{P}$ につい考えてみます。これはそれぞれ

微小「角度」
「手先位置」の微小変化量

であるため、順運動学、逆運動学と同様に

微小「角度」→「手先位置」の微小変化量
「手先位置」の微小変化量→微小「角度」

と互いに変換することができそうです。

すなわち、 $\Delta\vec{Q}$ と $\Delta\vec{P}$ を結びつけるような数式（ここでは行列）が存在すれば良いわけです。

さて、微小変化といえば「微分」です。

式(2)(3)は、2次元平面における2リンクロボットアームの順運動学の式ですが、それぞれの式の「手先位置 $\vec{P}(t)$ 」と「各関節角度 $\vec{Q}(t)$ 」は時間の関数なので、両辺を時間微分することで「微小変化」の関係性を導くことができそうです。

🔍

他の箇所では省略していますが、ここでは「時間の関数」とわかりやすいようにあえて

x(t),\theta_1(t)

のような書き方をしています

x(t) = L_1\cos(\theta_{1}(t)) + L_2\cos(\theta_1(t)+\theta_2(t)) \tag{2}

y(t) = L_1\sin(\theta_{1}(t)) + L_2\sin(\theta_1(t)+\theta_2(t)) \tag{3}

ここで、式(2)(3)の両辺を時間微分すると式(2’)(3’)になり

速度（=微小時間あたりの「手先位置の微小変化量」）
角速度（＝微小時間あたりの「微小角度」）

の関係になります（式が見にくくなるので、以降も $(t)$ は省略しています）。

\begin{aligned} \frac{dx}{dt} &= -L_1\sin (\theta_1)\frac{d\theta_{1}}{dt} - L_2\sin(\theta_1+\theta_2)\left(\frac{d\theta_1}{dt}+\frac{d\theta_2}{dt}\right) \\ &= (-L_1\sin (\theta_1)- L_2\sin(\theta_1+\theta_2))\frac{d\theta_{1}}{dt} - L_2\sin(\theta_1+\theta_2)+\frac{d\theta_2}{dt} \end{aligned} \tag{2'}

\begin{aligned} \frac{dy}{dt} &= L_1\cos(\theta_1)\frac{d\theta_{1}}{dt} + L_2\cos(\theta_1+\theta_2)\left(\frac{d\theta_1}{dt}+\frac{d\theta_2}{dt} \right)\\ &= (L_1\cos(\theta_1)+L_2\cos(\theta_1+\theta_2))\frac{d\theta_{1}}{dt} + L_2\cos(\theta_1+\theta_2)\frac{d\theta_2}{dt} \end{aligned} \tag{3'}

今回は「微小変化」についてのみ知りたいので式(2’)(3’)の両辺に $dt$ をかけ、 $dt$ を消します（式(2’’)(3’’)）。

dx= (-L_1\sin (\theta_1)- L_2\sin(\theta_1+\theta_2))d\theta_{1} - L_2\sin(\theta_1+\theta_2)d\theta_2(t) \tag{2''}

dy = (L_1\cos(\theta_1)+L_2\cos(\theta_1+\theta_2))d\theta_{1} + L_2\cos(\theta_1+\theta_2)d\theta_2 \tag{3''}

これで、 $dx, dy, d\theta$ の関係になりましたが、今回求めたいような「目に見える微小変化」は、一般に $d$ ではなく $\Delta$ で表現するため、 $d$ を $\Delta$ に置き換えると式(2’’’)(3’’’)になります。

\Delta x = (-L_1\sin (\theta_1)- L_2\sin(\theta_1+\theta_2))\Delta\theta_{1} - L_2\sin(\theta_1+\theta_2)\Delta\theta_2\tag{2'''}

\Delta y = (L_1\cos(\theta_1)+L_2\cos(\theta_1+\theta_2))\Delta\theta_{1} + L_2\cos(\theta_1+\theta_2)\Delta\theta_2\tag{3'''}

最後に、式(2’’’)(3’’’)を行列の形に表現し直すと式(4)のようになり、 $\Delta\vec{Q}$ を $\Delta\vec{P}$ に変換するような行列ができました。

\begin{aligned} \begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} &= \begin{bmatrix} (-L_1\sin (\theta_1)- L_2\sin(\theta_1+\theta_2))\Delta\theta_{1} - L_2\sin(\theta_1+\theta_2)\Delta\theta_2 \\ (L_1\cos(\theta_1)+L_2\cos(\theta_1+\theta_2))\Delta\theta_{1} + L_2\cos(\theta_1+\theta_2)\Delta\theta_2 \end{bmatrix} \\ &= \begin{bmatrix} -L_1\sin(\theta_1) - L_2\sin(\theta_1+\theta_2) & -L_2\sin(\theta_1+\theta_2)\\ L_1\cos(\theta_1) +L_2\cos(\theta_1+\theta_2) & L_2\cos(\theta_1+\theta_2) \end{bmatrix} \begin{bmatrix} \Delta\theta_1 \\ \Delta\theta_2 \end{bmatrix}\\ \Delta\vec{P} &= J\Delta \vec{Q} \end{aligned} \tag{4}

この $\Delta\vec{Q}$ と $\Delta\vec{P}$ を関連付ける役割を持つ行列を「ヤコビ行列 $J$ 」といいます。

🔍

ヤコビ行列

J

を一般化すると式(5)のようになり、順運動学の式を偏微分することで求めることができます。

J = \begin{bmatrix} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} &\cdots& \frac{\partial f_1}{\partial \theta_m}\\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} &\cdots& \frac{\partial f_2}{\partial \theta_m} \\ \vdots & \vdots & \vdots & \vdots\\ \frac{\partial f_n}{\partial \theta_1} & \frac{\partial f_n}{\partial \theta_2} &\cdots& \frac{\partial f_n}{\partial \theta_m} \end{bmatrix} \tag{5}

🔍

\vec{f} = \begin{bmatrix} f_1 \\ f_2 \\ \vdots\\ f_n \end{bmatrix}

：順運動学の式のベクトル

m

: 手先の自由度

n

: 関節数

今回の場合、 $\vec{f}$ に式(1)(2)を当てはめると、 $\vec{f}= \begin{bmatrix} f_1\\ f_2 \end{bmatrix} = \begin{bmatrix} L_1\cos(\theta_{1}) + L_2\cos(\theta_1+\theta_2)\\ L_1\sin(\theta_{1}) + L_2\sin(\theta_1+\theta_2) \end{bmatrix}$ になり、

\begin{aligned} J &= \begin{bmatrix} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2}\\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} \\ \end{bmatrix} &= \begin{bmatrix} -L_1\sin(\theta_1) - L_2\sin(\theta_1+\theta_2) & -L_2\sin(\theta_1+\theta_2)\\ L_1\cos(\theta_1) +L_2\cos(\theta_1+\theta_2) & L_2\cos(\theta_1+\theta_2) \\ \end{bmatrix} \end{aligned}

となるため、式(4)と同じ結果が得られます

ここまでで、 $\Delta\vec{Q}$ と $\Delta\vec{P}$ を関連付ける事ができましたが、式(4)は

「微小角度」→「手先位置の微小変化量」

を求める式です。

今回求めたいのは

「手先位置の微小変化量」→「微小角度」

を求める式なので、式(4)をもう一段階変換しましょう。

式(4)の両辺に「ヤコビ行列の逆行列 $J^{-1}$ 」をかけると式(6)になり、 $\Delta \vec{Q}$ を求めることができました。

\begin{aligned} \Delta\vec{P} &= J\Delta \vec{Q} \\ J^{-1}\Delta\vec{P} &= J^{-1}J\Delta \vec{Q} \\ J^{-1}\Delta\vec{P} &= \Delta \vec{Q} \end{aligned} \tag{6}

🔍

「ヤコビ行列

J

」が正方行列でない場合は逆行列を求めることができません例えば、関節の数と手先の自由度が異なる場合は正方行列になりません。そのような場合は、「擬似逆行列

J^{\#}

」とよばれる行列を使用することで、逆行列と同様の振る舞いをすることが可能です。ここでは、「擬似逆行列

J^{\#}

」は 逆行列が作れない場合に使う便利な行列 という程度の説明に留めます。導出方法は以下サイトなどを参照ください。いまさら聞けないMoore-Penroseの一般化逆行列（疑似逆行列）の話 #C - Qiita

2次元平面における2リンクアームのヤコビ行列による逆運動学の具体的な手順

ここまでで、計算に必要な変数はすべて求めることができたので、ここからは「ヤコビ行列による逆運動学のイメージ」で述べた手順をもっと具体的に説明します。

「目標手先位置 $\vec{P}_{\mathrm{goal}}$ 」を決定します。

「現在手先位置 $\vec{P}_{\mathrm{current}}$ 」から「目標手先位置 $\vec{P}_{\mathrm{goal}}$ 」方向へ向かう「手先の微小移動量 $\Delta\vec{ P}$ 」を式(1)によって求めます。

\Delta\vec{P} = P_{\mathrm{param}}(\vec{P}_{\mathrm{goal}} - \vec{P}_{\mathrm{current}} ) \tag{1:再掲}

式(5)により「ヤコビ行列 $J$ 」を求めます。

J = \begin{bmatrix} \frac{\partial f_1}{\partial \theta_1} & \frac{\partial f_1}{\partial \theta_2} &\cdots& \frac{\partial f_1}{\partial \theta_m}\\ \frac{\partial f_2}{\partial \theta_1} & \frac{\partial f_2}{\partial \theta_2} &\cdots& \frac{\partial f_2}{\partial \theta_m} \\ \vdots & \vdots & \vdots & \vdots\\ \frac{\partial f_n}{\partial \theta_1} & \frac{\partial f_n}{\partial \theta_2} &\cdots& \frac{\partial f_n}{\partial \theta_m} \end{bmatrix} \tag{5:再掲}

「ヤコビ行列の逆行列 $J^{-1}$ （正方行列でない場合は、疑似逆行列 $J^{\#}$ ）」と「手先の微小移動量 $\Delta\vec{P}$ 」を用いて「各関節の手先微小量 $\Delta \vec{Q}$ 」を式(6)により求めます。

\Delta \vec{Q} = J^{-1}\Delta\vec{P} \tag{6:再掲}

現在の各関節角度 $\vec{Q}_{\mathrm{current}}$ を「微小角度 $\Delta\vec{ Q}$ 」だけ動かします（式7）。

\vec{Q}_{\mathrm{new}} = \vec{Q}_{\mathrm{current}} +\Delta\vec{ Q} \tag{7}

更新後の各関節角度 $\vec{Q}_{\mathrm{new}}$ を用いて順運動学により、新たな手先位置 $\vec{P}_{\mathrm{new}}$ を求めます（式(8)）。

\vec{P}_{\mathrm{new}} = \vec{f}(\vec{Q}_{\mathrm{new}}) \tag{8}

現在の各関節角度 $\vec{Q}_{\mathrm{current}}$ 、現在の手先位置 $\vec{P}_{\mathrm{current}}$ を更新します（式(9)(10)）。

\vec{Q}_{\mathrm{current}} = \vec{Q}_{\mathrm{new}} \tag{9}

\vec{P}_{\mathrm{current}} = \vec{P}_{\mathrm{new}} \tag{10}

「現在手先位置 $\vec{P}_{\mathrm{current}}$ 」が「目標手先位置 $\vec{P}_{\mathrm{goal}}$ 」に十分近づくまで2.～7.の計算を繰り返します。

コード

上記の手順をpythonで記述した例が以下のようになります。

コードを実行すると下図のような「初期姿勢」と「最終姿勢」が描画されます。

🔍

「初期設定（この箇所を変更すると、結果が変わります）」の箇所を変更すると結果が変わります

import math
import numpy as np
import matplotlib.pyplot as plt

def forward_kinematics2d(link_length, theta):
    """
    2次元平面における（三角関数による）順運動学を求める
    Parameters
    ----------
    link_length : float
        リンクの長さ
    theta : float
        回転角度(rad)

    Returns
    -------
    x: リンク先端座標(x)
    y: リンク先端座標(y)
    """
    x = link_length*np.cos(theta)
    y = link_length*np.sin(theta)
    return x, y

def make_jacobian_matrix(link1_length, theta1, link2_length, theta2):
    """
    2次元平面における2リンクアームのヤコビ行列を求める
    J = [[∂x/∂θ1, ∂x/∂θ2],
         [∂y/∂θ1, ∂y/∂θ2]]

    Parameters
    ----------
    link1_length : float
        リンク1の長さ
    link2_length : float
        リンク2の長さ
    theta1 : float
        リンク1の回転角度(rad)
    theta2 : float
        リンク2の回転角度(rad)

    Returns
    -------
    J : numpy.ndarray
        2次元平面における2リンクアームのヤコビ行列
    """

    J = np.array([[-link1_length*math.sin(theta1) - link2_length*math.sin(theta1+theta2), -link2_length*math.sin(theta1+theta2)],
                  [ link1_length*math.cos(theta1) + link2_length*math.cos(theta1+theta2),  link2_length*math.cos(theta1+theta2)]])
    return J

def make_trans_matrix(mat):#転置行列2×2
    """
    2×2行列の転置行列を求める

    Parameters
    ----------
    mat: numpy.ndarray
         2×2行列

    Returns
    -------
    trans_mat : numpy.ndarray
                2次元平面における2リンクアームのヤコビ行列
    """
    mat_minus =  np.array([[mat[0,0], mat[1,0]],
                           [mat[0,1], mat[1,1]]])
    return mat_minus

def make_inverse_matrix(mat):
    """
    2×2行列の逆行列を求める

    Parameters
    ----------
    mat: numpy.ndarray
         2×2行列

    Returns
    -------
    inverse_mat : numpy.ndarray
                  2×2行列の逆行列
    """
    inverse_mat = 1/(mat[0,0]*mat[1,1] - mat[0,1]*mat[1,0]) * np.array([[mat[1,1], -mat[0,1]],
                                                                        [-mat[1,0], mat[0,0]]])
    return inverse_mat

def make_pseudo_inverse_matrix(mat):
    """
    疑似逆行列を求める

    Parameters
    ----------
    mat: numpy.ndarray
         行列

    Returns
    -------
    inverse_mat : numpy.ndarray
                  疑似逆行列
    """
    trans_mat = make_trans_matrix(mat)
    pseudo_inverse_mat = make_inverse_matrix(trans_mat @ mat) @ trans_mat
    return pseudo_inverse_mat

# 初期設定（この箇所を変更すると、結果が変わります） ========================
# 最大繰り返し回数
max_loop_num = 100
# 各リンクの長さ
link1_length = 2.0
link2_length = 2.0
# 各リンクの初期角度（deg）
theta1_deg = 30
theta2_deg = 30
# 目標手先位置 ※実現できない位置を指定すると解が求まらないので注意
xe_goal = -3.0
ye_goal = 2.0
P_goal = np.array([[xe_goal],
                    [ye_goal]]) # （初期）目標手先位置P_goal（ここは変更しない）
# 手先位置Pを各ループごとにどの程度動かすかを決める定数（値が大きいほど早く収束しようとするが安定しない、小さいほど収束がゆっくりだが安定する）
P_delta_param = 0.1
# ゴールしたと判定する閾値(P_currentとP_goalの距離がどの程度まで近づいたらゴールとみなすか)
goal_dis = 0.01
# ==========================================================================

# （初期）関節角度Q_currentを設定
theta1_rad = theta1_deg * math.pi/180
theta2_rad = theta2_deg * math.pi/180
theta1_current = theta1_rad
theta2_current = theta2_rad
Q_current = np.array([[theta1_current],
                      [theta2_current]])

# 三角関数による順運動学を用いて（初期）手先位置P_currentを計算
x1, y1 = 0, 0
# link1の根本から見た時の，link1の先端位置
x1_to_2, y1_to_2 = forward_kinematics2d(link1_length, Q_current[0][0])

# link2の根本から見た時の，link2の先端位置（=エンドエフェクタの位置）
x2_to_e, y2_to_e = forward_kinematics2d(link2_length, Q_current[0][0]+Q_current[1][0])

# link1の根本（原点座標）から見た時の，エンドエフェクタの位置
x2 = x1 + x1_to_2
y2 = y1 + y1_to_2
xe = x1 + x2 + x2_to_e
ye = y1 + y2 + y2_to_e

P_current = np.array([[xe],
                      [ye]])

# 描画用に初期値を保存 =================
x2_init = x2
y2_init = y2
xe_init = xe
ye_init = ye
# ========================================

for i in range(max_loop_num):
    # ヤコビ行列を計算
    jacobian = make_jacobian_matrix(link1_length, Q_current[0][0], link2_length, Q_current[1][0]) # ヤコビ行列J
    jacobian_trans = make_trans_matrix(jacobian) # ヤコビ行列の転置行列 J^T
    jacobian_inverse = make_inverse_matrix(jacobian) # ヤコビ行列の逆行列 J^{-1}
    # jacobian_inverse = make_pseudo_inverse_matrix(jacobian) # 擬似逆行列（今回の場合、ヤコビ行列が2×2の正方行列なので、逆行列/擬似逆行列 のどちらを使用しても良い（どちらを使用しても同じ行列が求まる））

    # 現在の手先P_current→目標手先P_goal 方向のベクトル
    P_current_to_P_goal = P_goal - P_current

    # 現在の手先P_current→目標手先P_goal 方向に向かう微小量ΔP
    P_delta = P_current_to_P_goal * P_delta_param

    # ΔP移動するために必要な「各関節の、微小角度ΔQ」を計算
    Q_delta = jacobian_inverse @ P_delta

    # ΔQだけ各関節を動かす
    Q_new = Q_current + Q_delta
    Q_new = Q_new % (2*math.pi) #（関節角度が0～2πの間に収まるようにするための処理）

    # 更新後の関節角度Q_newを用いて、手先位置P_newを計算（三角関数による順運動学）
    # link1の根本から見た時の，link1の先端位置
    x1_to_2, y1_to_2 = forward_kinematics2d(link1_length, Q_new[0][0])

    # link2の根本から見た時の，link2の先端位置（=エンドエフェクタの位置）
    x2_to_e, y2_to_e = forward_kinematics2d(link2_length, Q_new[0][0]+Q_new[1][0])

    # link1の根本（原点座標）から見た時の，エンドエフェクタの位置
    x2 = x1 + x1_to_2
    y2 = y1 + y1_to_2
    xe = x1 + x2 + x2_to_e
    ye = y1 + y2 + y2_to_e
    P_new = np.array([[xe],
                      [ye]])
    print(i,"P_new = [",P_new[0][0],",", P_new[1][0], "]")

    # 現在関節角度Q_current、現在手先位置P_currentを更新
    Q_current = Q_new
    P_current = P_new

    # 目標手先P_goalに到達したら終了
    if (P_goal[0][0]-P_current[0][0])**2  + (P_goal[1][0]-P_current[1][0])**2 <  goal_dis**2: # 2点間の距離の公式を使用（ただし、sqrt関数は処理が重いので、両辺を2乗した値で比較）
        break

# 繰り返し回数 が ループの最大回数 の場合、解が見つからなかったとして終了
if i == max_loop_num-1:
    print("位置を計算できませんでした（特異点，もしくは実現不可能な座標の可能性があります）")


# --------------- 以下、描画関連 --------------------------------------------------------
# リンクが画面内に収まるように設定
fig = plt.figure(figsize=(10,5))
# 初期姿勢
ax_init = fig.add_subplot(1, 2, 1)
ax_init.set_xlim(-(link1_length+link2_length), link1_length+link2_length)
ax_init.set_ylim(-(link1_length+link2_length), link1_length+link2_length)
ax_init.set_title("init pose")

# 最終姿勢
ax_goal = fig.add_subplot(1, 2, 2)
ax_goal.set_xlim(-(link1_length+link2_length), link1_length+link2_length)
ax_goal.set_ylim(-(link1_length+link2_length), link1_length+link2_length)
ax_goal.set_title("goal pose")


# 各linkを描画し、表示
# 初期姿勢
ax_init.plot([x1, x2_init], [y1, y2_init], color="tomato") # link1の描画
ax_init.plot([x2_init, xe_init], [y2_init, ye_init], color="lightgreen") # link2の描画

# 最終姿勢
x2 = x1 + x1_to_2
y2 = y1 + y1_to_2
ax_goal.plot([x1, x2], [y1, y2], color="tomato") # link1の描画
ax_goal.plot([x2, xe], [y2, ye], color="lightgreen") # link2の描画
plt.show()

アニメーション

以下コードを実行すると、アニメーションで初期姿勢→目標姿勢に推移する様子が可視化できます。

🔍

「初期設定（この箇所を変更すると、結果が変わります）」の箇所を変更すると結果が変わります

import math
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTML

def forward_kinematics2d(link_length, theta):
    """
    2次元平面における（三角関数による）順運動学を求める
    Parameters
    ----------
    link_length : float
        リンクの長さ
    theta : float
        回転角度(rad)

    Returns
    -------
    x: リンク先端座標(x)
    y: リンク先端座標(y)
    """
    x = link_length*np.cos(theta)
    y = link_length*np.sin(theta)
    return x, y

def make_jacobian_matrix(link1_length, theta1, link2_length, theta2):
    """
    2次元平面における2リンクアームのヤコビ行列を求める
    J = [[∂x/∂θ1, ∂x/∂θ2],
         [∂y/∂θ1, ∂y/∂θ2]]

    Parameters
    ----------
    link1_length : float
        リンク1の長さ
    link2_length : float
        リンク2の長さ
    theta1 : float
        リンク1の回転角度(rad)
    theta2 : float
        リンク2の回転角度(rad)

    Returns
    -------
    J : numpy.ndarray
        2次元平面における2リンクアームのヤコビ行列
    """

    J = np.array([[-link1_length*math.sin(theta1) - link2_length*math.sin(theta1+theta2), -link2_length*math.sin(theta1+theta2)],
                  [ link1_length*math.cos(theta1) + link2_length*math.cos(theta1+theta2),  link2_length*math.cos(theta1+theta2)]])
    return J

def make_trans_matrix(mat):#転置行列2×2
    """
    2×2行列の転置行列を求める

    Parameters
    ----------
    mat: numpy.ndarray
         2×2行列

    Returns
    -------
    trans_mat : numpy.ndarray
                2次元平面における2リンクアームのヤコビ行列
    """
    mat_minus =  np.array([[mat[0,0], mat[1,0]],
                           [mat[0,1], mat[1,1]]])
    return mat_minus

def make_inverse_matrix(mat):
    """
    2×2行列の逆行列を求める

    Parameters
    ----------
    mat: numpy.ndarray
         2×2行列

    Returns
    -------
    inverse_mat : numpy.ndarray
                  2×2行列の逆行列
    """
    inverse_mat = 1/(mat[0,0]*mat[1,1] - mat[0,1]*mat[1,0]) * np.array([[mat[1,1], -mat[0,1]],
                                                                        [-mat[1,0], mat[0,0]]])
    return inverse_mat

def make_pseudo_inverse_matrix(mat):
    """
    疑似逆行列を求める

    Parameters
    ----------
    mat: numpy.ndarray
         行列

    Returns
    -------
    inverse_mat : numpy.ndarray
                  疑似逆行列
    """
    trans_mat = make_trans_matrix(mat)
    pseudo_inverse_mat = make_inverse_matrix(trans_mat @ mat) @ trans_mat
    return pseudo_inverse_mat

# =============== アニメーション関連 ===========================================
def plot_robot_arm(ax, x1, y1, x2, y2, xe, ye):
  ax.plot([x1, x2], [y1, y2], color="tomato") # link1の描画
  ax.plot([x2, xe], [y2, ye], color="lightgreen") # link2の描画
# ==============================================================================

# 初期設定（この箇所を変更すると、結果が変わります） ========================
# 最大繰り返し回数
max_loop_num = 100
# 各リンクの長さ
link1_length = 2.0
link2_length = 2.0
# 各リンクの初期角度（deg）
theta1_deg = 30
theta2_deg = 30
# 目標手先位置 ※実現できない位置を指定すると解が求まらないので注意
xe_goal = -3.0
ye_goal = 2.0
P_goal = np.array([[xe_goal],
                    [ye_goal]]) # （初期）目標手先位置P_goal（ここは変更しない）
# 手先位置Pを各ループごとにどの程度動かすかを決める定数（値が大きいほど早く収束しようとするが安定しない、小さいほど収束がゆっくりだが安定する）
P_delta_param = 0.1
# ゴールしたと判定する閾値(P_currentとP_goalの距離がどの程度まで近づいたらゴールとみなすか)
goal_dis = 0.01
# ==========================================================================

# （初期）関節角度Q_currentを設定
theta1_rad = theta1_deg * math.pi/180
theta2_rad = theta2_deg * math.pi/180
theta1_current = theta1_rad
theta2_current = theta2_rad
Q_current = np.array([[theta1_current],
                      [theta2_current]])

# 三角関数による順運動学を用いて（初期）手先位置P_currentを計算
x1, y1 = 0, 0
# link1の根本から見た時の，link1の先端位置
x1_to_2, y1_to_2 = forward_kinematics2d(link1_length, Q_current[0][0])

# link2の根本から見た時の，link2の先端位置（=エンドエフェクタの位置）
x2_to_e, y2_to_e = forward_kinematics2d(link2_length, Q_current[0][0]+Q_current[1][0])

# link1の根本（原点座標）から見た時の，エンドエフェクタの位置
x2 = x1 + x1_to_2
y2 = y1 + y1_to_2
xe = x1 + x2 + x2_to_e
ye = y1 + y2 + y2_to_e

P_current = np.array([[xe],
                      [ye]])

# ==================アニメーション関連 =====================================
theta1_list = [theta1_rad] # 現在地のリスト（描画用）
theta2_list = [theta2_rad] # 現在地のリスト（描画用）
x2 = x1 + x1_to_2
y2 = y1 + y1_to_2
x2_list = [x2]
y2_list = [y2]
xe_list = [xe]
ye_list = [ye]
# ========================================================================== 

for i in range(max_loop_num):
    # ヤコビ行列を計算
    jacobian = make_jacobian_matrix(link1_length, Q_current[0][0], link2_length, Q_current[1][0]) # ヤコビ行列J
    jacobian_trans = make_trans_matrix(jacobian) # ヤコビ行列の転置行列 J^T
    jacobian_inverse = make_inverse_matrix(jacobian) # ヤコビ行列の逆行列 J^{-1}
    # jacobian_inverse = make_pseudo_inverse_matrix(jacobian) # 擬似逆行列（今回の場合、ヤコビ行列が2×2の正方行列なので、逆行列/擬似逆行列 のどちらを使用しても良い（どちらを使用しても同じ行列が求まる））

    # 現在の手先P_current→目標手先P_goal 方向のベクトル
    P_current_to_P_goal = P_goal - P_current

    # 現在の手先P_current→目標手先P_goal 方向に向かう微小量ΔP
    P_delta = P_current_to_P_goal * P_delta_param

    # ΔP移動するために必要な「各関節の、微小角度ΔQ」を計算
    Q_delta = jacobian_inverse @ P_delta

    # ΔQだけ各関節を動かす
    Q_new = Q_current + Q_delta
    Q_new = Q_new % (2*math.pi) #（関節角度が0～2πの間に収まるようにするための処理）

    # 更新後の関節角度Q_newを用いて、手先位置P_newを計算（三角関数による順運動学）
    # link1の根本から見た時の，link1の先端位置
    x1_to_2, y1_to_2 = forward_kinematics2d(link1_length, Q_new[0][0])

    # link2の根本から見た時の，link2の先端位置（=エンドエフェクタの位置）
    x2_to_e, y2_to_e = forward_kinematics2d(link2_length, Q_new[0][0]+Q_new[1][0])


    # link1の根本（原点座標）から見た時の，エンドエフェクタの位置
    x2 = x1 + x1_to_2
    y2 = y1 + y1_to_2
    xe = x1 + x2 + x2_to_e
    ye = y1 + y2 + y2_to_e
    P_new = np.array([[xe],
                      [ye]])

    # アニメーション用 =====================================================
    theta1_list.append(Q_new[0][0]) # 現在地のリスト（描画用）
    theta2_list.append(Q_new[1][0]) # 現在地のリスト（描画用）
    x2_list.append(x2)
    y2_list.append(y2)
    xe_list.append(xe)
    ye_list.append(ye)
    # ======================================================================

    # 現在関節角度Q_current、現在手先位置P_currentを更新
    Q_current = Q_new
    P_current = P_new

    # 目標手先P_goalに到達したら終了
    if (P_goal[0][0]-P_current[0][0])**2  + (P_goal[1][0]-P_current[1][0])**2 <  goal_dis**2: # 2点間の距離の公式を使用（ただし、sqrt関数は処理が重いので、両辺を2乗した値で比較）
        break

# 繰り返し回数 が ループの最大回数 の場合、解が見つからなかったとして終了
if i == max_loop_num-1:
    print("位置を計算できませんでした（特異点，もしくは実現不可能な座標の可能性があります）")


# =============== 以下、アニメーション関連 ==================================
# リンクが画面内に収まるように設定
fig = plt.figure(figsize=(5,5))
ax = fig.add_subplot(1, 1, 1)

# 各フレーム毎の描画処理
def update(time):
    ax.cla()
    ax.set_xlim(-(link1_length+link2_length), link1_length+link2_length)
    ax.set_ylim(-(link1_length+link2_length), link1_length+link2_length)
    plot_robot_arm(ax, x1, y1, x2_list[time], y2_list[time], xe_list[time], ye_list[time])
    plt.suptitle('theta1={:.3g}, theta2={:.3g}, x={:.3g}, y={:.3g}'.format(math.degrees(theta1_list[time]), math.degrees(theta2_list[time]), xe_list[time], ye_list[time]))

# アニメーション化
ani = FuncAnimation(fig, update, interval=50, frames=len(x2_list))
HTML(ani.to_jshtml()) # HTMLに
# ani.save('inverse_kinematics_jacobian.mp4', writer="ffmpeg") # mp4で保存．これを実行すると処理時間が増加します

まとめと基礎ヤコビ行列について

今回は

ヤコビ行列を用いた数値解法による逆運動学

について解説しました。

ヤコビ行列を用いて繰り返し計算をすることで、「手先位置」→「各関節角度」を近似的に求めることができます。　

一方で、ロボットアームの構成が複雑になるとヤコビ行列もすごく複雑になることがあります。

そこで、基礎ヤコビ行列という行列を使用することがあります。基礎ヤコビ行列はヤコビ行列と比較して簡易的な記述が可能です。

次回は、この基礎ヤコビ行列について解説します。

以上で，「ヤコビ行列を用いた数値解法による逆運動学」の説明は終わりになります．ロボティクスに興味のある方のお役に少しでも立てたら幸いです．

ロボティクスの辞書　記事一覧

ロボットアーム関連

記事タイトル
【図解】順運動学【概要】
【図解】順運動学（三角関数）【pythonコード付き】
【図解】逆運動学（解析的解法）【pythonコード付き】
【図解】順運動学（同次変換行列）【pythonコード付き】
【図解】ヤコビ行列を用いた数値解法による逆運動学【pythonコード付】
【図解】タスク空間におけるロボットアームの動作計画【pythonコード付】
【図解】ロボットアームの動作計画【概要】

移動ロボット関連

記事タイトル
占有格子地図　徹底解説【図解】

衝突判定

画像処理

音声処理

機械学習

センサ

Robot Operating system(ROS)

【図解】ヤコビ行列を用いた数値解法による逆運動学【pythonコード付】