2軸ロボットアーム Part1 (順運動学)

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

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

## 本記事では実装できないこと (将来実装したい内容)
・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)
l1はリンク1の長さ[m]，l2はリンク2の長さ[m]です．
$\theta_1$は関節1の角度[rad]，$\theta_2$は関節2の角度[rad]です．
手先位置P を各関節角度(theta_1，theta_2)から計算する．

上図を一度下図のようにして考えてみる．
![ForwardKinematics_V1.drawio.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3275280/19a3ca8f-a9e5-4cf6-817f-7e40249bf4da.png)

はじめにリンク1の先端位置P1を計算する．
P1(x1, y1)は三角関数を使用することで，以下となる．
```math
\displaylines{
x_{1} = l_{1} * \cos(\theta_{1}) \\
y_{1} = l_{1} * \sin(\theta_{1}) \\
}
```

次に手先位置Pを計算する．
P(x, y)は三角関数とP1(x1, y1)を使用することで，以下となる．
```math
\displaylines{
x = x_{1} + l_{2} * \cos(\theta_{1} + \theta_{2}) \\
y = y_{1} + l_{2} * \sin(\theta_{1} + \theta_{2}) \\
}
```

手先位置P(x, y)はP1(x1, y1)を使用しない場合は，以下となる．
```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}) \\
}
```


# 順運動学のソースコード
## 定数を定義するファイル (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(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
            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()をオーバーライドしてください.")



class Robot2DoF(Robot):
    """
    2軸ロボットクラス
    
    プロパティ
        _links(numpy.ndarray): ロボットのリンク長
        _rot(Rotation): 回転行列クラス
    
    メソッド
        public
            forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
    """
    # 定数の定義
    _DIMENTION_POSE  = DIMENTION_2D         # 手先位置の次元数
    _DIMENTION_THETA = DIMENTION_2D         # 関節角度の次元数
    _DIMENTION_LINK  = DIMENTION_2D         # リンク数
    
    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.\nthetas'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
```

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

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

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


LINE_WIDTH = 3                      # プロット時の線の太さ


def main():
    """
    メイン処理
    """
    # 2軸ロボットのリンク長
    links = np.array([1.0, 1.0])
    # 2軸ロボットのインスタンスを作成
    robot = Robot2DoF(links)

    # 関節角度 [deg] を定義
    deg_thetas = [0, 30, 60, 90]
    # [deg]から[rad]へ変換
    candidate  = np.deg2rad(deg_thetas)
    all_thetas = np.array([[theta1, theta2] for theta1 in candidate for theta2 in candidate])

    # 順運動学により，位置を算出
    for thetas in all_thetas:
        pose = robot.forward_kinematics(thetas)
        plot(robot, thetas)

    # 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(bbox_to_anchor=(1.05, 1), loc='upper left', borderaxespad=0, fontsize=10)
    plt.legend(fontsize=6)
    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])

    # 線プロット (小数点1桁まで記載)
    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/f9eaf34a-f54c-4c42-a6b2-0ea27ef64289.png)
左下に判例を記載してある．グラフを見る限り，順運動学の処理は正常であることがわかる．

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

次記事では，下記内容を実装していきます．
・2軸ロボットアームの逆運動学 (逆運動学 ... ロボットの手先位置からロボットの関節角度を算出する)
(https://qiita.com/haruhiro1020/items/9416bea4b5b0757f4f36)