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

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

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

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

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


# 逆運動学
逆運動学について説明する．
今回は下図のような3軸ロボットアームを考える．
![ForwardKinematics_V1.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/78f2aa40-fc48-4441-9f1b-9f252a5a756b.png)

$l_{1}$はリンク1の長さ[m]，$l_{2}$はリンク2の長さ[m]，$l_{3}$はリンク3の長さ[m]です．
$\theta_{1}$は関節1の角度[rad]であり，z軸方向に回転する．
$\theta_{2}$は関節2の角度[rad]であり，y軸方向に回転する．
$\theta_{3}$は関節3の角度[rad]であり，y軸方向に回転する．
各関節角度($\theta_{1}$，$\theta_{2}$, $\theta_{3}$)を手先位置$P(x, y, z)$から計算する．

前記事にて，手先位置$P(x, y, z)$を各関節角度($\theta_{1}$，$\theta_{2}$, $\theta_{3}$)から計算した．
その結果は下記の通りとなった．
```math
\displaylines{
 x = \cos\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) ) \\
 y = \sin\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) ) \\
 z = l_{1} + l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) \\
}
```
 
逆運動学で関節角度を算出するは，以下の順番で求める．
1：$\theta_{3}$
2：$\theta_{1}$
3：$\theta_{2}$

## theta3 の算出方法
$\theta_{3}$の算出方法を説明する．
(1)：$x$，$y$，$z - l_{1}$を二乗して足し合わせる．
```math
\displaylines{
x^{2} = \cos^{2}\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} \\
y^{2} = \sin^{2}\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} \\
(z - l_{1})^{2} = (l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) )^{2} \\
x^{2} + y^{2} + (z - l_{1})^{2} = (\cos^{2}\theta_{1} + \sin^{2}\theta_{1}) * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} + (l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) )^{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} + (z - l_{1})^{2} = (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} + (l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) )^{2} \\
x^{2} + y^{2} + (z - l_{1})^{2} = (l^{2}_{2} * \cos^{2}\theta_{2} + 2 * l_{2} * l_{3} * \cos\theta_{2} * \cos(\theta_{2} + \theta_{3}) + l^{2}_{3} * \cos^{2}(\theta_{2} + \theta_{3}) ) + (l^{2}_{2} *  \sin^{2}\theta_{2} + 2 * l_{2} * l_{3} * \sin\theta_{2} * \sin(\theta_{2} + \theta_{3}) + l^{2}_{3} * \sin^{2}(\theta_{2} + \theta_{3}) ) \\
x^{2} + y^{2} + (z - l_{1})^{2} = (\cos^{2}\theta_{2} + \sin^{2}\theta_{2}) * l^{2}_{2} + (\cos^{2}(\theta_{2} + \theta_{3}) + \sin^{2}(\theta_{2} + \theta_{3}) ) * l^{2}_{3} + 2 * l_{2} * l_{3} * (\cos\theta_{2} * \cos(\theta_{2} + \theta_{3}) + \sin\theta_{2} *  \sin(\theta_{2} + \theta_{3})) \\
x^{2} + y^{2} + (z - l_{1})^{2} = l^{2}_{2} + l^{2}_{3} + 2 * l_{2} * l_{3} * (\cos\theta_{2} * (\cos\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \sin\theta_{3}) ) + sin\theta_{2} * (sin\theta_{2} * \cos\theta_{3} + \cos\theta_{2} * \sin\theta_{3} ) ) \\
x^{2} + y^{2} + (z - l_{1})^{2} = l^{2}_{2} + l^{2}_{3} + 2 * l_{2} * l_{3} * (\cos^{2}\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \cos\theta_{2} * \sin\theta_{3}) + sin^{2}\theta_{2} * \cos\theta_{3} + \sin\theta_{2} * \cos\theta_{2} * \sin\theta_{3} ) \\
x^{2} + y^{2} + (z - l_{1})^{2} = l^{2}_{2} + l^{2}_{3} + 2 * l_{2} * l_{3} * (\cos^{2}\theta_{2} + sin^{2}\theta_{2}) * \cos\theta_{3} \\
x^{2} + y^{2} + (z - l_{1})^{2} = l^{2}_{2} + l^{2}_{3} + 2 * l_{2} * l_{3} * \cos\theta_{3} \\
2 * l_{2} * l_{3} * \cos\theta_{3} = (x^{2} + y^{2} + (z - l_{1})^{2}) - (l^{2}_{2} + l^{2}_{3}) \\
\cos\theta_{3} = ( (x^{2} + y^{2} + (z - l_{1})^{2}) - (l^{2}_{2} + l^{2}_{3}) ) / (2 * l_{2} * l_{3}) \\
\theta_{3} = \arccos( (x^{2} + y^{2} + (z - l_{1})^{2}) - (l^{2}_{2} + l^{2}_{3}) ) / (2 * l_{2} * l_{3}) \\
}
```

