2軸ロボットアーム Part3 (軌道生成)

はじめに

私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
前記事では，2軸ロボットアームの逆運動学を説明した．
(https://qiita.com/haruhiro1020/items/9416bea4b5b0757f4f36)
最終的にはロボットアームにRRT + python-fcl を合体させて，ロボットアームの経路生成を実装したいと考えている．
本記事では，2軸ロボットアームの軌道生成手法を説明する．
2点間の軌道を生成する手法を説明する．
2点間には干渉物が存在しない前提で説明していく．
干渉物が存在する環境では，RRT による経路生成を考えている．

本記事で実装すること

・2軸ロボットアームの軌道生成

本記事では実装できないこと (将来実装したい内容)

・2軸ロボットアームに経路生成手法のRRT を組み合わせる
(干渉物が存在する環境下)

動作環境

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

軌道生成

軌道生成について説明する．
今回は下図のような2軸ロボットアームを考える．

$P_{s}$がロボットの初期位置で，$P_{e}$がロボットの終端位置とする．
今回は初期位置($P_{s}$)から終端位置($P_{e}$)までの軌道生成を実施する．

軌道生成としては，大きく分けると下表のような2パターンとなる．

パターン	概要	メリット	デメリット
1	初期位置/終端位置を関節角度に変換(逆運動学)してから，関節角度の軌道生成(関節空間内での軌道生成)	逆運動学の回数が少ない(軌道生成の成功率が高い)	軌道が外回り(遠回り)になる
2	初期位置/終端位置間の軌道生成(位置空間内での軌道生成)	初期位置/終端位置を直線的に移動できる	逆運動学の回数が多い(軌道生成の成功率が低い)

パターン1 (初期位置/終端位置を関節角度に変換(逆運動学)してから，関節角度の軌道生成(関節空間内での軌道生成))

パターン1の内容について，説明する．

はじめに，初期位置($P_{s}$)と終端位置($P_{e}$)を逆運動学により，初期角度($\theta_{s}$)と終端角度($\theta_{e}$)を算出する．

次に，初期角度($\theta_{s}$)と終端角度($\theta_{e}$)より，軌道生成を実施する．
要するに，位置空間から関節空間に変換して，関節空間内で軌道生成を実行する．

2点間での軌道生成に関しては，以下の3パターンを提案する．

パターン	概要	メリット	デメリット
1	初期角度と終端角度を直線補間	計算が簡単	角速度を考慮できない
2	初期位置と終端位置間を3次多項式による補間	角速度を考慮できる	角加速度を考慮できない
3	初期位置と終端位置間を5次多項式による補間	角速度と角加速度を考慮できる	計算が複雑

初期角度と終端角度を直線補間

直線補間による軌道生成に関して説明する．
直線補間の式は下記のようになる．

\displaylines{
f(t) = a * t + b  \\
f(t) ... 関節角度 \\
t ... 時間 \\
}

今回，初期角度($t=0$)が$\theta_{s}$，終端角度($t=t_{f}$, $t_{f}$は適当に決めた変数)であるため，$f(t)$のパラメータである$a$と$b$は下記のように算出できる．

\displaylines{
f(0) = a * 0 + b = b = \theta_{s} \\
f(t_{f}) = a * t_{f} + b = a * t_{f} + \theta_{s} = \theta_{e} \\
a = (\theta_{e} - \theta_{s}) / t_{f} \\
}

直線補間では，下図のような軌道が生成される．

上図では，関節空間内の軌道生成であるため，後ほど順運動学(関節角度から手先位置を算出する)により，手先位置へ変換する必要がある．(関節空間内では，実際の手先位置の軌道が理解できないから，順運動学を実施して，軌道をプロットする)

後ほど，直線補間/3次補間/5次補間による軌道のアニメーションを載せる．

初期角度と終端角度を3次多項式による補間

3次多項式による軌道生成に関して説明する．
3次多項式の式は下記のようになる．

\displaylines{
f(t) = a * t^{3} + b * t^{2} + c * t + d \\
f'(t) = 3 * a * t^{2} + 2 * b * t + c \\
f(t) ... 関節角度 \\
f'(t) ... 関節角速度 \\
t ... 時間 \\
}

今回，初期角度($t=0$)が$\theta_{s}$，終端角度($t=t_{f}$, $t_{f}$は適当に決めた変数)である．
また，初期角速度($t=0$)と終端角速度($t=t_{f}$)を$0$と仮定すると，$f(t)$のパラメータである$a$，$b$，$c$と$d$は下記のように算出できる．

\displaylines{
f(0) = a * 0^{3} + b * 0^{2} + c * 0 + d = d = \theta_{s} \\
f'(0) = 3 * a * 0^{2} + 2 * b * 0 + c = c = 0 \\
f(t_{f}) = a * t^{3}_{f} + b * t^{2}_{f} + c * t_{f} + d = a * t^{3}_{f} + b * t^{3}_{f} + \theta_{s} = \theta_{e} \\
f'(t_{f}) = 3 * a * t^{2}_{f} + 2 * b * t_{f} + c = 3 * a * t^{2}_{f} + 2 * b * t_{f} = 0 \\
}

上記より，$d = \theta_{s}$，$c = 0$を求める事ができた．
次に，上記の$f(t_{f})$と$f'(t_{f})$より，パラメータ$b$を算出する．

\displaylines{
3 * f(t_{f}) = 3 * a * t^{3}_{f} + 3 * b * t^{3}_{f} + 3 * \theta_{s} = 3 * \theta_{e} \\
t_{f} * f'(t_{f}) = 3 * a * t^{3}_{f} + 2 * b * t^{2}_{f} = 0 \\
3 * f(t_{f}) - t_{f} * f'(t_{f}) = 3 * a * t^{3}_{f} + 3 * b * t^{3}_{f} + 3 * \theta_{s} - (3 * a * t^{3}_{f} + 2 * b * t^{2}_{f}) = 3 * \theta_{e} \\
3 * f(t_{f}) - t_{f} * f'(t_{f}) = b * t^{2}_{f}) + 3 * \theta_{s} = 3 * \theta_{e} \\
b * t^{2}_{f}) = 3 * (\theta_{e} - \theta_{s}) \\
b = 3 * (\theta_{e} - \theta_{s}) / t^{2}_{f} \\
}

