2軸ロボットアーム Part2 (逆運動学)

# はじめに
私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
前記事では，2軸ロボットアームの順運動学(ロボットの関節角度からロボットの手先位置を算出する) を説明した．
(https://qiita.com/haruhiro1020/items/4529e648161beac7754f)
最終的にはロボットアームにRRT + python-fcl を合体させたいと考えている．
本記事では，2軸ロボットアームの逆運動学を説明する．
逆運動学とは，手先位置から関節角度を計算することです．
関節角度を算出するために，三角関数(sin, cos, tan)を使用する．以下サイトはわかりやすいです．
・https://www.meikogijuku.jp/meiko-plus/study/trigonometric-function.html

## 本記事で実装すること
・2軸ロボットアームの逆運動学

## 本記事では実装できないこと (将来実装したい内容)
・2軸ロボットアームに経路生成手法のRRT を組み合わせる

## 動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)

# 逆運動学
逆運動学について説明する．
今回は下図のような2軸ロボットアームを考える．
![ForwardKinematics_V0.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/74c3a952-90c9-4912-9155-901a7b77b245.png)
$l_{1}$はリンク1の長さ[m]，$l_{2}$はリンク2の長さ[m]です．
$\theta_1$は関節1の角度[rad]，$\theta_2$は関節2の角度[rad]です．
各関節角度($\theta_{1}$，$\theta_{2}$)を手先位置から計算する．

前記事にて，手先位置を各関節角度($\theta_{1}$，$\theta_{2}$)から計算した．
その結果は下記の通りとなった．
```math
\displaylines{
x = l_{1} * \cos\theta_{1} + l_{2} * \cos(\theta_{1} + \theta_{2}) \\
y = l_{1} * \sin\theta_{1} + l_{2} * \sin(\theta_{1} + \theta_{2}) \\
}
```

逆運動学で関節角度を算出するは，以下の順番で求める．
1：$\theta_{2}$
2：$\theta_{1}$

## theta2 の算出方法
$\theta_{2}$の算出方法を説明する．
(1)：$x$と$y$を二乗して足し合わせる．
```math
\displaylines{
x^{2} = ( l_{1} * \cos\theta_{1} + l_{2} * \cos(\theta_{1} + \theta_{2}) )^{2} \\
x^{2} = l_{1}^{2} * \cos^{2}\theta_{1} + 2 * l_{1} * l_{2} * \cos\theta_{1} * \sin(\theta_{1} + \theta_{2}) + l_{2}^{2} * \cos^{2}(\theta_{1} + \theta_{2}) \\
y^{2} = ( l_{1} * \sin\theta_{1} + l_{2} * \sin(\theta_{1} + \theta_{2}) )^{2} \\
y^{2} = l_{1}^{2} * \sin^{2}\theta_{1} + 2 * l_{1} * l_{2} * \sin\theta_{1} * \sin(\theta_{1} + \theta_{2}) + l_{2}^{2} * \sin^{2}(\theta_{1} + \theta_{2}) \\

x^{2} + y^{2} = l_{1}^{2} * ( \cos^{2}\theta_{1} + \sin^{2}\theta_{1} ) + 2 * l_{1} * l_{2} * ( \cos\theta_{1} * \cos(\theta_{1} + \theta_{2}) + \sin\theta_{1} * \sin(\theta_{1} + \theta_{2}) ) + l_{2}^{2} * ( \cos^{2}(\theta_{1} + \theta_{2}) + \sin^{2}(\theta_{1} + \theta_{2}) )
}
```

(2)：三角関数の公式を使用する．
```math
\displaylines{
\sin^{2}\theta + \cos^{2}\theta = 1 \\
\sin(\theta_{1} + \theta_{2}) = \sin\theta_{1} * \cos\theta_{2} + \cos\theta_{1} * \sin\theta_{2} \\
\sin(\theta_{1} - \theta_{2}) = \sin\theta_{1} * \cos\theta_{2} - \cos\theta_{1} * \sin\theta_{2} \\
\cos(\theta_{1} + \theta_{2}) = \cos\theta_{1} * \cos\theta_{2} - \sin\theta_{1} * \sin\theta_{2} \\
\cos(\theta_{1} - \theta_{2}) = \cos\theta_{1} * \cos\theta_{2} + \sin\theta_{1} * \sin\theta_{2} \\
}
```

(3)：(1)の結果に(2)を代入する．
```math
\displaylines{
x^{2} + y^{2} = l_{1}^{2} + 2 * l_{1} * l_{2} * ( \sin\theta_{1} * ( \sin\theta_{1} * \cos\theta_{2} + \cos\theta_{1} * \sin\theta_{2} ) + \cos\theta_{1} * (\cos\theta_{1} * \cos\theta_{2} - \sin\theta_{1} * \sin\theta_{2}) ) + l_{2}^{2} \\
x^{2} + y^{2} = l_{1}^{2} + 2 * l_{1} * l_{2} * ( \sin^{2}\theta_{1}  * \cos\theta_{2} + \sin\theta_{1} * \cos\theta_{1} * \sin\theta_{2} + \cos^{2}\theta_{1} * \cos\theta_{2} - \sin\theta_{1} * \cos\theta_{1} * \sin\theta_{2} ) + l_{2}^2 \\
x^{2} + y^{2} = l_{1}^{2} + 2 * l_{1} * l_{2} * \cos\theta_{2} * (\sin^{2}\theta_{1} + \cos^{2}\theta_{1} ) + l_{2}^2 \\
x^{2} + y^{2} = l_{1}^{2} + 2 * l_{1} * l_{2} * \cos\theta_{2} + l_{2}^2 \\
\cos\theta_{2} = ( x^{2} + y^{2} - (l_{1}^{2} + l_{2}^2) ) / (2 * l_{1} * l_{2}) \\
\theta_{2} = \arccos( ( x^{2} + y^{2} - (l_{1}^{2} + l_{2}^2) ) / (2 * l_{1} * l_{2}) )
}
```
上記計算により，$\theta_{2}$が算出される．しかし，算出されるnumpy.arccos()で取得できる角度は$0$から$\pi$の範囲である．そのため，$-\pi$から$0$の範囲は算出できない．
今回は，$-\pi$から$\pi$の範囲の角度が欲しいため，np.arctan2()を採用する．
np.arctan2()の引数には，$\sin{\theta}$と$\cos{\theta}$の情報が必要である．

$\cos\theta_{2}$は(3)で算出されているため，(2)より以下のように$\sin\theta_{2}$と$\theta_{2}$を算出する．
```math
\displaylines{
\sin\theta_{2} = \sqrt{1^2 - \cos^{2}\theta_{2}} \\
\theta_{2} = \arctan2( \sin\theta_{2}, \cos\theta_{2} )
}
```

ここで，$\theta_{2}$は下図のような2通りの解が得られる．
![InverseKinematics.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/ac6ef39b-9540-4efc-b714-c328dfafef45.png)
$+\theta_{2}$は上向きで，$-\theta_{2}$は下向きの角度となる．
要するに逆運動学を解くと，2つの解が得られる．上向きの解か下向きの解かを選択する必要があるんです．

## theta1 の算出方法
$\theta_{1}$の算出方法を説明する．
(1)：$x$と$y$を行列形式で表す．
```math
\displaylines{
\begin{bmatrix}
x \\
y \\
\end{bmatrix}
=
\begin{bmatrix}
l_{1} * \cos\theta_{1} + l_{2} * \cos(\theta_{1} * \cos\theta_{2} - \sin\theta_{1} * \sin\theta_{2} ) \\
l_{1} * \sin\theta_{1} + l_{2} * (\sin\theta_{1} * \cos\theta_{2} + \cos\theta_{1} * \sin\theta_{2} ) \\
\end{bmatrix} \\

\begin{bmatrix}
x \\
y \\
\end{bmatrix}
=
\begin{bmatrix}
\cos\theta_{1} * (l_{1} + l_{2} * \cos\theta_{2} ) - \sin\theta_{1} * (l_{2} * \sin\theta_{2} ) \\
\cos\theta_{1} * (l_{2} * \sin\theta_{2} ) + \sin\theta_{1} * (l_{1} + l_{2} * \cos\theta_{2} ) \\
\end{bmatrix} \\

\begin{bmatrix}
x \\
y \\
\end{bmatrix}
=
\begin{bmatrix}
l_{1} + l_{2} * \cos\theta_{2} & - l_{2} * \sin\theta_{2} \\
l_{2} * \sin\theta_{2} &  l_{1} + l_{2} * \cos\theta_{2} \\
\end{bmatrix}
\begin{bmatrix}
\cos(\theta_{1}) \\
\sin(\theta_{1}) \\
\end{bmatrix}
}
```

(2)：行列から，$\cos\theta_{1}$と$\sin\theta_{1}$を算出する．
```math
\displaylines{
\begin{bmatrix}
\cos\theta_{1} \\
\sin\theta_{1} \\
\end{bmatrix}
=
\begin{bmatrix}
l_{1} + l_{2} * \cos\theta_{2} & - l_{2} * \sin\theta_{2} \\
l_{2} * \sin\theta_{2} &  l_{1} + l_{2} * \cos\theta_{2} \\
\end{bmatrix} ^ {-1}
\begin{bmatrix}
x \\
y \\
\end{bmatrix} \\
\theta_{1} = \arctan2( \sin\theta_{1}, \cos\theta_{1} )
}
```
上記では，逆行列を使用している．逆行列に関しては，下記が参考になります．
(https://qiita.com/nognog/items/efae10aceef1ac9d1d09)

これにより，逆運動学によって手先位置から関節角度を算出することができる．


# 逆運動学のソースコード
## 定数を定義するファイル (constant.py)
定数を定義するファイルを下記に記す．
```constant.py
# 複数ファイルで使用する定数の定義


# 次元数を定義
DIMENTION_NONE            = -1          # 未定義
DIMENTION_2D              =  2          # 2次元
DIMENTION_3D              =  3          # 3次元
```

## 2軸ロボットアームを定義するファイル (robot.py)
2軸ロボットアームを定義するファイルを下記に記す．
```robot.py
# 2軸ロボットアームの運動学を記載

# ライブラリの読み込み
import numpy as np

# 自作モジュールの読み込み
from constant import *              # 定数


class Robot:
    """
    ロボットのベースクラス(抽象クラス)
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長 [m]
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
            inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
            links(): _linksプロパティのゲッター
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_NONE       # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_NONE       # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_NONE       # リンク数
    
    def __init__(self, links):
        """
        コンストラクタ

        パラメータ
            links(numpy.ndarray): ロボットのリンク長 [m]
        """
        if np.size(links) != self._DIMENTION_LINK:
            # 異常
            raise ValueError(f"links's size is abnormal. correct is {self._DIMENTION_Link}")
        
        # プロパティの初期化
        self._links = links

    @property
    def links(self):
        """
        _linksプロパティのゲッター
        """
        return self._links

    def forward_kinematics(self, thetas):
        """
        順運動学 (ロボットの関節角度からロボットの手先位置を算出)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
        """
        raise NotImplementedError("forward_kinematics() is necessary override.")

    def inverse_kinematics(self, pose):
        """
        逆運動学 (ロボットの手先位置からロボットの関節角度を算出)

        パラメータ
            pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
        
        戻り値
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        raise NotImplementedError("inverse_kinematics() is necessary override.")


class Robot2DoF(Robot):
    """
    2軸ロボットクラス
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長
        _rot(Rotation): 回転行列クラス
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_2D         # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_2D         # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_2D         # リンク数
    _DETERMINANT_THRESHOLD = 1e-4           # 行列式の閾値
    
    def __init__(self, links):
        """
        コンストラクタ

        パラメータ
            links(numpy.ndarray): ロボットのリンク長 [m]
        """
        # 親クラスの初期化
        super().__init__(links)

    def forward_kinematics(self, thetas):
        """
        順運動学 (ロボットの関節角度からロボットの手先位置を算出)

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
        """
        # パラメータの次元数を確認
        if np.size(thetas) != self._DIMENTION_THETA:
            raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")

        # あらかじめ三角関数を算出する
        sin1  = np.sin(thetas[0])
        sin12 = np.sin(thetas[0] + thetas[1])
        cos1  = np.cos(thetas[0])
        cos12 = np.cos(thetas[0] + thetas[1])

        theta1  = np.array([cos1,  sin1])
        theta12 = np.array([cos12, sin12])

        # ロボットの手先位置を算出
        pose = self._links[0] * theta1 + self._links[1] * theta12

        return pose

    def inverse_kinematics(self, pose, upper=False):
        """
        逆運動学 (ロボットの手先位置からロボットの関節角度を算出)

        パラメータ
            pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
            upper(bool): 腕が上向かどうか
        
        戻り値
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        # パラメータの次元数を確認
        if np.size(pose) != self._DIMENTION_POSE:
            raise ValueError(f"parameter pose's size is abnormal. pose's size is {np.size(pose)}")

        # c2 = {(px ** 2 + py ** 2) - (l1 ** 2 + l2 ** 2)} / (2 * l1 * l2)
        px = pose[0]
        py = pose[1]
        l1 = self._links[0]
        l2 = self._links[1]
        cos2  = ((px ** 2 + py ** 2) - (l1 ** 2 + l2 ** 2)) / (2 * l1 * l2)
        # cosの範囲は-1以上1以下である
        if cos2 < -1 or cos2 > 1:
            # 異常
            raise ValueError(f"cos2 is abnormal. cos2 is {cos2}")

        # sinも求めて，theta2をatan2()より算出する
        sin2 = np.sqrt(1 - cos2 ** 2)
        theta2 = np.arctan2(sin2,  cos2)
        if not upper:
            theta2 = -theta2
            sin2 = np.sin(theta2)
            cos2 = np.cos(theta2)

        # 行列計算
        # [c1, s1] = [[l1 + l2 * c2, -l2 * s2], [l2 * s2, l1 + l2 * c2]] ** -1 * [px, py]
        element1 =  l1 + l2 * cos2
        element2 = -l2 * sin2
        matrix = np.array([[ element1, element2],
                           [-element2, element1]])
        # 行列式を計算
        det = np.linalg.det(matrix)
        # 0近傍の確認
        if det <= self._DETERMINANT_THRESHOLD and det >= -self._DETERMINANT_THRESHOLD:
            # 0近傍 (異常)
            raise ValueError(f"det is abnormal. det is {det}")

        # [c1, s1]の計算
        cos1_sin1 = np.dot(np.linalg.inv(matrix), pose)
        # theta1をatan2()より算出する
        theta1 = np.arctan2(cos1_sin1[1], cos1_sin1[0])

        thetas = np.array([theta1, theta2])

        return thetas
```

## メイン(プロットなど)処理 (main.py)
メイン処理ファイルを下記に記す．
```main.py
# メイン処理

# ライブラリの読み込み
import numpy as np                  # 数値計算
import matplotlib.pyplot as plt     # 描画

# 自作モジュールの読み込み
from robot import Robot2DoF         # ロボットクラス


FORWARD_KINE = False                 # 順運動学の計算フラグ (True / False = 順運動学 / 逆運動学)
LINE_WIDTH = 3                      # プロット時の線の太さ
GRAPH_FORWARD_FILE_NAME = "plot_2drob_forward_kinematics.png"
GRAPH_INVERSE_FILE_NAME = "plot_2drob_inverse_kinematics.png"


def forward(links, robot, thetas, plot_flg=True):
    """
    順運動学
    
    パラメータ
        links(numpy.ndarray): ロボットのリンク長 [m]
        robot(Robot2DoF): ロボットクラス
        thetas(numpy.ndarray): 関節角度 [rad]
        plot_flg(bool): 描画の有無 (True / False = 描画する / しない)
    
    戻り値
        pose(numpy.ndarray): 手先位置
    """
    # 順運動学により，位置を算出
    pose = robot.forward_kinematics(thetas)

    if plot_flg:
        plot(robot, thetas)
    else:
        # print(f"forward theta = {thetas}")
        # print(f"forward pose  = {pose}")
        pass

    return pose


def inverse(links, robot, pose, plot_flg=True, upper=True):
    """
    逆運動学
    
    パラメータ
        links(numpy.ndarray): ロボットのリンク長 [m]
        robot(Robot2DoF): ロボットクラス
        pose(numpy.ndarray): 手先位置 [m]
        plot_flg(bool): 描画の有無 (True / False = 描画する / しない)
        upper(bool): 関節が上向きとするかどうか (True / False = Yes / No)
    
    戻り値
        thetas(list): 関節角度
    """
    # 順運動学により，位置を算出
    thetas = robot.inverse_kinematics(pose, upper=upper)
    if plot_flg:
        plot(robot, thetas)
    else:
        # print(f"inverse pose  = {pose}")
        # print(f"inverse theta = {thetas}")
        pass

    return thetas


def main():
    """
    メイン処理
    """
    # 2軸ロボットのリンク長
    links = np.array([1.0, 1.0])
    # 2軸ロボットのインスタンスを作成
    robot = Robot2DoF(links)
    # プロット有無
    plot_flg = True
    # 逆運動学の上向き解とするかの有無
    uppers = [True, False]

    # 順運動学で使用する関節角度 [deg] を定義
    deg_thetas = [30, 120]
    candidate = np.deg2rad(deg_thetas)
    all_thetas = np.array([[theta1, theta2] for theta1 in candidate for theta2 in candidate])

    if FORWARD_KINE:
        # 順運動学
        file_name = GRAPH_FORWARD_FILE_NAME
        for thetas in all_thetas:
            pose = forward(links, robot, thetas, plot_flg=plot_flg)
    else:
        # 逆運動学
        file_name = GRAPH_INVERSE_FILE_NAME
        for thetas in all_thetas:
            for upper in uppers:
                # 順運動学で位置を取得
                pose1  = forward(links, robot, thetas, plot_flg=False)
                # 取得した位置で逆運動学
                theta  = inverse(links, robot, pose1,  plot_flg=plot_flg, upper=upper)
                # 逆運動学の結果が正しいかを順運動学で計算
                pose2  = forward(links, robot, theta,  plot_flg=False)
                print(f"pose1 = {pose1[0]:.3f}, {pose1[1]:.3f}")
                print(f"pose2 = {pose2[0]:.3f}, {pose2[1]:.3f}")
            print()

    if plot_flg:
        show(file_name)


def show(graph_file_name):
    """
    描画する

    パラメータ
        graph_file_name(str): 描画ファイル名
    """
    # X軸，Y軸の名称
    plt.xlabel("X [m]")
    plt.ylabel("Y [m]")

    # X軸，Y軸の描画範囲
    plt.xlim(-2.5, 2.5)
    plt.ylim(-2.5, 2.5)

    plt.grid()
    plt.legend(fontsize=6, loc='center left', bbox_to_anchor=(1., .5))
    # 凡例を見切れないようにするために，plt.tight_layout()
    plt.tight_layout()
    plt.savefig(graph_file_name)
    plt.show()


def plot(robot, thetas):
    """
    ロボットをプロット (将来的にはクラス化する)

    パラメータ
        robot(Robot2DoF): ロボット
        thetas(numpy.ndarray): 回転角度
    """
    # 回転角度とリンク長をローカル変数に保存
    theta1 = thetas[0]
    theta2 = thetas[1]
    link1  = robot.links[0]
    link2  = robot.links[1]

    # 三角関数を計算
    s1  = np.sin(theta1)
    c1  = np.cos(theta1)
    s2  = np.sin(theta2)
    c2  = np.cos(theta2)
    s12 = np.sin(theta1 + theta2)
    c12 = np.cos(theta1 + theta2)

    # 各リンクの位置を算出
    base_pos  = np.zeros(2)
    link1_pos = link1 * np.array([c1, s1])
    link2_pos = link1_pos + link2 * np.array([c12, s12])

    # 全リンクの位置を算出
    all_link_pos = np.array([base_pos, link1_pos, link2_pos])
    # print(f"all_link_pos = {all_link_pos}")
    # 全リンクの回転角度を算出
    all_link_theta = np.array([theta1, theta2])

    # 線プロット
    plt.plot(all_link_pos[:, 0], all_link_pos[:, 1], linewidth=LINE_WIDTH, label=f"theta1={np.rad2deg(theta1):.1f} [deg], theta2={np.rad2deg(theta2):.1f} [deg]")
    # 点プロット
    plt.scatter(all_link_pos[:, 0], all_link_pos[:, 1])


if __name__ == "__main__":
    # 本ファイルがメインで呼ばれた時の処理
    main()
```

# 処理結果
main.pyを実行し，順運動学と逆運動学のグラフを下図に記す．
下図は順運動学のグラフである．
![plot_2drob_forward_kinematics.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/4c106e4b-c560-4b58-9477-e99325046d0b.png)
下図は逆運動学のグラフである．
![plot_2drob_inverse_kinematics.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/de679cd4-d02d-4599-a93c-b8b8daeb53f2.png)

右に判例を記載してある．グラフを見る限り，逆運動学の処理は正常であることがわかる．
また，逆運動学では解が2つとなることも確認できた．

# おわりに
本記事では，Pythonを使用して，下記内容を実装しました．
・2軸ロボットアームの逆運動学 (逆運動学 ... ロボットの手先位置からロボットの関節角度を算出する)

次記事では，下記内容を実装していきます．
・2軸ロボットアームの軌道生成
(https://qiita.com/haruhiro1020/items/6deca78f9f1fd868b2c0)