上記計算により，$\theta_{3}$が算出される．しかし，算出されるnumpy.arccos()で取得できる角度は$0$から$\pi$の範囲である．そのため，$-\pi$から$0$の範囲は算出できない．
今回は，$-\pi$から$\pi$の範囲の角度が欲しいため，np.arctan2()を採用する．
np.arctan2()の引数には，$\sin\theta$と$\cos\theta$の情報が必要である．

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

ここで，$\theta_{3}$は下図のような2通りの解が得られる．
![InverseKinematics_V1.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/036445f9-4de1-42ce-b6b5-dd2c7ce25057.png)

$+\theta_{3}$は上向きで，$-\theta_{3}$は下向きの角度となる．
要するに逆運動学を解くと，2つの解が得られる．上向きの解か下向きの解かを選択する必要がある．


## theta1 の算出方法
$\theta_{1}$の算出方法を説明する．
ロボットをz軸から見ると，下図のようになる．
![ForwardKinematics_V3.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/bb5b5608-1b3c-4c44-a5ce-07379508f9ca.png)

$\theta_{1}$はロボットの手先位置$P$の$(x, y)$で以下の通りに，算出することができる．
```math
\displaylines{
\theta_{1} = arctan2(y, x) \\
\theta_{1} = arctan2(-y, -x) \\
}
```
ロボット正面に手先位置が存在するときは$\theta_{1} = arctan2(y, x)$となり，ロボット背面に手先位置が存在するときは$\theta_{1} = arctan2(-y, -x)$となる．
ロボット背面に位置が存在する時は，ロボットの座標系から180度回転するから，$arctan2(-y, -x)$を使用する．

ただし，$(x, y) = (0, 0)$では，解が不定となることに注意すること．
($(x, y) = (0, 0)$では\theta_{1}にどのような値も入れることが可能となる)


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

(2)：(1)の両辺の平方根を算出する．
```math
\displaylines{
+-\sqrt(x^{2} + y^{2}) = l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) \\
}
```

(3)：(2)と$z$を行列形式として，$\cos(\theta_{2})と\sin(\theta_{2})$を算出し，$\theta_{2}$を求める．
```math
\displaylines{
\begin{pmatrix}
l_{2} * \cos\theta_{2} + l_{3} * (\cos\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \sin\theta_{3} ) \\
l_{2} * \sin\theta_{2} + l_{3} * (\sin\theta_{2} * \cos\theta_{3} + \cos\theta_{2} * \sin\theta_{3})
\end{pmatrix}
=
\begin{pmatrix}
+-\sqrt(x^{2} + y^{2}) \\
z - l_{1} \\
\end{pmatrix} \\

\begin{pmatrix}
l_{2} + l_{3} * \cos\theta_{3} & -l_{3} * \sin\theta_{3} \\
l_{3} * \sin\theta_{3} & l_{2} + l_{3} * \cos\theta_{3} \\
\end{pmatrix}
\begin{pmatrix}
\cos\theta_{2} \\
\sin\theta_{2} \\
\end{pmatrix}
=
\begin{pmatrix}
+-\sqrt(x^{2} + y^{2}) \\
z - l_{1} \\
\end{pmatrix} \\

\begin{pmatrix}
\cos\theta_{2} \\
\sin\theta_{2} \\
\end{pmatrix}
=
\begin{pmatrix}
l_{2} + l_{3} * \cos\theta_{3} & -l_{3} * \sin\theta_{3} \\
l_{3} * \sin\theta_{3} & l_{2} + l_{3} * \cos\theta_{3} \\
\end{pmatrix} ^{-1}
\begin{pmatrix}
+-\sqrt(x^{2} + y^{2}) \\
z - l_{1} \\
\end{pmatrix} \\

\theta_{2} = \arctan2(\sin\theta_{2}, \cos\theta_{2}) \\
}
```