上記で算出したパラメータ$b$と$d$, $c$を$f(t_{f})$に代入することで，パラメータ$a$を算出する．

\displaylines{
f(t_{f}) = a * t^{3}_{f} + 3 * (\theta_{e} - \theta_{s}) / t^{2}_{f} * t^{2}_{f} + 0 * t_{f} + \theta_{s} = \theta_{e} \\
a * t^{3}_{f} = \theta_{e} - \theta_{s} - 3 * (\theta_{e} - \theta_{s}) \\
a * t^{3}_{f} = -2 * (\theta_{e} - \theta_{s}) \\
a = -2 * (\theta_{e} - \theta_{s}) / t^{3}_{f} \\
}

最終的に各パラメータは下表の通りとなる．

パラメータ名	値
a	$-2 * (\theta_{e} - \theta_{s}) / t^{3}_{f}$
b	$ 3 * (\theta_{e} - \theta_{s}) / t^{2}_{f}$
c	$0$
d	$ \theta_{s}$

3次補間では，下図のような軌道が生成される．

上図では，関節空間内の軌道生成であるため，後ほど順運動学(関節角度から手先位置を算出する)により，手先位置へ変換する必要がある．(関節空間内では，実際の手先位置の軌道が理解できないから，順運動学を実施して，軌道をプロットする)

後ほど，直線補間/3次補間/5次補間による軌道のアニメーションを載せる．

初期角度と終端角度を5次多項式による補間

5次多項式による軌道生成に関して説明する．
5次多項式の式は下記のようになる．

\displaylines{
f(t) = a * t^{5} + b * t^{4} + c * t^{3} + d * t^{2} + e * t + f \\
f'(t) = 5 * a * t^{4} + 4 * b * t^{3} + 3 * c * t^{2} + 2 * d * t + e \\
f''(t) = 20 * a * t^{3} + 12 * b * t^{2} + 6 * c * t + 2 * d \\
f(t) ... 関節角度 \\
f'(t) ... 関節角速度 \\
f''(t) ... 関節角加速度 \\
t ... 時間 \\
}

今回，初期角度($t=0$)が$\theta_{s}$，終端角度($t=t_{f}$, $t_{f}$は適当に決めた変数)である．
また，初期角速度($t=0$)と終端角速度($t=t_{f}$)を$0$，初期角加速度($t=0$)と終端角加速度($t=t_{f}$)を$0$仮定すると，$f(t)$のパラメータである$a$，$b$，$c$と$d$は下記のように算出できる．

