はじめに

前記事では，3軸ロボットアームの微分逆運動学を用いた軌道生成を説明した．
(https://qiita.com/haruhiro1020/items/c07c2635d03cf31396f6)
本記事では，手先姿勢の軌道生成を説明する．
手先位置に関する軌道生成は容易だが，姿勢に関する軌道生成は一癖ある．
6軸ロボットアームの微分逆運動学を使用する際に，姿勢に関する誤差を計算する必要がある．要するに回転行列の誤差を計算する方法を本記事にて説明する．
姿勢に関する誤差を計算して，姿勢の軌道生成を1軸回転法と呼ばれる手法を用いて，作成する．
1軸回転法に関しては，後ほど説明する．
本記事では，下図のような姿勢に関する軌道生成が算出できるようになることが目標である．
($x$軸が$0$のデータは初期姿勢(回転行列)，$x$軸が$100$のデータは目標姿勢(回転行列)である．下図では初期姿勢から目標姿勢までの軌道を作成した．凡例は3*3行列である回転行列の各要素である．)

本記事で実装すること

・手先姿勢(回転行列)の軌道生成を実装
(6軸ロボットアームで，微分逆運動学を使用した軌道生成を実装するために必要)

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

・6軸ロボットアームで，微分逆運動学を使用した軌道生成

動作環境

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

姿勢の誤差の計算方法に関して

2つの姿勢(回転行列)の誤差を計算する方法に関して説明する．
2つの姿勢の誤差を計算するために，下図のような2つの座標系を考えてみる．

今回計算したい内容は，座標系$\sum_{a}$と$\sum_{b}$の誤差を計算することである．
座標系$\sum_{a}$の回転行列を${}^{w}R_{a}$，座標系$\sum_{b}$の回転行列を${}^{w}R_{b}$と置いて，考えてみる．
(${}^{w}R_{a}$はワールド座標系から見た，座標系$\sum_{a}$の回転行列を指す．)
座標系$\sum_{a}$の回転行列を${}^{w}R_{a}$から，座標系$\sum_{b}$の回転行列を${}^{w}R_{b}$を計算する方法は下式の通りとなる．

\displaylines{
{}^{w}R_{b} = {}^{w}R_{a} * {}^{a}R_{b} ... ① \\
{}^{a}R_{b}は座標系\sum_{a}から見た座標系\sum_{b}の回転行列を指す． \\
}

上式で考えてみると，座標系$\sum_{a}$の回転行列に何かしらの回転行列を掛け合わせると座標系$\sum_{b}$の回転行列を計算することができる．
何かしらの回転行列が今回，計算したい姿勢(回転行列)の誤差となる．
①の数式を${}^{a}R_{b}$について，解いてみると

\displaylines{
{}^{a}R_{b} = {}^{w}R_{a}^{-1} * {}^{w}R_{b} \\
{}^{w}R_{a}^{-1}は回転行列{}^{w}R_{a}の逆行列を指す． \\
}

ここで，回転行列の性質として，正規直交行列を上げられる．
そのため，回転行列の逆行列である${}^{w}R_{a}^{-1}$は下式の通りに求めることができる．

\displaylines{
{}^{w}R_{a} * {}^{w}R_{a}^{-1} = I(3) \\
I(3)は3 * 3の単位行列を指す (行列のi行i列のデータが1，それ以外のデータは0) \\
回転行列は正規直交行列であるため，回転行列の逆行列は転置行列となる \\
{}^{w}R_{a}^{-1} = {}^{w}R_{a}^{T} \\
}

上式と式①より，一般的な姿勢の誤差は下式のようになる．

\displaylines{
{}^{init}R_{target} = {}^{w}R_{init}^{T} * {}^{w}R_{target} ... ② \\
{}^{w}R_{init}はワールド座標系から見た，初期値の回転行列 \\
{}^{w}R_{target}はワールド座標系から見た，目標値の回転行列 \\
{}^{init}R_{target}は初期値の座標系から見た，目標値の回転行列 \\
}

1軸回転法に関して

姿勢軌道を算出するための1軸回転法に関して説明する．
先ほどは2つの姿勢(回転行列)の誤差に関して説明した．2つの姿勢の誤差が3*3行列である回転行列を用いて説明した．
今回は，2つの姿勢の誤差をあるベクトル($\lambda$)周りにある角度($\theta$)回転していると考える．
下図のように2つの姿勢に対して，誤差を算出するためのベクトル($\lambda$)と角度($\theta$)が存在することを考える．

$\lambda$は下式のような性質である．

\displaylines{
\lambda
=
\begin{pmatrix}
\lambda_{x} \\
\lambda_{y} \\
\lambda_{z} \\
\end{pmatrix} \\
|\lambda| = \sqrt(\lambda^{2}_{x} + \lambda^{2}_{y} + \lambda^{2}_{z}) = 1 \\
|\lambda|は\lambdaの大きさである．\lambdaは単位方向ベクトルである． \\
}

回転軸である$\lambda$と回転角度$\theta$を回転行列へ変換するためには，ロドリゲスの回転行列を用いる．
ロドリゲスの回転行列は下記サイトを参考にした．下記サイトより，回転軸と回転角度を回転行列への変換は下式の通りとなる．
(https://manabitimes.jp/math/2649)

\displaylines{
R
=
\begin{pmatrix}
(1 - \cos\theta) * \lambda^{2}_{x} + \cos\theta & 
(1 - \cos\theta) * \lambda_{x} * \lambda_{y} - \lambda_{z}  * \sin\theta & 
(1 - \cos\theta) * \lambda_{z} * \lambda_{x} + \lambda_{y}  * \sin\theta \\

(1 - \cos\theta) * \lambda_{x} * \lambda_{y} + \lambda_{z}  * \sin\theta & 
(1 - \cos\theta) * \lambda^{2}_{y} + \cos\theta & 
(1 - \cos\theta) * \lambda_{y} * \lambda_{z} - \lambda_{x}  * \sin\theta \\

(1 - \cos\theta) * \lambda_{z} * \lambda_{x} - \lambda_{y}  * \sin\theta & 
(1 - \cos\theta) * \lambda_{y} * \lambda_{z} + \lambda_{x}  * \sin\theta &
(1 - \cos\theta) * \lambda^{2}_{z} + \cos\theta \\
\end{pmatrix} ... ③ \\
}

式③より，ある1軸の回転を回転行列へ変換ができた．
次は式③より，$\theta$の算出方法を説明する．

\displaylines{
式③の回転行列を下記のように置いて考えると， \\
R
=
\begin{pmatrix}
r11 & r12 & r13 \\
r21 & r22 & r23 \\
r31 & r32 & r33 \\
\end{pmatrix} \\
r11 ~ r33の値は式③を参照ください． \\
はじめに，回転角度\thetaを算出するために，r11，r22，r33を足し合わせると \\
r11 + r22 + r33 = (1 - \cos\theta) * \lambda^{2}_{x} + \cos\theta + (1 - \cos\theta) * \lambda^{2}_{y} + \cos\theta + (1 - \cos\theta) * \lambda^{2}_{z} + \cos\theta \\
r11 + r22 + r33 = (1 - \cos\theta) * (\lambda^{2}_{x} + \lambda^{2}_{y} + \lambda^{2}_{z}) + 3 * \cos\theta \\
ここで，\lambdaは単位方向ベクトルであるため \\
\lambda^{2}_{x} + \lambda^{2}_{y} + \lambda^{2}_{z} = 1 となる． \\
r11 + r22 + r33 = (1 - \cos\theta) + 3 * \cos\theta = 1 + 2 * 3 * \cos\theta \\
ここで，r11 + r22 + r33は回転行列の対角成分の足し合わせであるため，trace(R) と置き換えることができる． \\
trace(R) = r11 + r22 + r33 = 1 + 2 * \cos\theta \\
\cos\thetaに関して解くと \\
\cos\theta = (trace(R) - 1) / 2 ... ④ \\
次に，回転行列の対角成分以外を使用して，回転軸を算出する． \\
u
=
\begin{pmatrix}
r32 - r23 \\
r13 - r31 \\
r21 - r12 \\
\end{pmatrix}
=
\begin{pmatrix}
( (1 - \cos\theta) * \lambda_{y} * \lambda_{z} + \lambda_{x}  * \sin\theta ) - ( (1 - \cos\theta) * \lambda_{y} * \lambda_{z} - \lambda_{x}  * \sin\theta ) \\
( (1 - \cos\theta) * \lambda_{z} * \lambda_{x} + \lambda_{y}  * \sin\theta ) - ( (1 - \cos\theta) * \lambda_{z} * \lambda_{x} - \lambda_{y}  * \sin\theta ) \\
( (1 - \cos\theta) * \lambda_{x} * \lambda_{y} + \lambda_{z}  * \sin\theta ) - ( (1 - \cos\theta) * \lambda_{x} * \lambda_{y} - \lambda_{z}  * \sin\theta ) \\
\end{pmatrix} \\
u
=
\begin{pmatrix}
2 * \lambda_{x} * \sin\theta \\
2 * \lambda_{y} * \sin\theta \\
2 * \lambda_{z} * \sin\theta \\
\end{pmatrix}
=
2 * \sin\theta
*
\begin{pmatrix}
\lambda_{x} \\
\lambda_{y} \\
\lambda_{z} \\
\end{pmatrix} ... ⑤ \\
両辺の大きさを取ると \\
2 * |\sin\theta| * |\lambda| = |u| \\
ここで，\lambdaは単位方向ベクトルのため，|\lambda| = 1となり， \\
2 * |\sin\theta| = |u| \\
\sin\thetaについて解くと， \\
\sin\theta = +-|u| / 2 ... ⑥ \\
式④と式⑥より，\thetaについて解くと， \\
\theta = atan2(\sin\theta, \cos\theta) = atan2(+-|u| / 2, (trace(R) - 1) / 2) \\
}

上式より，回転角度である$\theta$を算出することが可能となる．
次に，回転軸である$\lambda$の算出を実装する．
式⑤を利用すると

\displaylines{
2 * \sin\theta * \lambda = u =
\begin{pmatrix}
r32 - r23 \\
r13 - r31 \\
r21 - r12 \\
\end{pmatrix} \\
\lambdaについて解くと， \\
\lambda = u / (2 * \sin\theta) \\
ここで，\sin\theta = 0かどうかで処理が変わる \\
\sin\theta = 0の時は，\thetaが0または\piである． \\
回転軸である\lambdaを下記のように適当な値とする．　\\
\lambda = [1, 0, 0] \\
\sin\theta \neq 0の時は，
\lambda = u / (2 * \sin\theta) \\
}

上記にて，1軸回転法で使用する回転軸と回転角度を算出することが可能となる．
次は，手先姿勢の軌道生成に関して説明する．

手先姿勢の軌道生成

姿勢に関する軌道生成を説明する．
前章にて，1軸回転法で使用する回転軸と回転角度を算出した．
姿勢の軌道生成では，回転軸と回転角度を使用する．
姿勢軌道は下式のように定義することができる．

\displaylines{
{}^{w}R_{trajectory}(\theta(t)) = {}^{w}R_{init} * {}^{init}R_{target}(\theta(t)) \\
{}^{w}R_{trajectory}(t)は姿勢軌道． \\
{}^{w}R_{init}はワールド座標系から見た初期値の姿勢． \\
{}^{init}R_{target}(t)は初期値の姿勢から見た目標値の姿勢の中間姿勢．\theta(t)=0は初期値の姿勢となり，\theta(t)=1は目標値の姿勢となる． \\
\theta(t)は0〜\thetaの値．\thetaは1軸回転法で算出した回転角度． \\
}

手先姿勢に関する軌道生成 (ソースコード)

手先姿勢の軌道生成を実施するソースコードを記載する．

定数を定義するソースコード (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軸周りに回転
ROTATION_X_NEGATIVE_AXIS = "rot_neg_x"  # x軸周りに逆回転
ROTATION_Y_NEGATIVE_AXIS = "rot_neg_y"  # y軸周りに逆回転
ROTATION_Z_NEGATIVE_AXIS = "rot_neg_z"  # z軸周りに逆回転

# 0割を防ぐための定数
EPSILON                   = 1e-6

1軸回転法などの回転行列を定義するソースコード (rotation.py)

1軸回転法などの回転行列を定義するソースコードを下記に記載する．

rotation.py

# 回転行列の定義

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


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


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



class MyRotation:
    """
    回転行列クラス
    """
    _PITCH_THRESHOLD = 1e-4             # ピッチ角の閾値
    _ZERO_NEAR = 1e-4                   # 0近傍の閾値
    _EPSILON   = 1e-5                   # 微小値
    
    _ROT_MAX_VALUE =  1.0           # 回転行列の最大値
    _ROT_MIN_VALUE = -1.0           # 回転行列の最小値
    
    
    def _rot_x(self, theta):
        """
        x軸方向にtheta[rad]回転させる回転行列

        パラメータ
            theta(float): 回転角度 [rad]
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        rotation = np.array([[1, 0,              0            ],
                             [0, np.cos(theta), -np.sin(theta)],
                             [0, np.sin(theta),  np.cos(theta)]])

        return rotation
    
    def _rot_y(self, theta):
        """
        y軸方向にtheta[rad]回転させる回転行列

        パラメータ
            theta(float): 回転角度 [rad]
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        rotation = np.array([[ np.cos(theta), 0, np.sin(theta)],
                             [ 0,             1, 0            ],
                             [-np.sin(theta), 0, np.cos(theta)]])

        return rotation
    
    def _rot_z(self, theta):
        """
        z軸方向にtheta[rad]回転させる回転行列

        パラメータ
            theta(float): 回転角度 [rad]
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        rotation = np.array([[np.cos(theta), -np.sin(theta), 0],
                             [np.sin(theta),  np.cos(theta), 0],
                             [0,              0,             1]])

        return rotation

    def rot(self, theta, axis):
        """
        回転軸に応じた回転行列の取得

        パラメータ
            theta(float): 回転角度 [rad]
            axis(str): 回転軸
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        if axis == ROTATION_X_AXIS:
            # 回転軸がx軸
            rotation = self._rot_x(theta)
        elif axis == ROTATION_Y_AXIS:
            # 回転軸がy軸
            rotation = self._rot_y(theta)
        elif axis == ROTATION_Z_AXIS:
            # 回転軸がz軸
            rotation = self._rot_z(theta)
        elif axis == ROTATION_X_NEGATIVE_AXIS:
            # 回転軸がx軸だが，逆回転
            rotation = self._rot_x(-theta)
        elif axis == ROTATION_Y_NEGATIVE_AXIS:
            # 回転軸がy軸だが，逆回転
            rotation = self._rot_y(-theta)
        elif axis == ROTATION_Z_NEGATIVE_AXIS:
            # 回転軸がz軸だが，逆回転
            rotation = self._rot_z(-theta)
        else:
            # 異常
            raise ValueError(f"axis is abnormal. now is {axis}")

        return rotation

    def rot_from_zyx_euler(self, euler):
        """
        Z(theta1)-Y(theta2)-X(theta3)オイラー角による回転行列の計算
        
        パラメータ
            euler(float): Z(theta1)-Y(theta2)-X(theta3)オイラー角 [rad]
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        # 引数を分解する
        theta1 = euler[0]
        theta2 = euler[1]
        theta3 = euler[2]

        # 回転行列を計算
        rot_z  = self._rot_z(theta1)
        rot_y  = self._rot_y(theta2)
        rot_x  = self._rot_x(theta3)

        # Z-Y-X軸に順番に回転させた時の回転行列を計算
        rotation = np.dot(rot_z, rot_y)
        rotation = np.dot(rotation, rot_x)

        return rotation

    def rot_to_zyx_euler(self, rotation, y_cos_plus=True):
        """
        回転行列からZ(theta1)-Y(theta2)-X(theta3)オイラー角を取得
        
        パラメータ
            rotation(numpy.ndarray): 回転行列
            y_cos_plus(bool): Y(theta2)を+にするか
        
        戻り値
            euler(numpy.ndarray): Z(theta1)-Y(theta2)-X(theta3)オイラー角 [rad] の順番にデータを保存
        """
        # 引数の確認
        if rotation.shape != (3, 3):
            # 回転行列ではない
            raise ValueError(f"rotation's shape is abnormal. rotaton'shape is {rotation.shape}")

        # 回転行列の要素を一度，ローカル変数に保存
        # r11 =  cos(theta1) * cos(theta2)
        # r12 = -sin(theta1) * cos(theta3) + cos(theta1) * sin(theta2) * sin(theta3)
        # r13 =  sin(theta1) * sin(theta3) + cos(theta1) * sin(theta2) * cos(theta3)
        r11, r12, r13 = rotation[0, :]

        # r21 =  sin(theta1) * cos(theta2)
        # r22 =  cos(theta1) * cos(theta3) + sin(theta1) * sin(theta2) * sin(theta3)
        # r23 = -cos(theta1) * sin(theta3) + sin(theta1) * sin(theta2) * cos(theta3)
        r21, r22, r23 = rotation[1, :]

        # r31 = -sin(theta2)
        # r32 =  cos(theta2) * sin(theta3)
        # r33 =  cos(theta2) * cos(theta3)
        r31, r32, r33 = rotation[2, :]

        # はじめにtheta2から求める
        # r32 ** 2 + r33 * 2 = cos^2(theta2)
        # r31 = -sin(theta2)
        # theta2 = np.atan2(sin(theta2), cos(theta2)) = np.atan2(-r31, root(r32 ** 2 + r33 ** 2))
        theta2_sin = -r31
        if y_cos_plus:
            theta2_cos =  np.sqrt(r32 ** 2 + r33 ** 2)
        else:
            theta2_cos = -np.sqrt(r32 ** 2 + r33 ** 2)
        theta2 = np.arctan2(theta2_sin, theta2_cos)

        if abs(theta2 - np.pi / 2) <= self._PITCH_THRESHOLD:
            # theta2がpi/2付近では特異点となる
            # cos(theta2) = 0でsin(theta2) = 1となる
            # r13 =  sin(theta1) * sin(theta3) + cos(theta1) * sin(theta2) * cos(theta3) =  sin(theta1) * sin(theta3) + cos(theta1) * cos(theta3) = cos(theta1 - theta3)
            # r23 = -cos(theta1) * sin(theta3) + sin(theta1) * sin(theta2) * cos(theta3) = -cos(theta1) * sin(theta3) + sin(theta1) * cos(theta3) = sin(theta1 - theta3)
            # theta1 - theta3 = np.atan2(sin(theta1 - theta3), cos(theta1 - theta3)) = np.atan2(r23, r13)
            # theta1 = 0とすると theta3 = -np.atan2(r23, r13)となる
            theta1 = 0.0
            theta3 = -np.arctan2(r23, r13)
        elif abs(theta2 + np.pi / 2) <= self._PITCH_THRESHOLD:
            # theta2が-pi/2付近では特異点となる
            # cos(theta2) = 0でsin(theta2) = -1となる
            # r13 =  sin(theta1) * sin(theta3) + cos(theta1) * sin(theta2) * cos(theta3) =  sin(theta1) * sin(theta3) - cos(theta1) * cos(theta3) = -cos(theta1 + theta3)
            # r23 = -cos(theta1) * sin(theta3) + sin(theta1) * sin(theta2) * cos(theta3) = -cos(theta1) * sin(theta3) - sin(theta1) * cos(theta3) = -sin(theta1 + theta3)
            # theta1 + theta3 = np.atan2(sin(theta1 + theta3), cos(theta1 + theta3)) = np.atan2(-r23, -r13)
            # theta1 = 0とすると theta3 = np.atan2(-r23, -r13)となる
            theta1 = 0.0
            theta3 = np.arctan2(-r23, -r13)
        else:
            # 特異点付近ではない場合
            # r21 = sin(theta1) * cos(theta2)
            # r11 = cos(theta1) * cos(theta2)
            # theta1 = np.atan2(sin(theta1), cos(theta1)) = np.atan2(r21/cos(theta2), r11/cos(theta2)) = np.atan2(r21, r11)
            theta1 = np.arctan2(r21, r11)

            # r32 = cos(theta2) * sin(theta3)
            # r33 = cos(theta2) * cos(theta3)
            # theta3 = np.atan2(sin(theta3), cos(theta3)) = np.atan2(r32/cos(theta2), r33/cos(theta2)) = np.atan2(r32, r33)
            theta3 = np.arctan2(r32, r33)

        # Z(theta1)-Y(theta2)-X(theta3)オイラー角の順番に保存
        rpy = np.array([theta1, theta2, theta3])

        return rpy

    def rot_to_zyz_euler(self, rotation):
        """
        回転行列からZ(theta1)-Y(theta2)-Z(theta3)オイラー角を取得
        
        パラメータ
            rotation(numpy.ndarray): 回転行列
        
        戻り値
            euler(numpy.ndarray): Z(theta1)-Y(theta2)-Z(theta3) [rad] の順番にデータを保存
        """
        # 引数の確認
        if rotation.shape != (3, 3):
            # 回転行列ではない
            raise ValueError(f"rotation's shape is abnormal. rotaton'shape is {rotation.shape}")

        # 回転行列の要素を一度，ローカル変数に保存
        # r11 =  c1 * c2 * c3 - s1 * s3
        # r12 = -c1 * c2 * s3 - s1 * c3
        # r13 = c1 * s2
        r11, r12, r13 = rotation[0, :]

        # r21 = s1 * c2 * c3 + c1 * s3
        # r22 = -s1 * c2 * s3 + c1 * c3
        # r23 = s1 * s2
        r21, r22, r23 = rotation[1, :]

        # r31 = -s2 * c3
        # r32 = s2 * s3
        # r33 = c2
        r31, r32, r33 = rotation[2, :]

        # Y(theta2)の値から求める
        # r33 = c2よりtheta2 = arccos(r33)
        # cos()は-1 ~ 1の範囲のため，制限しておく
        r33 = np.clip(r33, self._ROT_MIN_VALUE, self._ROT_MAX_VALUE)
        # arccos()には2通りの解が存在する cos(theta) = cos(-theta) だから
        theta2 = np.arccos(r33)

        if abs(r33 - 1) <= self._ZERO_NEAR:
            # cos(theta2)が1近傍の時は，特異点
            # r11 = c1 * c2 * c3 - s1 * s3 で c2 = 1 より，r11 = c1 * c3 - s1 * s3 = c13
            # r21 = s1 * c2 * c3 + c1 * s3 で c2 = 1 より，r21 = s1 * c3 + c1 * s3 = s13
            # よって，theta1 + theta3 = np.atan2(r21, r11) で，theta1 = 0 とする
            theta1 = 0.0
            theta3 = np.arctan2(r21, r11)
        elif abs(r33 + 1) <= self._ZERO_NEAR:
            # cos(theta2)が-1近傍の時も特異点
            # r11 = c1 * c2 * c3 - s1 * s3 で c2 = -1 より，r11 = -c1 * c3 - s1 * s3 = -c(1-3)
            # r21 = s1 * c2 * c3 + c1 * s3 で c2 = -1 より，r21 = -s1 * c3 + c1 * s3 = -s(1-3)
            # よって，theta1 - theta3 = np.atan2(-r21, -r11) で，theta1 = 0 とする
            theta1 = 0.0
            theta3 = -np.arctan2(-r21, -r11)
        else:
            # 特異点以外の場合
            # r32 =  s2 * s3よりs3 = r32 / s2
            # r31 = -s2 * c3よりc3 = r31 / -s2
            # よって，theta3 = np.atan2(r32, -r31)
            theta3 = np.arctan2(r32, -r31)
            # r23 = s1 * s2よりs1 = r23 / s2
            # r13 = c1 * s2よりc1 = r13 / s2
            # よって，theta1 = np.atan2(r23, r13)
            theta1 = np.arctan2(r23, r13)

        # Z(theta1)-Y(theta2)-Z(theta3)の順番に保存
        euler = np.array([theta1, theta2, theta3])

        return euler

    def _get_single_axis(self, init_rot, target_rot, t):
        """
        一軸回転法 (single axis rotation method) による回転角度と回転軸を算出

        パラメータ
            init_rot(numpy.ndarray): 初期姿勢
            target_rot(numpy.ndarray): 目標姿勢
            t(float): 目標姿勢への割合 (0 ~ 1)
        
        戻り値
            sigle_axis(numpy.ndarray): 一軸回転(回転角度(スカラー)，回転軸(3ベクトル))
        """
        single_axis = np.zeros(4)

        # 引数の範囲確認
        if t < 0 or t > 1:
            raise ValueError(f"t is abnormal. t's range is 0 to 1. t is {t}")

        # 2姿勢の差分を計算
        diff_rot = np.dot(init_rot.T, target_rot)
        # 回転角度のcos()を計算
        # cos(theta) = (trace(diff_rot) - 1) / 2
        # cos(theta)の値を最小値が-1，最大値が+1となるようにnp.clip()を採用
        cos_rot_angle = np.clip((np.trace(diff_rot) - 1) / 2, self._ROT_MIN_VALUE, self._ROT_MAX_VALUE)

        # 回転軸関連のデータを計算
        # u = [r32 - r23, r13 - r31, r21 - r12] = 2 * sin(theta) * lambda
        # 両辺の大きさをとり，|lambda| = 1の性質を使用し，sin(theta)について解くと
        # sin(theta) = 1 / 2 * |u|
        u = np.array([diff_rot[2, 1] - diff_rot[1, 2], diff_rot[0, 2] - diff_rot[2, 0], diff_rot[1, 0] - diff_rot[0, 1]])
        sin_rot_angle = 1 / 2 * np.linalg.norm(u)

        # sin()，cos()からthetaを算出
        rot_angle = np.arctan2(sin_rot_angle, cos_rot_angle)
        # rot_angle = np.arccos(cos_rot_angle)

        # 回転軸を算出する
        # lambda = u / (2 * sin(theta))
        rot_axis = u / (2 * np.sin(rot_angle) + self._EPSILON)
        # rot_axis = rot_angle / 
        # 正規化する
        rot_axis = rot_axis / (np.linalg.norm(rot_axis) + self._EPSILON)

        single_axis[0]  = rot_angle * t
        single_axis[1:] = rot_axis

        return single_axis

    def _rot_from_single_axis(self, single_axis):
        """
        一軸回転法から回転行列を算出

        パラメータ
            single_axis(numpy.ndarray): 一軸回転法で算出した一軸(回転角度(スカラー)と回転軸(3ベクトル)
        
        戻り値
            rotation(numpy.ndarray): 回転行列
        """
        # パラメータを分解する
        theta = single_axis[0]
        lambda_x = single_axis[1]
        lambda_y = single_axis[2]
        lambda_z = single_axis[3]

        # 三角関数を算出する
        sin = np.sin(theta)
        cos = np.cos(theta)
        one_minus_cos = 1 - cos

        # r11 = (1 - cos(theta)) * lambda_x ** 2 + cos(theta)
        # r12 = (1 - cos(theta)) * lambda_x * lambda_y - lambda_z * sin(theta)
        # r13 = (1 - cos(theta)) * lambda_z * lambda_x + lambda_y * sin(theta)
        # r21 = (1 - cos(theta)) * lambda_x * lambda_y + lambda_z * sin(theta)
        # r22 = (1 - cos(theta)) * lambda_y ** 2 + cos(theta)
        # r23 = (1 - cos(theta)) * lambda_y * lambda_z - lambda_x * sin(theta)
        # r31 = (1 - cos(theta)) * lambda_z * lambda_x - lambda_y * sin(theta)
        # r32 = (1 - cos(theta)) * lambda_y * lambda_z + lambda_x * sin(theta)
        # r33 = (1 - cos(theta)) * lambda_z ** 2 + cos(theta)
        r11 = one_minus_cos * lambda_x ** 2 + cos
        r12 = one_minus_cos * lambda_x * lambda_y - lambda_z * sin
        r13 = one_minus_cos * lambda_z * lambda_x + lambda_y * sin
        r21 = one_minus_cos * lambda_x * lambda_y + lambda_z * sin
        r22 = one_minus_cos * lambda_y ** 2 + cos
        r23 = one_minus_cos * lambda_y * lambda_z - lambda_x * sin
        r31 = one_minus_cos * lambda_z * lambda_x - lambda_y * sin
        r32 = one_minus_cos * lambda_y * lambda_z + lambda_x * sin
        r33 = one_minus_cos * lambda_z ** 2 + cos

        rotation = np.array([[r11, r12, r13],
                             [r21, r22, r23],
                             [r31, r32, r33]])

        return rotation

    def intermediate_rot(self, init_rot, target_rot, t):
        """
        初期姿勢と目標姿勢の中間姿勢を計算

        パラメータ
            init_rot(numpy.ndarray): 初期姿勢
            target_rot(numpy.ndarray): 目標姿勢
            t(float): 目標姿勢への割合 (0 ~ 1)
        
        戻り値  
            rotation(numpy.ndarray): 中間姿勢
        """
        # 引数の範囲確認
        if init_rot.shape != (3, 3):
            # 回転行列ではない
            raise ValueError(f"init_rot' shape is abnormal. init_rot.shape is {init_rot.shape}")

        if target_rot.shape != (3, 3):
            # 回転行列ではない
            raise ValueError(f"target_rot' shape is abnormal. target_rot.shape is {target_rot.shape}")

        if t < 0 or t > 1:
            raise ValueError(f"t is abnormal. t's range is 0 to 1. t is {t}")

        # 1軸回転法により，回転角度と回転軸を算出
        single_axis = self._get_single_axis(init_rot, target_rot, t)

        # 回転角度と回転軸から，回転行列に変換
        sub_rotation = self._rot_from_single_axis(single_axis)

        # 目標姿勢 = 初期姿勢 * 差分姿勢
        rotation = np.dot(init_rot, sub_rotation)

        return rotation

軌道生成を実装するメイン処理 (main.py)

軌道生成を実装するメイン処理は下記に記載する．

main.py

# メイン処理

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


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



N_MIDDLE = 101          # 中間姿勢の数
COLOR = ["red", "blue", "black", "orange", "purple", "green", "magenta", "lime", "cyan"]
FILE_NAME = "rotation_trajectory.png"


def main():
    """
    メイン処理
    """
    rot = MyRotation()
    # 初期角度と目標角度より，初期姿勢と目標姿勢を算出
    init_thetas = np.deg2rad(np.array([0, 0, 0]))
    target_thetas = np.deg2rad(np.array([90, 60, 30]))
    init_rot = rot.rot_from_zyx_euler(init_thetas)
    target_rot = rot.rot_from_zyx_euler(target_thetas)

    # 回転行列の軌道を保存
    plot_rotation = np.zeros((N_MIDDLE, 3, 3))


    # 一軸回転法
    for i in range(N_MIDDLE):
        t = i / 100
        # t = 1
        print(f"t = {t}")
        middle_rot = rot.intermediate_rot(init_rot, target_rot, t)
        plot_rotation[i] = middle_rot

    # print(f"diff_rot = {np.dot(init_rot.T, target_rot)}")
    # print(f"plot_rotation[-1] = {plot_rotation[-1]}")

    # 回転行列の各要素を描画
    plt.plot(plot_rotation[:, 0, 0], label="r11", color=COLOR[0])
    plt.plot(plot_rotation[:, 0, 1], label="r12", color=COLOR[1])
    plt.plot(plot_rotation[:, 0, 2], label="r13", color=COLOR[2])
    plt.plot(plot_rotation[:, 1, 0], label="r21", color=COLOR[3])
    plt.plot(plot_rotation[:, 1, 1], label="r22", color=COLOR[4])
    plt.plot(plot_rotation[:, 1, 2], label="r23", color=COLOR[5])
    plt.plot(plot_rotation[:, 2, 0], label="r31", color=COLOR[6])
    plt.plot(plot_rotation[:, 2, 1], label="r32", color=COLOR[7])
    plt.plot(plot_rotation[:, 2, 2], label="r33", color=COLOR[8])

    # 初期値をプロット
    plot_scatter(init_rot, 0)
    # 目標位置をプロット
    plot_scatter(target_rot, N_MIDDLE)

    plt.legend()
    plt.grid()

    # ファイル保存
    plt.savefig(FILE_NAME)

    plt.show()

def plot_scatter(rot, x):
    """
    各要素を点プロット

    パラメータ
        rot(numpy.ndarray): 回転行列
    """
    plt.scatter(x, rot[0, 0], color=COLOR[0])
    plt.scatter(x, rot[0, 1], color=COLOR[1])
    plt.scatter(x, rot[0, 2], color=COLOR[2])
    plt.scatter(x, rot[1, 0], color=COLOR[3])
    plt.scatter(x, rot[1, 1], color=COLOR[4])
    plt.scatter(x, rot[1, 2], color=COLOR[5])
    plt.scatter(x, rot[2, 0], color=COLOR[6])
    plt.scatter(x, rot[2, 1], color=COLOR[7])
    plt.scatter(x, rot[2, 2], color=COLOR[8])


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

結果

main.pyを実行すると，"png"が作成される．
姿勢の軌道生成は下図のようになった．
画像の見方は以下の通りである．
・$x$軸が$0$のデータ：姿勢の初期値
・$x$軸が$100$のデータ：姿勢の目標値
・r11 ~ r33：3 * 3行列である回転行列の各要素

おわりに

本記事では，Pythonを使用して，下記内容を実装しました．
・1軸回転法を使用して，手先姿勢の軌道生成を実装

次記事では，下記内容を実装していきます．
・1軸回転法を使用して，6軸ロボットアームの微分逆運動学を実装