上記では，逆行列を使用している．逆行列に関しては，下記が参考になります．
(https://qiita.com/nognog/items/efae10aceef1ac9d1d09)

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


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


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

# 回転軸
ROTATION_X_AXIS = "rot_x"   # x軸周りに回転
ROTATION_Y_AXIS = "rot_y"   # y軸周りに回転
ROTATION_Z_AXIS = "rot_z"   # z軸周りに回転
```

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

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

# サードパーティーの読み込み


# 自作モジュールの読み込み
from constant import *              # 定数
from rotation import Rotation       # 回転行列


class Robot:
    """
    ロボットのベースクラス(抽象クラス)
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長 [m]
        _rot(Rotation): 回転行列クラス
        _objects(list): 干渉物オブジェクト
        _manager(fcl.DynamicAABBTreeCollisionManager): 干渉判定クラス
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
            inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
            forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
            update(): 角度を与えて，各リンクの直方体を更新する
            links(): _linksプロパティのゲッター
            manager(): _managerプロパティのゲッター
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_NONE       # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_NONE       # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_NONE       # リンク数
    _DIMENTION_AXIS  = DIMENTION_NONE       # 回転軸数
    
    _INITIAL_THETA   = 0.0                  # 初期回転角度 [rad]
    
    
    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
        self._rot = Rotation()
        self._objects = []
        self._manager = None

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

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

    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.")

    def forward_kinematics_all_pos(self, thetas):
        """
        順運動学で全リンクの位置を取得

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

    def update(self, thetas):
        """
        角度を与えて，各リンクの直方体を更新する

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        """
        raise NotImplementedError("update() is necessary override.")


class Robot3DoF(Robot):
    """
    3軸ロボットクラス
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長
        _rot(Rotation): 回転行列クラス
        _axiss(list): 関節の回転軸
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
            inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
            forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
            update(): 角度を与えて，各リンクの直方体を更新する
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_3D         # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_3D         # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_3D         # リンク数
    
    _DETERMINANT_THRESHOLD = 1e-4           # 行列式の閾値
    _THETA1_XY_THRESHOLD   = 1e-4           # theta1算出時のx, y閾値
    _BOX_WIDTH = 1e-2                       # 各リンクの幅を定義
    
    
    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])
        sin2  = np.sin(thetas[1])
        sin23 = np.sin(thetas[1] + thetas[2])
        cos1  = np.cos(thetas[0])
        cos2  = np.cos(thetas[1])
        cos23 = np.cos(thetas[1] + thetas[2])

        # ロボットの手先位置を算出
        pxy_common = self._links[1] * cos2 + self._links[2] * cos23
        px = cos1 * pxy_common
        py = sin1 * pxy_common
        pz = self._links[0] + self._links[1] * sin2 + self._links[2] * sin23
        pose = np.array([px, py, pz])

        return pose

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

        パラメータ
            pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
            upper(bool): 腕が上向かどうか
            front(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)}")

        # はじめにtheta3を算出する
        # c3 = {(px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l3 ** 2)} / (2 * l2 * l3)
        px = pose[0]
        py = pose[1]
        pz = pose[2]
        l1 = self._links[0]
        l2 = self._links[1]
        l3 = self._links[2]
        cos3  = ((px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l3 ** 2)) / (2 * l2 * l3)
        # cosの範囲は-1以上1以下である
        if abs(cos3) > 1:
            # 異常
            raise ValueError(f"cos3 is abnormal. cos3 is {cos3}")

        # sinも求めて，theta3をatan2(sin2, cos2)より算出する
        sin3 = np.sqrt(1 - cos3 ** 2)
        theta3 = np.arctan2(sin3,  cos3)
        if not upper:
            # 下向きの角度のため，三角関数も更新
            theta3 = -theta3
            sin3 = np.sin(theta3)
            cos3 = np.cos(theta3)

        # 次にtheta1を算出する
        # theta1 = atan2(py, px)
        # py, pxが0近傍なら失敗
        if abs(py) <= self._THETA1_XY_THRESHOLD and abs(px) <= self._THETA1_XY_THRESHOLD:
            # 近傍のため，エラー
            raise ValueError(f"abs(py) and abs(px) is abnormal. py is {py}, px is {px}")
        if front:
            # 位置(pose)に対して正面向き
            theta1 = np.arctan2( py,  px)
        else:
            # 位置(pose)に対して後ろ向き
            theta1 = np.arctan2(-py, -px)

        # 最後にtheta2を算出する
        # 行列計算
        # [c2, s2] = [[l2 + l3 * c3, -l3 * s3], [l3 * s3, l2 + l3 * c3]] ** -1 * [root(px ** 2 + py ** 2), pz - l1]
        element1 =  l2 + l3 * cos3
        element2 = -l3 * sin3
        matrix = np.array([[ element1, element2],
                           [-element2, element1]])
        # 行列式を計算
        det = np.linalg.det(matrix)
        # 0近傍の確認
        if abs(det) <= self._DETERMINANT_THRESHOLD:
            # 0近傍 (異常)
            raise ValueError(f"det is abnormal. det is {det}")

        # 位置を保存 ([root(px ** 2 + py ** 2), pz - l1])
        position = np.array([np.sqrt(px ** 2 + py ** 2), pz - l1])

        # [c2, s2]の計算
        cos2_sin2 = np.dot(np.linalg.inv(matrix), position)
        # theta2をatan2()より算出する
        theta2 = np.arctan2(cos2_sin2[1], cos2_sin2[0])

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

        return thetas

    def forward_kinematics_all_link_pos(self, thetas):
        """
        順運動学で全リンクの位置を取得 (グラフの描画で使用する)

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

        # 回転角度とリンク長をローカル変数に保存
        theta1 = thetas[0]
        theta2 = thetas[1]
        theta3 = thetas[2]
        link1  = self._links[0]
        link2  = self._links[1]
        link3  = self._links[2]

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

        # 各リンクの位置を算出
        base_pos  = np.zeros(self._DIMENTION_POSE)
        link1_pos = np.array([0, 0, link1])
        link2_xy_common = link2 * c2
        link2_pos = np.array([c1 * link2_xy_common, s1 * link2_xy_common, link1 + link2 * s2])
        link3_xy_common = link2_xy_common + link3 * c23
        link3_pos = np.array([c1 * link3_xy_common, s1 * link3_xy_common, link1 + link2 * s2 + link3 * s23])

        # 全リンクの位置を算出
        all_link_pose = np.array([base_pos, link1_pos, link2_pos, link3_pos])

        return all_link_pose
```

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

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

# 自作モジュールの読み込み
from constant import *              # 定数
from robot import Robot3DoF         # ロボットクラス


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


def forward(robot, thetas, axis, plot_flg=True, label=True):
    """
    順運動学
    
    パラメータ
        robot(Robot3DoF): ロボットクラス
        thetas(numpy.ndarray): 関節角度 [rad]
        axis(figure.add_subplot): 3次元の描画軸
        plot_flg(bool): 描画の有無 (True / False = 描画する / しない)
        label(bool): 描画時のラベルの有無
    
    戻り値
        pose(numpy.ndarray): 手先位置
    """
    # 順運動学により，位置を算出
    pose = robot.forward_kinematics(thetas)

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

    return pose


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

    return thetas


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

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

    # 3次元グラフの作成
    fig = plt.figure()
    axis = fig.add_subplot(111, projection="3d")

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

    if plot_flg:
        show(axis, file_name)


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

    パラメータ
        axis(figure.add_subplot): 3次元の描画軸
        graph_file_name(str): 描画ファイル名
    """
    # X軸，Y軸，Z軸の名称
    axis.set_xlabel("X [m]")
    axis.set_ylabel("Y [m]")
    axis.set_zlabel("Z [m]")

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


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

    パラメータ
        robot(Robot3DoF): ロボット
        thetas(numpy.ndarray): 回転角度
        axis(figure.add_subplot): 3次元の描画軸
        label(bool): 描画時のラベルの有無
    """
    # 全リンクの位置を算出
    all_link_pos = robot.forward_kinematics_all_link_pos(thetas)

    # 線プロット
    if label:
        axis.plot(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], linewidth=LINE_WIDTH, label=f"theta1={np.rad2deg(thetas[0]):.1f} [deg], theta2={np.rad2deg(thetas[1]):.1f} [deg], theta3={np.rad2deg(thetas[2]):.1f} [deg]")
    else:
        axis.plot(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], linewidth=LINE_WIDTH)

    # 点プロット
    axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2])


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


# 処理結果
main.pyを実行し，順運動学と逆運動学のグラフを下図に記す．
下図は順運動学のグラフである．
![plot_3drob_forward_kinematics.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/27d47d75-8775-4895-ab8b-3de9ee993d54.png)

下図は逆運動学のグラフである．
![plot_3drob_inverse_kinematics.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/9713a608-7947-49d8-a13c-ebaf176b00ac.png)

右に判例を記載してある．グラフを見る限り，逆運動学の処理は正常であることがわかる．


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

次記事では，下記内容を実装していきます．
・干渉物が存在する環境下で，3軸ロボットアームの経路生成
(https://qiita.com/haruhiro1020/items/6994a94efaff98d8dae1)