\displaylines{
f(0) = a * 0^{5} + b * 0^{4} + c * 0^{3} + d * 0^{2} + e * 0 + f = f = \theta_{s} \\
f'(0) = 5 * a * 0^{4} + 4 * b * 0^{3} + 3 * c * 0^{2} + 2 * d * 0 + e = e = 0 \\
f''(0) = 20 * a * 0^{3} + 12 * b * 0^{2} + 6 * c * 0 + 2 * d = 2 * d = 0 \\
f(t_{f}) = a * t^{5}_{f} + b * t^{4}_{f} + c * t^{3}_{f} + d * t^{2}_{f} + e * t_{f} + f = a * t^{5}_{f} + b * t^{4}_{f} + c * t^{3}_{f} + \theta_{s} = \theta_{e} \\
f'(t_{f}) = 5 * a * t^{4}_{f} + 4 * b * t^{3}_{f} + 3 * c * t^{2}_{f} + 2 * d * t_{f} + e = 5 * a * t^{4}_{f} + 4 * b * t^{3}_{f} + 3 * c * t^{2}_{f} = 0 \\
f''(t_{f}) = 20 * a * t^{3}_{f} + 12 * b * t^{2}_{f} + 6 * c * t_{f} + 2 * d = 20 * a * t^{3}_{f} + 12 * b * t^{2}_{f} + 6 * c * t_{f} = 0 \\
}

上記より，$f = \theta_{s}$，$e = 0$，$d = 0$を求める事ができた．
次に，上記の$f(t_{f})$，$f'(t_{f})$と$f''(t_{f})$より，パラメータ$b$と$c$の方程式を算出する．

\displaylines{
4 * f'(t_{f}) = 20 * a * t^{4}_{f} + 16 * b * t^{3}_{f} + 12 * c * t^{2}_{f} = 0 \\
t_{f} * f''(t_{f}) = 20 * a * t^{4}_{f} + 12 * b * t^{3}_{f} + 6 * c * t^{2}_{f} = 0 \\
4 * f'(t_{f}) - t_{f} * f''(t_{f}) = 20 * a * t^{4}_{f} + 16 * b * t^{3}_{f} + 12 * c * t^{2}_{f} - (20 * a * t^{4}_{f} + 12 * b * t^{3}_{f} + 6 * c * t^{2}_{f}) = 4 * b * t^{3}_{f} + 6 * c * t^{2}_{f} = 0 \\
4 * b * t^{3}_{f} + 6 * c * t^{2}_{f} = 0 ... ① \\
5 * f(t_{f}) = 5 * a * t^{5}_{f} + 5 * b * t^{4}_{f} + 5 * c * t^{3}_{f} + 5 * \theta_{s} = 5 * \theta_{e} \\
t_{f} * f'(t_{f}) = 5 * a * t^{5}_{f} + 4 * b * t^{4}_{f} + 3 * c * t^{3}_{f} = 0 \\
5 * f(t_{f}) - t_{f} * f'(t_{f}) = 5 * a * t^{5}_{f} + 5 * b * t^{4}_{f} + 5 * c * t^{3}_{f} + 5 * \theta_{s} - (5 * a * t^{5}_{f} + 4 * b * t^{4}_{f} + 3 * c * t^{3}_{f}) = b * t^{4}_{f} + 2 * c * t^{3}_{f} + 5 * \theta_{s} = 5 * \theta_{e} \\
b * t^{4}_{f} + 2 * c * t^{3}_{f} + 5 * \theta_{s} = 5 * \theta_{e} ... ② \\
}

上記の$① * -1$と$② * 4$より，パラメータ$b$，$c$を算出する．

\displaylines{
② * 4 = 4 * b * t^{4}_{f} + 8 * c * t^{3}_{f} + 20 * \theta_{s} = 20 * \theta_{e} \\
① * -1 = -4 * b * t^{3}_{f} + -6 * c * t^{2}_{f} = 0 \\
② * 4 - ① = 4 * b * t^{4}_{f} + 8 * c * t^{3}_{f} + 20 * \theta_{s} - (4 * b * t^{3}_{f} + 6 * c * t^{2}_{f}) = 2 * c * t^{3}_{f} + 20 * \theta_{s} = 20 * \theta_{e} \\
2 * c * t^{3}_{f} = 20 * (\theta_{e} - \theta_{s}) \\
c = 10 * (\theta_{e} - \theta_{s}) / t^{3}_{f} \\
①より， 4 * b * t^{3}_{f} =  -6 * c * t^{2}_{f} = -6 * 10 * (\theta_{e} - \theta_{s}) / t^{3}_{f} * t^{2}_{f} = -60 * (\theta_{e} - \theta_{s}) / t_{f} \\
b = -15 * (\theta_{e} - \theta_{s}) / t^{4}_{f} \\
}

