はじめに
私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
前記事では，6軸ロボットアームの順運動学を実装した．
(https://qiita.com/haruhiro1020/items/a240f8c8e538e75473a3)
本記事では，6軸ロボットアームの順運動学を説明する．
順運動学とは，関節角度から手先位置を計算することです．
手先位置を算出するために，三角関数(sin, cos, tan)を使用する．以下サイトはわかりやすいです．
・https://www.meikogijuku.jp/meiko-plus/study/trigonometric-function.html
本記事で実装すること
・6軸ロボットアームの逆運動学を算出する．
本記事では実装できないこと (将来実装したい内容)
・6軸ロボットアームの経路生成(RRT)
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)
6軸ロボットアームの逆運動学
6軸ロボットアームの逆運動学を実施する．
今回は下図のような6軸ロボットアームを考える．
関節$1$はz軸周りに$\theta_{1}$で回転し，関節$2$はy軸周りに$\theta_{2}$で回転し，関節$3$はy軸周りに$\theta_{3}$で回転し，関節$4$はz軸周りに$\theta_{4}$で回転し，関節$5$はy軸周りに$\theta_{5}$で回転し，関節$6$はz軸周りに$\theta_{6}$で回転する．
関節$1$~$6$全部が角度$0 [rad]$の時は，下図のようになる6軸ロボットアームを考える．
(全関節の座標系がワールド座標系と同じになる)
6つの関節角度を算出する必要がある．
関節角度の算出順番は以下の通りである．
１：$\theta_{3}$の算出
２：$\theta_{1}$の算出
３：$\theta_{2}$の算出
４：$\theta_{5}$の算出
５：$\theta_{4}$の算出
６：$\theta_{6}$の算出
theta1 ~ theta3 の算出
$\theta_{1}$ ~ $\theta_{3}$と$\theta_{4}$ ~ $\theta_{6}$の2つに分けて説明する．
ワールド座標系から見た関節5の位置を算出
順運動学により，手先位置${}^{w}p_{e}$と手先姿勢${}^{w}R_{e}$が算出されている．
はじめにワールド座標系から見た関節5の位置を計算する．
関節5の位置の算出方法は下図を使用しながら，説明する．
\displaylines{
{}^{w}p_{e} = {}^{w}p_{5} + {}^{5}R_{e} * {}^{5}p_{e} \\
{}^{w}p_{5} = {}^{w}p_{e} - {}^{5}R_{e} * {}^{5}p_{e} \\
{}^{w}p_{e} ... ワールド座標系から見た手先位置 \\
{}^{w}p_{5} ... ワールド座標系から見た関節5の位置 \\
}
${}^{5}R_{e} * {}^{5}p_{e}$を算出できれば，${}^{w}p_{5}$を算出することが可能となる．
ここで上図の手先の座標系$\sum_{e}$を見てみると，手先の座標系の$z$成分である$Z_{e}$方向と逆方向に$l_{5} + l_{6}$移動させると，${}^{w}p_{5}$と一致することが確認できる．
関節6の回転方向が$z$軸であるため，$\theta_{6}$を回転させても手先の位置には影響がないため，$Z_{e}$方向と逆方向に$l_{5} + l_{6}$移動させることで，${}^{w}p_{5}$と一致する．
よって，上式を下式のように修正することが可能となる．
\displaylines{
{}^{w}p_{5} = {}^{w}p_{e} - Z_{e} * {}^{5}p_{e} ... ① \\
手先の回転行列(3*3行列)は下記のように，X_{e}(3*1行列)とY_{e}(3*1行列)，Z_{e}(3*1行列)に分解できる\\
{}^{w}R_{e}
=
\begin{pmatrix}
X_{e} & Y_{e} & Z_{e} \\
\end{pmatrix} \\
{}^{5}p_{e}は下記の通りである．\\
{}^{5}p_{e}
=
\begin{pmatrix}
0 \\
0 \\
l_{5} + l_{6} \\
\end{pmatrix} \\
}
上式により，${}^{w}p_{5}$を算出することが可能となる．
次は，同時変換行列を使用して，${}^{w}p_{5}$を算出する．
\displaylines{
{}^{w}\tilde p_{5} = {}^{w}T_{4} * {}^{4}\tilde p_{5} \\
ワールド座標系から見た関節4の同時変換行列は下記の通りとなる．\\
{}^{w}T_{4} = {}^{w}T_{1} * {}^{1}T_{2} * {}^{2}T_{3} * {}^{3}T_{4} \\
{}^{4}\tilde p_{5}
=
\begin{pmatrix}
0 \\
0 \\
l_{4} \\
1 \\
\end{pmatrix} \\
{}^{w}T_{1}
=
\begin{pmatrix}
\cos\theta_{1} & -\sin\theta_{1} & 0 & 0 \\
\sin\theta_{1} & \cos\theta_{1} & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1 \\
\end{pmatrix} ,
{}^{1}T_{2}
=
\begin{pmatrix}
\cos\theta_{2} & 0 & \sin\theta_{2} & 0 \\
0 & 1 & 0 & 0 \\
-\sin\theta_{2} & 0 & \cos\theta_{2} & l_{1} \\
0 & 0 & 0 & 1 \\
\end{pmatrix} \\
{}^{2}T_{3}
=
\begin{pmatrix}
\cos\theta_{3} & 0 & \sin\theta_{3} & 0 \\
0 & 1 & 0 & 0 \\
-\sin\theta_{3} & 0 & \cos\theta_{3} & l_{2} \\
0 & 0 & 0 & 1 \\
\end{pmatrix} ,
{}^{3}T_{4}
=
\begin{pmatrix}
\cos\theta_{4} & -\sin\theta_{4} & 0 & 0 \\
\sin\theta_{4} & \cos\theta_{4} & 0 & 0 \\
0 & 0 & 1 & l_{3} \\
0 & 0 & 0 & 1 \\
\end{pmatrix} \\
計算すると，{}^{w}\tilde p_{5}は下記のようになる．\\
{}^{w}\tilde p_{5}
=
\begin{pmatrix}
\cos\theta_{1} * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) ) \\
\sin\theta_{1} * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) ) \\
l_{1} + l_{2} * \cos\theta_{2} + (l_{3} + l_{4}) * \cos(\theta_{2} + \theta_{3}) \\
1 \\
\end{pmatrix}
=
\begin{pmatrix}
{}^{w}p_{5, x} \\
{}^{w}p_{5, y} \\
{}^{w}p_{5, z} \\
1 \\
\end{pmatrix} ... ② \\
}
①と②を使用して，$\theta_{1}$ ~ $\theta_{3}$を算出していく．②より，関節5の位置は$\theta_{1}$ ~ $\theta_{3}$で算出することができる．
theta3 の算出
②の${}^{w}p_{5, x}$と${}^{w}p_{5, y}$，${}^{w}p_{5, z}$を二乗して足し合わせる．
\displaylines{
{}^{w}p^{2}_{5, x} = \cos^{2}\theta_{1} * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) )^{2} \\
{}^{w}p^{2}_{5, y} = \sin^{2}\theta_{1} * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) )^{2} \\
({}^{w}p_{5, z} - l_{1})^{2} = (l_{2} * \cos\theta_{2} + (l_{3} + l_{4}) * \cos(\theta_{2} + \theta_{3}) )^{2} \\
{}^{w}p^{2}_{5, x} + {}^{w}p^{2}_{5, y} + ({}^{w}p_{5, z} - l_{1})^{2} = (\cos^{2}\theta_{1} + \sin^{2}\theta_{1}) * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) )^{2} + (l_{2} * \cos\theta_{2} + (l_{3} + l_{4}) * \cos(\theta_{2} + \theta_{3}) )^{2} \\
{}^{w}p^{2}_{5, x} + {}^{w}p^{2}_{5, y} + ({}^{w}p_{5, z} - l_{1})^{2} = (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) )^{2} + (l_{2} * \cos\theta_{2} + (l_{3} + l_{4}) * \cos(\theta_{2} + \theta_{3}) )^{2} \\
{}^{w}p^{2}_{5, x} + {}^{w}p^{2}_{5, y} + ({}^{w}p_{5, z} - l_{1})^{2} = (l^{2}_{2} * \sin^{2}\theta_{2} + 2 * l_{2} * (l_{3} + l_{4}) * \sin\theta_{2} * \sin(\theta_{2} + \theta_{3}) + (l_{3} + l_{4})^{2} * \sin^{2}(\theta_{2} + \theta_{3})) + (l^{2}_{2} * \cos^{2}\theta_{2} + 2 * l_{2} * (l_{3} + l_{4}) * \cos\theta_{2} * \cos(\theta_{2} + \theta_{3}) + (l_{3} + l_{4})^{2} * \cos^{2}(\theta_{2} + \theta_{3})) \\
{}^{w}p^{2}_{5, x} + {}^{w}p^{2}_{5, y} + ({}^{w}p_{5, z} - l_{1})^{2} = l^{2}_{2} + (l_{3} + l_{4})^{2} + 2 * l_{2} * (l_{3} + l_{4}) * (\sin\theta_{2} * \sin(\theta_{2} + \theta_{3}) + \cos\theta_{2} * \cos(\theta_{2} + \theta_{3})) \\
{}^{w}p^{2}_{5, x} + {}^{w}p^{2}_{5, y} + ({}^{w}p_{5, z} - l_{1})^{2} = l^{2}_{2} + (l_{3} + l_{4})^{2} + 2 * l_{2} * (l_{3} + l_{4}) * (\sin\theta_{2} * \sin\theta_{2} * \cos\theta_{3} + \sin\theta_{2} * \cos\theta_{2} * \sin\theta_{3} + \cos\theta_{2} * \cos\theta_{2} * \cos\theta_{3} - \cos\theta_{2} * \sin\theta_{2} * \sin\theta_{3}) \\
{}^{w}p^{2}_{5, x} + {}^{w}p^{2}_{5, y} + ({}^{w}p_{5, z} - l_{1})^{2} = l^{2}_{2} + (l_{3} + l_{4})^{2} + 2 * l_{2} * (l_{3} + l_{4}) * \cos\theta_{3} \\
2 * l_{2} * (l_{3} + l_{4}) * \cos\theta_{3} = ({}^{w}p^{2}_{5, x} + {}^{w}p^{2}_{5, y} + ({}^{w}p_{5, z} - l_{1})^{2}) - (l^{2}_{2} + (l_{3} + l_{4})^{2}) \\
\cos\theta_{3} = ( ({}^{w}p^{2}_{5, x} + {}^{w}p^{2}_{5, y} + ({}^{w}p_{5, z} - l_{1})^{2}) - (l^{2}_{2} + (l_{3} + l_{4})^{2}) ) / (2 * l_{2} * (l_{3} + l_{4})) \\
\sin\theta_{3} = +-\sqrt(1 - \cos^{2}\theta_{3}) \\
\theta_{3} = atan2(\sin\theta_{3}, \cos\theta_{3}) \\
}
上式より，$\theta_{3}$ を算出することができる．
theta1 の算出
②の${}^{w}p_{5, x}$と${}^{w}p_{5, y}$を使用して，$\theta_{1}$を算出する．
\displaylines{
{}^{w}p_{5, x} = \cos\theta_{1} * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) ) より，\cos\theta_{1}について解くと \\
\cos\theta_{1} = {}^{w}p_{5, x} / (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) ) \\
{}^{w}p_{5, y} = \sin\theta_{1} * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) ) より，\sin\theta_{1}について解くと \\
\sin\theta_{1} = {}^{w}p_{5, y} / (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) ) \\
\theta_{1} = atan2(\sin\theta_{1}, \cos\theta_{1}) = atan2({}^{w}p_{5, y}, {}^{w}p_{5, x}) \\
{}^{w}p_{5, x} == 0かつ{}^{w}p_{5, y} == 0ならば，特異点となる．\\
特異点の時は，\theta_{1} = 0とおく\\
}
上式より，$\theta_{1}$を算出することが可能となる．
theta2 の算出
②の${}^{w}p_{5, x}$と${}^{w}p_{5, y}$を二乗して足し合わせ，${}^{w}p_{5, z}$と行列形式で記述することによって，$\theta_{2}$を算出する．
\displaylines{
{}^{w}p_{5, x}^{2} = \cos^{2}\theta_{1} * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}))^{2} \\
{}^{w}p_{5, y}^{2} = \sin^{2}\theta_{1} * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}))^{2} \\
{}^{w}p_{5, x}^{2} + {}^{w}p_{5, y}^{2} = (\cos^{2}\theta_{1} + \sin^{2}\theta_{1}) * (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}))^{2} \\
{}^{w}p_{5, x}^{2} + {}^{w}p_{5, y}^{2} = (l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}))^{2} \\
両辺の平方根を取り，(l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}))で解くと \\
(l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3})) = +-\sqrt({}^{w}p_{5, x}^{2} + {}^{w}p_{5, y}^{2}) \\
{}^{w}p_{5, z} - l_{1} = l_{2} * \cos\theta_{2} + (l_{3} + l_{4}) * \cos(\theta_{2} + \theta_{3}) \\
+-\sqrt({}^{w}p_{5, x}^{2} + {}^{w}p_{5, y}^{2})と{}^{w}p_{5, z} - l_{1}を行列形式で記載すると \\
\begin{pmatrix}
+-\sqrt({}^{w}p_{5, x}^{2} + {}^{w}p_{5, y}^{2}) \\
{}^{w}p_{5, z} - l_{1} \\
\end{pmatrix}
=
\begin{pmatrix}
l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * \sin(\theta_{2} + \theta_{3}) \\
l_{2} * \cos\theta_{2} + (l_{3} + l_{4}) * \cos(\theta_{2} + \theta_{3}) \\
\end{pmatrix}
=
\begin{pmatrix}
l_{2} * \sin\theta_{2} + (l_{3} + l_{4}) * (\sin\theta_{2} * \cos\theta_{3} + \cos\theta_{2} * \sin\theta_{3}) \\
l_{2} * \cos\theta_{2} + (l_{3} + l_{4}) * (\cos\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \sin\theta_{3}) \\
\end{pmatrix} \\
\begin{pmatrix}
+-\sqrt({}^{w}p_{5, x}^{2} + {}^{w}p_{5, y}^{2}) \\
{}^{w}p_{5, z} - l_{1} \\
\end{pmatrix}
=
\begin{pmatrix}
\sin\theta_{2} * (l_{2} + (l_{3} + l_{4}) * \cos\theta_{3}) + \cos\theta_{2} * ((l_{3} + l_{4}) * \sin\theta_{3}) \\
\sin\theta_{2} * ((l_{3} * l_{4}) * \sin\theta_{3}) + \cos\theta_{2} * (l_{2} + (l_{3} + l_{4}) * \cos\theta_{3}) \\
\end{pmatrix}
=
\begin{pmatrix}
l_{2} + (l_{3} + l_{4}) * \cos\theta_{3} & (l_{3} + l_{4}) * \sin\theta_{3} \\
(l_{3} * l_{4}) * \sin\theta_{3} & l_{2} + (l_{3} + l_{4}) * \cos\theta_{3} \\
\end{pmatrix}
\begin{pmatrix}
\sin\theta_{2} \\
\cos\theta_{2} \\
\end{pmatrix} \\
\begin{pmatrix}
\sin\theta_{2} \\
\cos\theta_{2} \\
\end{pmatrix} について解くと \\
\begin{pmatrix}
\sin\theta_{2} \\
\cos\theta_{2} \\
\end{pmatrix}
=
\begin{pmatrix}
l_{2} + (l_{3} + l_{4}) * \cos\theta_{3} & (l_{3} + l_{4}) * \sin\theta_{3} \\
(l_{3} * l_{4}) * \sin\theta_{3} & l_{2} + (l_{3} + l_{4}) * \cos\theta_{3} \\
\end{pmatrix} ^{-1}
\begin{pmatrix}
+-\sqrt({}^{w}p_{5, x}^{2} + {}^{w}p_{5, y}^{2}) \\
{}^{w}p_{5, z} - l_{1} \\
\end{pmatrix} \\
\theta_{2} = atan2(\sin\theta_{2}, \cos\theta_{2}) \\
\begin{pmatrix}
l_{2} + (l_{3} + l_{4}) * \cos\theta_{3} & (l_{3} + l_{4}) * \sin\theta_{3} \\
(l_{3} * l_{4}) * \sin\theta_{3} & l_{2} + (l_{3} + l_{4}) * \cos\theta_{3} \\
\end{pmatrix} ^{-1} の解が存在しないときは特異点となる．\\
}
上式より，$\theta_{2}$を算出することが可能となる．
theta4 ~ theta6 の算出
手先姿勢と関節1, 2, 3の角度を計算したため，関節4, 5, 6の角度を計算する．
関節3から見た手先姿勢を算出
関節1~3までの角度は計算できたため，関節3から見た手先姿勢を計算する．
関節3から見た手先姿勢を使用して，$\theta_{4}$，$\theta_{5}$，$\theta_{6}$を算出していく．
\displaylines{
{}^{w}R_{e} = {}^{w}R_{3} * {}^{3}R_{e} \\
{}^{3}R_{e} = {}^{w}R^{-1}_{3} * {}^{w}R_{e} \\
{}^{w}R^{-1}_{3}は正規直交行列のため，{}^{w}R^{-1}_{3} = {}^{w}R^{T}_{3}となる \\
{}^{3}R_{e} = {}^{w}R^{T}_{3} * {}^{w}R_{e} \\
}
関節3から見た手先姿勢は，上式のようになる．
${}^{w}R^T_{3}$は関節1, 2, 3の角度を先程算出したため，計算することができる．
$ {}^{w}R_{e} $は順運動学より手先の姿勢は計算することができる．
次に，${}^{3}R_{e}$(Z-Y-Zオイラー角)を算出する．式は下記の通りとなる．
($\theta_{4}$は$z$軸に回転，$\theta_{5}$は$y$軸に回転，$\theta_{6}$は$z$軸に回転，手先は無回転とする．)
\displaylines{
{}^{3}R_{e} = {}^{3}R_{4} * {}^{4}R_{5} * {}^{5}R_{6} * {}^{6}R_{e} \\
{}^{3}R_{4}
=
\begin{pmatrix}
\cos\theta_{4} & -\sin\theta_{4} & 0 \\
\sin\theta_{4} & \cos\theta_{4} & 0 \\
0 & 0 & 1 \\
\end{pmatrix} ,
{}^{4}R_{5}
=
\begin{pmatrix}
\cos\theta_{5} & 0 & \sin\theta_{5} \\
0 & 1 & 0 \\
-\sin\theta_{5} & 0 & \cos\theta_{5} \\
\end{pmatrix} \\
{}^{5}R_{6}
=
\begin{pmatrix}
\cos\theta_{6} & -\sin\theta_{6} & 0 \\
\sin\theta_{6} & \cos\theta_{6} & 0 \\
0 & 0 & 1 \\
\end{pmatrix} ,
{}^{6}R_{e}
=
\begin{pmatrix}
1 & 0 & 0 \\
0 & 1 & 0 \\
0 & 0 & 1 \\
\end{pmatrix} \\
{}^{3}R_{e}
=
\begin{pmatrix}
-\sin\theta_{4} * \sin\theta_{6} + \cos\theta_{4} * \cos\theta_{5} * \cos\theta_{6}
& -\sin\theta_{4} * \cos\theta_{6} - \cos\theta_{4} * \cos\theta_{5} * \sin\theta_{6}
& \cos\theta_{4} * \sin\theta_{5} \\
\cos\theta_{4} * \sin\theta_{6} + \sin\theta_{4} * \cos\theta_{5} * \cos\theta_{6}
& \cos\theta_{4} * \cos\theta_{6} - \sin\theta_{4} * \cos\theta_{5} * \sin\theta_{6}
& \sin\theta_{4} * \sin\theta_{5} \\
-\sin\theta_{5} * \cos\theta_{6}
& \sin\theta_{5} * \sin\theta_{6}
& \cos\theta_{5} \\
\end{pmatrix} ... ③ \\
}
次に，${}^{w}R^T_{3} * {}^{w}R_{e}$を下記のようにおく．
\displaylines{
{}^{w}R^{T}_{3} * {}^{w}R_{e}
=
\begin{pmatrix}
r11 & r12 & r13 \\
r21 & r22 & r23 \\
r31 & r32 & r33 \\
\end{pmatrix} ... ④ \\
}
③と④を比較することで，$\theta_{4}$，$\theta_{5}$，$\theta_{6}$を算出する．
theta5 の算出
③と④の$r31$，$r32$，$r33$を比較して，$\theta_{5}$を算出する．
\displaylines{
r31 = -\sin\theta_{5} * \cos\theta_{6} \\
r32 = \sin\theta_{5} * \sin\theta_{6} \\
r31^{2} + r32^{2} = \sin^{2}\theta_{5} * \cos^{2}\theta_{6} + \sin^{2}\theta_{5} * \sin^{2}\theta_{6} = \sin^{2}\theta_{5} * (\cos^{2}\theta_{6} + \sin^{2}\theta_{6}) = \sin^{2}\theta_{5} \\
\sin\theta_{5} = +-\sqrt(r31^{2} + r32^{2}) \\
r33 = \cos\theta_{5} \\
\cos\theta_{5} = r33 \\
\theta_{5} = atan2(\sin\theta_{5}, \cos\theta_{5}) で算出できるため，\\
\theta_{5} = atan2(\sin\theta_{5}, \cos\theta_{5}) = atan2(+-\sqrt(r31^{2} + r32^{2}), r33) \\
}
$\theta_{5}$が$0$，$\pi$の時に，ジンバルロック(特異点)となる．
そのため，$\theta_{5}$が$0$，$\pi$で場合分けする必要がある．
theta5が0またはpiの時のtheta4とtheta6の計算
$\theta_{5}$が$0$，$\pi$の時の$\theta_{4}$と$\theta_{6}$の計算を説明する．
はじめに，$\theta_{5}$が$0$の時，③と④の$r11$と$r21$を使用して，下式のように計算できる．
\displaylines{
r11 = -\sin\theta_{4} * \sin\theta_{6} + \cos\theta_{4} * \cos\theta_{5} * \cos\theta_{6} であり，\theta_{5} = 0より，\cos\theta_{5} = 1より \\
r11 = -\sin\theta_{4} * \sin\theta_{6} + \cos\theta_{4} * \cos\theta_{6} = \cos(\theta_{4} + \theta_{6}) \\
r21 = \cos\theta_{4} * \sin\theta_{6} + \sin\theta_{4} * \cos\theta_{5} * \cos\theta_{6} であり，\theta_{5} = 0より，\cos\theta_{5} = 1より \\
r21 = \cos\theta_{4} * \sin\theta_{6} + \sin\theta_{4} * \cos\theta_{6} = \sin(\theta_{4} + \theta_{6}) \\
\theta_{4} + \theta_{6} = atan2(\sin(\theta_{4} + \theta_{6}), \cos(\theta_{4} + \theta_{6})) = atan2(r21, r11) \\
\theta_{4} = 0.0 とおくと \\
\theta_{6} = atan2(r21, r11) \\
}
次に，$\theta_{5}$が$\pi$の時，③と④の$r11$と$r21$を使用して，下式のように計算できる．
\displaylines{
r11 = -\sin\theta_{4} * \sin\theta_{6} + \cos\theta_{4} * \cos\theta_{5} * \cos\theta_{6} であり，\theta_{5} = \piより，\cos\theta_{5} = -1より \\
r11 = -\sin\theta_{4} * \sin\theta_{6} - \cos\theta_{4} * \cos\theta_{6} = -\cos(\theta_{4} - \theta_{6}) \\
r21 = \cos\theta_{4} * \sin\theta_{6} + \sin\theta_{4} * \cos\theta_{5} * \cos\theta_{6} であり，\theta_{5} = \piより，\cos\theta_{5} = -1より \\
r21 = \cos\theta_{4} * \sin\theta_{6} - \sin\theta_{4} * \cos\theta_{6} = -\sin(\theta_{4} - \theta_{6}) \\
\theta_{4} - \theta_{6} = atan2(\sin(\theta_{4} - \theta_{6}), \cos(\theta_{4} - \theta_{6})) = atan2(-r21, -r11) \\
\theta_{4} = 0.0 とおくと \\
\theta_{6} = -atan2(-r21, -r11) \\
}
theta5が0とpi以外の時のtheta4とtheta6の計算
$\theta_{5}$が$0$と$\pi$以外の時の$\theta_{4}$と$\theta_{6}$の計算を説明する．
はじめに，$\theta_{4}$を算出する．
③と④の$r13$と$r23$を使用して，下式のように計算できる．
\displaylines{
r13 = \cos\theta_{4} * \sin\theta_{5} \\
\cos\theta_{4} = r13 / \sin\theta_{5} \\
r23 = \sin\theta_{4} * \sin\theta_{5} \\
\sin\theta_{4} = r23 / \sin\theta_{5} \\
\theta_{4} = atan2(\sin\theta_{4}, \cos\theta_{4}) より算出できるため， \\
\theta_{4} = atan2(\sin\theta_{4}, \cos\theta_{4}) = atan2(r23 / \sin\theta_{5}, r13 / \sin\theta_{5}) = atan2(r23, r13) \\
}
次に，$\theta_{6}$を算出する．
③と④の$r31$と$r32$を使用して，下式のように計算できる．
\displaylines{
r31 = -\sin\theta_{5} * \cos\theta_{6} \\
\cos\theta_{6} = -r31 / \sin\theta_{5} \\
r32 = \sin\theta_{5} * \sin\theta_{6} \\
\sin\theta_{6} = r32 / \sin\theta_{5} \\
\theta_{6} = atan2(\sin\theta_{6}, \cos\theta_{6}) より算出できるため， \\
\theta_{6} = atan2(\sin\theta_{6}, \cos\theta_{6}) = atan2(r32 / \sin\theta_{5}, -r31 / \sin\theta_{5}) = atan2(r32, -r31) \\
}
上記より，$\theta_{4}$，$\theta_{5}$，$\theta_{6}$を算出できた．
逆運動学のソースコード
定数を定義するファイル (constant.py)
定数を定義するファイルを下記に記載する．
constant.py
# 複数ファイルで使用する定数の定義
# 次元数を定義
DIMENTION_NONE = -1 # 未定義
DIMENTION_2D = 2 # 2次元
DIMENTION_3D = 3 # 3次元
DIMENTION_6D = 6 # 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
回転行列を定義するファイル (rotation.py)
回転行列を定義するファイルを下記に記載する．
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, y_plus=True):
"""
回転行列からZ(theta1)-Y(theta2)-Z(theta3)オイラー角を取得
パラメータ
rotation(numpy.ndarray): 回転行列
y_plus(bool): Y(theta2)を+にするか
戻り値
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の範囲のため，制限しておく
y_cos = r33
if y_plus:
y_sin = np.sqrt(r31 ** 2 + r32 ** 2)
else:
y_sin = -np.sqrt(r31 ** 2 + r32 ** 2)
# r33 = np.clip(r33, self._ROT_MIN_VALUE, self._ROT_MAX_VALUE)
# arccos()には2通りの解が存在する cos(theta) = cos(-theta) だから
# theta2 = np.arccos(r33)
# if not y_plus:
# # 角度を負にする
# theta2 *= -1
theta2 = np.arctan2(y_sin, y_cos)
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 / y_sin, -r31 / y_sin)
# r23 = s1 * s2よりs1 = r23 / s2
# r13 = c1 * s2よりc1 = r13 / s2
# よって，theta1 = np.atan2(r23, r13)
theta1 = np.arctan2(r23 / y_sin, r13 / y_sin)
# Z(theta1)-Y(theta2)-Z(theta3)の順番に保存
euler = np.array([theta1, theta2, theta3])
return euler
ロボットアームを定義するファイル (robot.py)
ロボットアームを定義するファイルを下記に記載する．
robot.py
# ロボットアームの運動学を記載
# ライブラリの読み込み
import numpy as np
# サードパーティーの読み込み
import fcl
# 自作モジュールの読み込み
from constant import * # 定数
from rotation import MyRotation # 回転行列
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プロパティのゲッター
protected
_calc_homogeneou_matrix(): 同時変換行列の計算
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_NONE # 手先位置の次元数
_DIMENTION_THETA = DIMENTION_NONE # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_NONE # リンク数
_DIMENTION_AXIS = DIMENTION_NONE # 回転軸数
_INITIAL_THETA = 0.0 # 初期回転角度 [rad]
_HOMOGENEOU_MAT_ELEMENT = 4 # 同時変換行列の次元数
_ROTATION_MAT_ELEMENT = 3 # 回転行列の次元数
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 = MyRotation()
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.")
def _calc_homogeneou_matrix(self, thetas):
"""
同時変換行列の計算
パラメータ
thetas(numpy.ndarray): 関節角度 [rad]
戻り値
homogeneou_matix(numpy.ndarray): 全リンクの同時変換行列
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
homogeneou_matix = np.zeros((self._DIMENTION_LINK, self._HOMOGENEOU_MAT_ELEMENT, self._HOMOGENEOU_MAT_ELEMENT))
# 1リンク前の同時変換行列
prev_homogeneou_matrix = np.eye(self._HOMOGENEOU_MAT_ELEMENT)
for i in range(self._DIMENTION_THETA):
# 4行4列の要素を1に更新
homogeneou_matix[i, -1, -1] = 1
# 回転行列の計算
rotation_matrix = self._rot.rot(thetas[i], self._axiss[i])
# リンク間の相対位置を取得
relative_position = self._relative_positions[i].reshape(1, -1)
# 同時変換行列に回転行列を保存
homogeneou_matix[i, :self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT] = rotation_matrix
# 同時変換行列に相対位置を保存
homogeneou_matix[i, :self._ROTATION_MAT_ELEMENT, self._ROTATION_MAT_ELEMENT] = relative_position
# 1リンク前の同時変換行列と組み合わせる
homogeneou_matix[i] = np.dot(prev_homogeneou_matrix, homogeneou_matix[i])
# 1リンク前の同時変換行列の更新
prev_homogeneou_matrix = homogeneou_matix[i]
return homogeneou_matix
class Robot6DoF(Robot):
"""
6軸ロボットクラス
プロパティ
_links(numpy.ndarray): ロボットのリンク長
_rot(Rotation): 回転行列クラス
_axiss(list): 関節の回転軸
メソッド
public
forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
update(): 角度を与えて，各リンクの直方体を更新する
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_6D # 手先位置・姿勢の次元数
_DIMENTION_THETA = DIMENTION_6D # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_6D # リンク数
_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)
# ロボットの各リンクを直方体として定義する
self._objects = []
# (全リンクの角度を0とした時の) 各リンク間の相対位置
self._relative_positions = np.zeros((self._DIMENTION_LINK + 1, 3))
self._relative_positions[0] = np.array([0, 0, 0])
self._relative_positions[1] = np.array([0, 0, self._links[0]])
self._relative_positions[2] = np.array([0, 0, self._links[1]])
self._relative_positions[3] = np.array([0, 0, self._links[2]])
self._relative_positions[4] = np.array([0, 0, self._links[3]])
self._relative_positions[5] = np.array([0, 0, self._links[4]])
self._relative_positions[6] = np.array([0, 0, self._links[5]])
# リンク1，リンク4，リンク6は回転軸がz軸，リンク2，リンク3，リンク5は回転軸がy軸である
self._axiss = [ROTATION_Z_AXIS, ROTATION_Y_AXIS, ROTATION_Y_AXIS, ROTATION_Z_AXIS, ROTATION_Y_AXIS, ROTATION_Z_AXIS]
def forward_kinematics(self, thetas):
"""
順運動学 (ロボットの関節角度からロボットの手先位置を算出)
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
pose(numpy.ndarray): ロボットの手先位置・姿勢(Z-Y-Xオイラー角) [m] / [rad]
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
# 同時変換行列(6 * 4 * 4)を計算する
homogeneou_matrix = self._calc_homogeneou_matrix(thetas)
# 最終リンクの同時変換行列(最終リンク座標の位置・姿勢)より，手先位置を計算する
final_link_matrix = homogeneou_matrix[-1]
# 最終リンクから手先位置までの相対位置(4ベクトル)を定義
relative_pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
relative_pos[:self._HOMOGENEOU_MAT_ELEMENT - 1] = self._relative_positions[-1]
# 手先位置(x, y, z)・姿勢(theta_z, theta_y, theta_x)を保存する
pose = np.zeros(self._DIMENTION_POSE)
# 手先位置(x, y, z)を保存
pose[:DIMENTION_3D] = np.dot(final_link_matrix, relative_pos)[:DIMENTION_3D]
# 最終リンクの回転行列からZ-Y-Xオイラー角を計算
zyx_euler = self._rot.rot_to_zyx_euler(final_link_matrix[:self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT])
# 手先位置姿勢を保存(手先姿勢は最終リンクと同じ姿勢と仮定する)
pose[DIMENTION_3D:] = zyx_euler
return pose
def inverse_kinematics(self, pose, right=True, front=True, above=False):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
pose(numpy.ndarray): ロボットの手先位置([m])・姿勢([rad])
right(bool): 腕が右向きかどうか
front(bool): 正面かどうか
above(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)}")
# 引数から位置・姿勢を取得
pos = pose[:DIMENTION_3D]
zyx_euler = pose[DIMENTION_3D:]
thetas = np.zeros(self._DIMENTION_THETA)
# 関節1, 2, 3の角度を計算
thetas[:DIMENTION_3D] = self._inverse_kinematics_123(pos, zyx_euler, right=right, front=front)
# 関節4, 5, 6の角度を計算
thetas[DIMENTION_3D:] = self._inverse_kinematics_456(zyx_euler, thetas[:DIMENTION_3D], above=above)
return thetas
def _inverse_kinematics_123(self, pos, zyx_euler, right=False, front=True):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出) で関節1, 2, 3を計算
パラメータ
pos(numpy.ndarray): ロボットの手先位置 [m]
zyx_euler(numpy.ndarray): ロボットの手先姿勢(Z-Y-Xオイラー角) [rad]
right(bool): 肘が右向きどうか
front(bool): 正面かどうか
戻り値
theta123(numpy.ndarray): ロボットの関節角度(関節1, 2, 3) [rad]
"""
# リンクをローカル変数に保存
l1 = self._links[0]
l2 = self._links[1]
l34 = self._links[2] + self._links[3]
l56 = self._links[4] + self._links[5]
# Z-Y-Xオイラー角から回転行列を計算
rotation = self._rot.rot_from_zyx_euler(zyx_euler)
# 回転行列のz方向を取得
rotation_z = rotation[:, 2]
# 関節5までの絶対位置を算出
pos5 = pos - rotation_z * l56
px = pos5[0]
py = pos5[1]
pz = pos5[2]
# theta1 ~ theta3までの関節角度を算出する
# はじめにtheta3を算出する
cos3 = ((px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l34 ** 2)) / (2 * l2 * l34)
# cos()は-1以上かつ1以下の範囲である
if abs(cos3) > 1:
# 異常
raise ValueError(f"cos3 is abnormal. cos3 is {cos3}")
# sinを算出して，theta3を計算する
if right:
# 右向き
sin3 = np.sqrt(1 - cos3 ** 2)
else:
# 左向き
sin3 = -np.sqrt(1 - cos3 ** 2)
theta3 = np.arctan2(sin3, cos3)
# 次にtheta1を求める
# theta1 = atan2(py, px)
if abs(px) <= self._THETA1_XY_THRESHOLD and abs(py) <= self._THETA1_XY_THRESHOLD:
# 近傍のため，theta1=0固定とする
theta1 = 0.0
else:
if front:
# 正面向き
theta1 = np.arctan2( py, px)
else:
# 背面向き
theta1 = np.arctan2(-py, -px)
# 最後にtheta2を算出する
# [s2, c2] = [[l2 + l34 * c3, l34 * s3], [-l34 * s3, l2 + l34 * c3]]^(-1) * [root(px ** 2 + py ** 2), pz - l1]
element1 = l2 + l34 * cos3
element2 = l34 * 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])
# [s2, c2]の計算
sin2_cos2 = np.dot(np.linalg.inv(matrix), position)
# theta2をatan2()より算出する
theta2 = np.arctan2(sin2_cos2[0], sin2_cos2[1])
# 関節1, 2, 3の順番に保存
theta123 = np.array([theta1, theta2, theta3])
return theta123
def _inverse_kinematics_456(self, zyx_euler, theta123, above=True):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出) で関節4, 5, 6を計算
パラメータ
zyx_euler(numpy.ndarray): ロボットの手先姿勢(Z-Y-Xオイラー角) [rad]
theta123(numpy.ndarray): 関節1, 2, 3の角度 [rad]
above(bool): 手首が上かどうか
戻り値
theta456(numpy.ndarray): ロボットの関節角度(関節4, 5, 6) [rad]
"""
thetas = np.zeros(self._DIMENTION_THETA)
thetas[:3] = theta123
# wR6 = wR3 * 3R6
# 3R6 = wR3.T * wR6
# ロボットの手先姿勢(Z-Y-Xオイラー角)から回転行列を算出
rot_we = self._rot.rot_from_zyx_euler(zyx_euler)
# 同時変換行列(6 * 4 * 4)を計算する (関節4, 5, 6は0で適当な値を入れる)
homogeneou_matrix = self._calc_homogeneou_matrix(thetas)
# ワールド座標系から見た関節3の回転行列を計算
rot_w3 = homogeneou_matrix[2, :3, :3]
# 関節3から見た関節6の回転行列を計算する
rot_36 = np.dot(rot_w3.T, rot_we)
# Z(関節4)-Y(関節5)-Z(関節6)オイラー角を算出する
theta456 = self._rot.rot_to_zyz_euler(rot_36, above)
return theta456
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)}")
# 同時変換行列(6 * 4 * 4)を計算する
homogeneou_matix = self._calc_homogeneou_matrix(thetas)
# 全リンクの座標系の原点を取得
all_link_pose = np.zeros((self._DIMENTION_LINK + 1, self._DIMENTION_POSE))
for i, matrix in enumerate(homogeneou_matix):
# 同時変換行列から位置を取得
all_link_pose[i, :DIMENTION_3D] = matrix[:self._ROTATION_MAT_ELEMENT, self._ROTATION_MAT_ELEMENT].reshape(1, -1)
# 同時変換行列から回転行列を取得して，Z-Y-Xオイラー角に変換する
zyx_euler = self._rot.rot_to_zyx_euler(matrix[:self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT])
all_link_pose[i, DIMENTION_3D:] = zyx_euler
# 最後のリンクの座標
# 最後のリンクの座標系の原点から，手先の位置を計算する
pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
pos[:DIMENTION_3D] = self._relative_positions[-1]
all_link_pose[-1, :DIMENTION_3D] = np.dot(homogeneou_matix[-1], pos)[:DIMENTION_3D].reshape(1, -1)
# 手先姿勢は最終リンクの姿勢と一致していると考え，最終リンクの回転行列をZ-Y-Xオイラー角に変換する
zyx_euler = self._rot.rot_to_zyx_euler(homogeneou_matix[-1, :self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT])
all_link_pose[-1, DIMENTION_3D:] = zyx_euler
return all_link_pose
メイン処理を実施するファイル (main.py)
メイン処理を実施するファイルを下記に記載する．
main.py
# メイン処理
# ライブラリの読み込み
import numpy as np # 数値計算
import matplotlib.pyplot as plt # 描画
# 自作モジュールの読み込み
from constant import * # 定数
from robot import Robot6DoF # ロボットクラス
from rotation import MyRotation
FORWARD_KINE = False # 順運動学の計算フラグ (True / False = 順運動学 / 逆運動学)
LINE_WIDTH = 3 # プロット時の線の太さ
GRAPH_FORWARD_FILE_NAME = "plot_6drob_forward_kinematics.png"
GRAPH_INVERSE_FILE_NAME = "plot_6drob_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, right=True, front=True, above=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)
label(bool): 描画時のラベルの有無
戻り値
thetas(list): 関節角度
"""
# 順運動学により，位置を算出
thetas = robot.inverse_kinematics(pose, right=right, front=front, above=above)
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, 0.1, 0.1, 0.1])
# 3軸ロボットのインスタンスを作成
robot = Robot6DoF(links)
# プロット有無
plot_flg = True
# ラベル有無
label = False
aboves = [True, False]
rights = [True, False]
# 順運動学で使用する関節角度 [deg] を定義
deg_thetas = [60, ]
candidate = np.deg2rad(deg_thetas)
all_thetas = np.array([[theta1, theta2, theta3, theta4, theta5, theta6] for theta1 in candidate for theta2 in candidate for theta3 in candidate for theta4 in candidate for theta5 in candidate for theta6 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, label=label)
else:
# 逆運動学
file_name = GRAPH_INVERSE_FILE_NAME
for thetas in all_thetas:
# 順運動学で位置を取得
for above in aboves:
for right in rights:
print(f"above = {above}")
print(f"right = {right}")
# print(f"thetas = {thetas}")
pose1 = forward(robot, thetas, axis, plot_flg=False)
# 取得した位置で逆運動学
# print(f"pose1 = {pose1}")
theta = inverse(robot, pose1, axis, plot_flg=plot_flg, right=right, above=above)
print(f"theta = {theta}")
# 逆運動学の結果が正しいかを順運動学で計算
pose2 = forward(robot, theta, axis, plot_flg=False)
# print(f"pose1 = {pose1}")
# print(f"pose2 = {pose2}")
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を実行した時の結果を下図に示す．
図を見る限りでは，逆運動学が上手く実装できていると考えられる．
おわりに
本記事では，下記内容を実装しました．
・6軸ロボットアームの逆運動学
次記事では，下記内容を実装していきます．
・6軸ロボットアームの経路生成 (RRT)
(https://qiita.com/haruhiro1020/items/e543294c6912e56e704f)