上記で算出したパラメータ$b$，$c$を$f(t_{f})$に代入することで，パラメータ$a$を算出する．

\displaylines{
f(t_{f}) = a * t^{5}_{f} + b * t^{4}_{f} + c * t^{3}_{f} + \theta_{s} = a * t^{5}_{f} + -15 * (\theta_{e} - \theta_{s}) / t^{4}_{f} * t^{4}_{f} + 10 * (\theta_{e} - \theta_{s}) / t^{3}_{f} * t^{3}_{f} + \theta_{s} = a * t^{5}_{f} -15 * (\theta_{e} - \theta_{s}) + 10 * (\theta_{e} - \theta_{s}) + \theta_{s}) = \theta_{e} \\
a * t^{5}_{f} = (\theta_{e} - \theta_{s}) + 15 * (\theta_{e} - \theta_{s}) - 10 * (\theta_{e} - \theta_{s}) = 6 * (\theta_{e} - \theta_{s}) \\
a = 6 * (\theta_{e} - \theta_{s}) / t^{5}_{f} \\
}

最終的に各パラメータは下表の通りとなる．

パラメータ名	値
a	$ 6 * (\theta_{e} - \theta_{s}) / t^{5}_{f}$
b	$-15 * (\theta_{e} - \theta_{s}) / t^{4}_{f}$
c	$ 10 * (\theta_{e} - \theta_{s}) / t^{3}_{f}$
d	$0$
e	$0$
f	$\theta_{s}$

5次補間では，下図のような軌道が生成される．

上図では，関節空間内の軌道生成であるため，後ほど順運動学(関節角度から手先位置を算出する)により，手先位置へ変換する必要がある．(関節空間内では，実際の手先位置の軌道が理解できないから，順運動学を実施して，軌道をプロットする)

後ほど，直線補間/3次補間/5次補間による軌道のアニメーションを載せる．

パターン2 (初期位置/終端位置間の軌道生成(位置空間内での軌道生成))

パターン2の内容について，説明する．
パターン1では，位置空間から関節空間に変換していたが，パターン2では位置空間内で軌道生成する．

2点間での軌道生成に関しては，以下の3パターンを提案する．

パターン	概要	メリット	デメリット
1	初期角度と終端角度を直線補間	計算が簡単	角速度を考慮できない
2	初期位置と終端位置間を3次多項式による補間	角速度を考慮できる	角加速度を考慮できない
3	初期位置と終端位置間を5次多項式による補間	角速度と角加速度を考慮できる	計算が複雑

上記パターンは，パターン1 (初期位置/終端位置を関節角度に変換(逆運動学)してから，関節角度の軌道生成(関節空間内での軌道生成)) で説明したため，割愛する．
パターン1では，$f(0) = \theta_{s}$，$f(t_{f}) = \theta_{e}$としていたが，位置空間では，$f(0) = P_{s}$，$f(t_{f}) = P_{e}$と置き換えるだけで問題ない．

軌道生成のソースコード

定数を定義するファイル (constant.py)

定数を定義するファイルを下記に記す．

constant.py

# 複数ファイルで使用する定数の定義
from enum import Enum
from enum import auto


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

# 補間時の分割する時の距離
DEVIDED_DISTANCE_JOINT      = 0.1      # 関節補間時の距離 [rad]
DEVIDED_DISTANCE_POSIION    = 0.1      # 位置補間時の距離 [m]

# プロットする時にグラフ(静止画)とするかアニメーションを定義
PLOT_NONE                 = 20          # プロットしない
PLOT_GRAPH                = 21          # グラフ
PLOT_ANIMATION            = 22          # アニメーション

# 補間方法の定義
class INTERPOLATION(Enum):
    """
    補間方法
    """
    JOINT     = 10      # 関節補間
    POSITION  = auto()  # 位置補間
    LINEAR    = auto()  # 直線補間
    CUBIC     = auto()  # 3次補間
    QUINTIC   = auto()  # 5次補間

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

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

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            all_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
        """
        raise NotImplementedError("forward_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

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

        パラメータ
            thetas(numpy.ndarray): ロボットの関節角度 [rad]
        
        戻り値
            all_link_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
        """
        # 回転角度とリンク長をローカル変数に保存
        theta1 = thetas[0]
        theta2 = thetas[1]
        link1  = self._links[0]
        link2  = self._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(self._DIMENTION_POSE)
        link1_pos = link1 * np.array([c1, s1])
        link2_pos = link1_pos + link2 * np.array([c12, s12])

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

        return all_link_pose

アニメーション作成 (animation.py)

アニメーション作成ファイルを下記に記す．

animation.py

# ロボットのアニメーションを実施

# ライブラリの読み込み
import numpy as np      # 数値計算
import matplotlib.pyplot as plt     # 描画用
import matplotlib.animation as ani  # アニメーション用

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


class RobotAnimation:
    """
    ロボットのアニメーション作成
    
    プロパティ
        _figure: 描画枠
        _axis: 描画内容
    
    publicメソッド (全てのクラスから参照可能)
        plot_Animation(): アニメーション作成
    
    protectedメソッド (自クラスまたは子クラスが参照可能)
        _reset2D(): 2次元データのリセット
    """
    # 定数の定義
    _ANIMATION_NAME = "robot_animation.gif"
    _PLOT_NAME      = "robot_plot.gif"

    def __init__(self):
        """
        コンストラクタ
        """
        pass

    def _reset2D(self):
        """
        2次元データのリセット
        """
        self._figure = plt.Figure()
        self._axis = self._figure.add_subplot(111)
        
        # X/Y軸に文字を記載
        self._axis.set_xlabel("X")
        self._axis.set_ylabel("Y")

        self._axis.grid()
        self._axis.set_aspect("equal")

    def plot_Animation(self, robot, all_link_thetas, anime_file_name=""):
        """
        アニメーション作成
        
        パラメータ
            robot(Robot2DoF): ロボットクラス
            all_link_thetas(numpy.ndarray): 全リンクの回転角度
            anime_file_name(str): アニメーションのファイル名
        """
        # 引数の確認
        if all_link_thetas.size == 0:
            raise ValueError(f"all_link_thetas's size is abnormal. all_link_thetas's size is {all_link_thetas.size}")

        # データをリセットする
        self._reset2D()

        # 全画像を保存する
        imgs = []

        # 手先位置の軌跡を保存
        position_trajectory = np.zeros((all_link_thetas.shape[0], DIMENTION_2D))

        # 軌道生成
        for i, thetas in enumerate(all_link_thetas):
            path_images = []
            # 順運動学により，全リンク (ベースリンク, リンク1，手先位置) の位置を計算
            all_link_pos = robot.forward_kinematics_all_link_pos(thetas)
            # 線プロット
            image = self._axis.plot(all_link_pos[:, 0], all_link_pos[:, 1], color="blue")
            path_images.extend(image)
            # 点プロット
            image = self._axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], color="black", alpha=0.5)
            path_images.extend([image])

            # 手先位置を保存
            position_trajectory[i] = all_link_pos[-1]
            # 手先位置の軌跡をプロット
            image = self._axis.plot(position_trajectory[:i + 1, 0], position_trajectory[:i + 1, 1], color="lime")
            path_images.extend(image)

            imgs.append(path_images)

        # アニメーション作成
        animation = ani.ArtistAnimation(self._figure, imgs)
        if anime_file_name:
            # ファイル名が存在する
            animation.save(anime_file_name, writer='imagemagick')
        else:
            # ファイル名が存在しない
            animation.save(self._ANIMATION_NAME, writer='imagemagick')
        plt.show()

軌道生成結果

main.pyを実行し，関節空間内の軌道生成と位置空間内の軌道生成のアニメーションを下図に記す．
下図は関節空間内で直線補間によるアニメーションである．

下図は関節空間内で3次多項式による補間のアニメーションである．

下図は関節空間内で5次多項式による補間のアニメーションである．

下図は位置空間内で直線補間によるアニメーションである．

下図は位置空間内で3次多項式による補間のアニメーションである．

下図は位置空間内で5次多項式による補間のアニメーションである．

上アニメーションより，以下のことをいう事ができる．
・関節空間内の軌道生成では，位置空間内の軌道生成に比べると外回りする
・3次補間や5次補間では，速度/角速度や加速度/角加速度を与える事ができるため，自由度が高い．

おわりに

本記事では，Pythonを使用して，下記内容を実装しました．
・2軸ロボットアームの軌道生成

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