はじめに
私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
前記事では，干渉物が存在する環境下での6軸ロボットアームの逆運動学を用いた経路生成を説明した．
(https://qiita.com/haruhiro1020/items/e543294c6912e56e704f)
本記事では，干渉物が存在しない環境下での6軸ロボットアームの微分逆運動学を用いた軌道生成を説明する．
微分逆運動学(順運動学にて算出した手先位置を微分して，逆運動学を解く)については，後ほど説明する．
また，6軸ロボットアームの順運動学 (関節角度から手先位置を算出する) と逆運動学 (手先位置から関節角度を算出する) は以下で説明した．
(・https://qiita.com/haruhiro1020/items/a240f8c8e538e75473a3
・https://qiita.com/haruhiro1020/items/af1d404b518aa6e13468)
本記事では，ロボットアームの初期位置・姿勢と目標位置・姿勢は下図の通りとして，微分逆運動学によりロボットを動かす．
下図の青色のロボットアームが初期位置・姿勢を表し，赤色のロボットアームが目標位置・姿勢を表している．
本記事で実装すること
・6軸ロボットアームで，微分逆運動学を使用した軌道生成を実装
・特異点(逆運動学の解が一意に決まらない)対応 (レーベンバーグ・マルカート法)
本記事では実装できないこと (将来実装したい内容)
・6軸ロボットアームで，干渉物が存在する環境下での微分逆運動学と経路生成(RRT)
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)
6軸ロボットアームに関して
6軸ロボットアームに関して説明する．
今回は下図のような6軸ロボットアームを考える．
以前の記事(https://qiita.com/haruhiro1020/items/a240f8c8e538e75473a3) にて，順運動学(関節角度からロボットの手先位置を算出)による手先位置を計算した．
手先速度と関節速度の関係性を算出して，関節速度を計算して，ロボットアームを動かすのが本記事の目標となる．
微分逆運動学に関して
下記のように，手先位置を$P_{a}(x, y, z)$から$P_{b}(x+dx, y+dy, z+dz)$へ移動，手先姿勢を$\sum_{a}$から$\sum_{b}$へ回転させることを考えてみる．
はじめに，順運動学による手先位置と関節角度に関する関係性を示す．
6軸ロボットアームの順運動学に関しては，以下サイトにて説明しております．
(https://qiita.com/haruhiro1020/items/a240f8c8e538e75473a3)
\displaylines{
r = f(\theta) \\
r ... 手先位置 \\
f(\theta) ... 関節角度から手先位置に変換する関数 \\
\theta ... 関節角度 \\
}
次に手先位置を微分すると，
\displaylines{
\dot r = \dot f(\theta) * \dot\theta \\
\dot r = J(\theta) * \dot\theta \\
\dot r ... 手先速度 \\
J(\theta) ... ヤコビ行列 \\
\dot\theta ... 関節速度 \\
}
2軸ロボットアームと3軸ロボットアームでは順運動学が容易なため，解析的な微分は容易であるが，6軸ロボットアームでは順運動学が複雑なため，解析的な微分が困難となる．
そのため，基礎ヤコビ行列と呼ばれているヤコビ行列を使用する．
絶対位置(ワールド座標系から見たある点の位置)の微分
はじめに，関節$i$から見た関節$i+1$の位置(${}^{w}p_{i+1}$)をワールド座標系から見た関節$i+1$の位置${}^{w}p_{i+1}$への変換を考える．
\displaylines{
{}^{w}p_{i+1} = {}^{w}p_{i} + {}^{w}R_{i} * {}^{i}p_{i+1} ... ① \\
{}^{w}p_{i+1} ... ワールド座標系から見た関節i+1の位置 (x, y, z) \\
{}^{w}p_{i} ... ワールド座標系から見た関節iの位置 (x, y, z) \\
{}^{w}R_{i} ... ワールド座標系から見た関節iの回転行列 \\
{}^{i}p_{i+1} ... 関節iから見た関節i+1の位置 (x, y, z) \\
}
次に，手先速度と関節速度の関係性を知りたいため，式①を微分してみる．
\displaylines{
{}^{w}\dot p_{i+1} = {}^{w}\dot p_{i} + {}^{w}\dot R_{i} * {}^{i}p_{i+1} + {}^{w}R_{i} * {}^{i+1}\dot p_{i+2} ... ② \\
{}^{w}\dot p_{i+1} ... ワールド座標系から見た関節i+1の速度 \\
{}^{w}\dot p_{i} ... ワールド座標系から見た関節iの速度 \\
{}^{w}\dot R_{i} ... ワールド座標系から見た関節iの回転行列の微分 \\
{}^{i}\dot p_{i+1} ... 関節iから見た関節i+1の速度 \\
}
次に，${}^{w}\dot R_{i}$の回転行列の微分に関して考えてみる．
回転行列は3 * 3行列の正規直交
\displaylines{
回転行列は3 * 3行列の正規直交行列であるため， \\
{}^{w}R_{i} * {}^{w}R^{T}_{i} = I(3) \\
{}^{w}R^{T}_{i} ... {}^{w}R_{i}の転置行列(行と列のデータを入れ替える) \\
I(3) ... 単位行列 \\
両辺を微分すると， \\
{}^{w}\dot R_{i} * {}^{w}R^{T}_{i} + {}^{w}R_{i} * {}^{w}\dot R^{T}_{i} = 0 \\
となる．{}^{w}\dot R_{i} * {}^{w}R^{T}_{i}を角速度行列である{}^{w}\Omega_{i}とおくと， \\
{}^{w}\dot R_{i} * {}^{w}R^{T}_{i} + {}^{w}R_{i} * {}^{w}\dot R^{T}_{i} = {}^{w}\Omega_{i} + ({}^{w}\dot R_{i} * {}^{w}R^{T}_{i})^{T} = {}^{w}\Omega_{i} + {}^{w}\Omega^{T}_{i} = 0 ... ③ \\
({}^{w}\dot R_{i} * {}^{w}R^{T}_{i})^{T} = ({}^{w}R^{T}_{i})^{T} * ({}^{w}\dot R_{i})^{T} = {}^{w}R_{i} * {}^{w}\dot R^{T}_{i} \\
}
最終的には，式③となる．
ここで，${}^{w}\Omega_{i} + {}^{w}\Omega^T_{i} = 0$となる．
${}^{w}\Omega_{i}$を歪対称行列と呼ばれる．
歪対称行列は下記サイトを参考にしました．
(https://manabitimes.jp/math/1899)
歪対称行列である${}^{w}\Omega_{i}$の角速度の行列は下式の通りとなる．
\displaylines{
{}^{w}\Omega_{i}
=
\begin{pmatrix}
0 & -\omega_{z} & \omega_{y} \\
\omega_{z} & 0 & -\omega_{x} \\
-\omega_{y} & \omega_{x} & 0 \\
\end{pmatrix} \\
\omega_{x} ... 角速度ベクトルのx方向 \\
\omega_{y} ... 角速度ベクトルのy方向 \\
\omega_{z} ... 角速度ベクトルのz方向 \\
}
${}^{w}\Omega_{i}$を算出できたため，回転行列の微分である${}^{w}\dot R_{i}$の解を求めると
\displaylines{
{}^{w}\dot R_{i} = {}^{w}\Omega_{i} * {}^{w}R_{i} ... ④ \\
{}^{w}\Omega_{i}
=
\begin{pmatrix}
0 & -\omega_{z} & \omega_{y} \\
\omega_{z} & 0 & -\omega_{x} \\
-\omega_{y} & \omega_{x} & 0 \\
\end{pmatrix} \\
}
式④を使用して，式②に代入すると
\displaylines{
{}^{w}\dot p_{i+1} = {}^{w}\dot p_{i} + {}^{w}\Omega_{i} * {}^{w}R_{i} * {}^{i}p_{i+1} + {}^{w}R_{i} * {}^{i}\dot p_{i+1} ... ⑤ \\
}
次に，関節$i$から見た関節$i+1$の角速度(${}^{i}\omega_{i+1}$)をワールド座標系から見た関節$i+1$の角速度${}^{w}\omega_{i+1}$への変換を考える．
\displaylines{
{}^{w}\omega_{i+1} = {}^{w}\omega_{i} + {}^{w}R_{i} * {}^{i}\omega_{i+1} ... ⑥ \\
}
式⑤と⑥にて，ワールド座標系から見た関節$i+1$の速度・角速度を算出することが可能となった．
関節($i+1$)が回転関節か直動関節かによって，式⑤と⑥が変わる．
本記事では，回転関節しか取り扱わない．
関節が回転関節の場合
関節($i+1$)が回転関節における，式⑤と⑥について説明する．
回転関節の場合は，${}^{i}\dot p_{i+1} = 0$となる．なぜならば，関節$i$から見た関節$i+1$の相対位置は常に一定であるから．
関節と関節の間には剛体であるリンクが存在する．剛体であるからリンクに力を加えても，リンクは変形しないため，相対位置は常に同じとなる．
剛体に関する情報は下記サイトがわかりやすいです．
(https://www.try-it.jp/chapters-8001/sections-8127/lessons-8132/)
よって，式⑤は下式のように修正できる．
\displaylines{
{}^{w}\dot p_{i+1} = {}^{w}\dot p_{i} + {}^{w}\Omega_{i} * {}^{w}R_{i} * {}^{i}p_{i+1} \\
}
次に，回転関節であるため，角速度の${}^{i}\omega_{i+1}$も求めることができる．
\displaylines{
{}^{i}\omega_{i+1} = {}^{i}R_{i+1} * e_{i+1} * \dot\theta_{i+1} \\
{}^{i}\omega_{i+1} ... 関節iから見た関節i+1の角速度 \\
{}^{i}R_{i+1} ... 関節iから見た関節i+1の回転行列 \\
e_{i+1} ... 関節i+1の回転軸 (z軸回転であれば(x, y, z) = (0, 0, 1)ベクトルとなる) \\
\dot\theta_{i+1} ... 関節i+1の角度の微分 \\
}
よって，ワールド座標系から見た関節$i+1$の速度・角速度は下式の通りとなる．
\displaylines{
{}^{w}\omega_{i+1} = {}^{w}\omega_{i} + {}^{w}R_{i} * e_{i+1} * \dot\theta_{i+1} \\
{}^{w}\dot p_{i+1} = {}^{w}\dot p_{i} + {}^{w}\Omega_{i} * {}^{w}R_{i} * {}^{i}p_{i+1} \\
{}^{w}\Omega_{i}は歪対称行列より，以下のようになる． \\
{}^{w}\Omega_{i}
=
\begin{pmatrix}
0 & -\omega_{z} & \omega_{y} \\
\omega_{z} & 0 & -\omega_{x} \\
-\omega_{y} & \omega_{x} & 0 \\
\end{pmatrix} \\
速度，角速度を一般化すると \\
{}^{w}\omega_{i} = \sum_{k=1}^{i} {}^{w}R_{k} * e_{k} * \dot\theta_{k} ... ⑦ \\
{}^{w}\dot p_{i} = \sum_{k=1}^{i} {}^{w}\Omega_{k} * {}^{w}R_{k} * {}^{k}p_{k+1} ... ⑧ \\
ワールド座標系から見たエンドエフェクタの位置は，ワールド座標系から見た関節6の位置を使用すると以下の通りとなる． \\
{}^{w}p_{e} = {}^{w}p_{6} + {}^{w}R_{6} * {}^{6}p_{e} \\
ワールド座標系から見たエンドエフェクタの位置は，ワールド座標系から見た関節iの位置を使用すると以下の通りとなる． \\
{}^{w}p_{e} = {}^{w}p_{i} + \sum_{j=i+1}^{n}{}^{w}R_{j-1} * {}^{j-1}p_{j} \\
\sum_{j=i+1}^{n}{}^{w}R_{j-1} * {}^{j-1}p_{j} = {}^{w}p_{e} - {}^{w}p_{i} \\
ヤコビ行列は関節iの動きが，エンドエフェクタの動きへどう影響するかを表すため，式⑧にエンドエフェクタの位置を採用すると下式のようになる． \\
{}^{w}\dot p_{i} = \sum_{k=1}^{i} {}^{w}\Omega_{k} * \sum_{j=k+1}^{n}{}^{w}R_{j-1} * {}^{j-1}p_{j} = \sum_{k=1}^{i} {}^{w}\Omega_{k} * ({}^{w}p_{e} - {}^{w}p_{k}) ... ⑨ \\
⑦の角速度と⑨の速度を使用して，基礎ヤコビ行列を算出すると下式のようになる． \\
\begin{pmatrix}
{}^{w}\dot p_{e} \\
{}^{w}\omega_{e}
\end{pmatrix}
=
\begin{pmatrix}
{}^{w}\Omega_{1} * ({}^{w}p_{e} - {}^{w}p_{1}) & {}^{w}\Omega_{2} * ({}^{w}p_{e} - {}^{w}p_{2}) & \cdots & {}^{w}\Omega_{6} * ({}^{w}p_{e} - {}^{w}p_{6}) \\
{}^{w}\omega_{1} = {}^{w}R_{1} * e_{1} * \dot\theta_{1} & {}^{w}\omega_{2} = {}^{w}R_{2} * e_{2} * \dot\theta_{2} & \cdots & {}^{w}\omega_{6} = {}^{w}R_{6} * e_{6} * \dot\theta_{6} \\
\end{pmatrix} \\
\begin{pmatrix}
{}^{w}\dot p_{e} \\
{}^{w}\omega_{e}
\end{pmatrix}
=
\begin{pmatrix}
{}^{w}Z_{1} * ({}^{w}p_{e} - {}^{w}p_{1}) & {}^{w}Z_{2} * ({}^{w}p_{e} - {}^{w}p_{2}) & \cdots & {}^{w}Z_{6} * ({}^{w}p_{e} - {}^{w}p_{6}) \\
{}^{w}z_{1} = {}^{w}R_{1} * e_{1} & {}^{w}z_{2} = {}^{w}R_{2} * e_{2} & \cdots & {}^{w}z_{6} = {}^{w}R_{6} * e_{6} \\
\end{pmatrix}
\begin{pmatrix}
\dot\theta_{1} \\
\dot\theta_{2} \\
\vdots \\
\dot\theta_{6} \\
\end{pmatrix} \\
\vec{v_{e}} = J_{v}(\vec{\theta}) * \vec{\dot\theta} ... ⑩ \\
\vec{\dot\theta} = J_{v}(\vec{\theta})^{-1} * \vec{v_{e}} ... ⑪ \\
J_{v}(\vec{\theta})を基礎ヤコビ行列と呼ぶ． \\
{}^{w}\Omega_{i}，{}^{w}Z_{i}は歪対称行列より，下式のようになる． \\
{}^{w}\Omega_{i}
=
\begin{pmatrix}
0 & -\omega_{z} & \omega_{y} \\
\omega_{z} & 0 & -\omega_{x} \\
-\omega_{y} & \omega_{x} & 0 \\
\end{pmatrix} ，
{}^{w}Z_{i}
=
\begin{pmatrix}
0 & -z_{z} & z_{y} \\
z_{z} & 0 & -z_{x} \\
-z_{y} & z_{x} & 0 \\
\end{pmatrix} \\
}
式⑩，式⑪より，手先速度・加速度と関節速度との関係性を表すことができた．
式⑩で，ワールド座標系から見た手先角速度が出てきた．手先角速度の代わりに手先姿勢の差分を採用する．手先姿勢の差分を算出するための方法としては，1軸回転法を使用する．1軸回転法を次に説明する．
式⑪で，基礎ヤコビ行列の逆行列が出てきた．特異点では逆行列が計算できない．特異点近傍では逆行列が計算できても，過大な値となってしまう．
そのため，逆行列の安定性を担保する方法も後ほど説明する．
手先姿勢の差分 (1軸回転法)
手先姿勢の差分を算出する方法として，1軸回転法を採用する．
1軸回転法に関しては，以前に下記記事を作成しました．詳細な内容は下記記事を参照ください．
(https://qiita.com/haruhiro1020/items/9e90b2399ee4d50ffff4)
ここでは，簡単な説明をします．
数式としては，下記のようになります．
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は単位方向ベクトルである． \\
}
上式より，回転角度である$\theta$を算出することが可能となる．
次に，回転軸である$\lambda$の算出を実装する．
\displaylines{
初期姿勢と目標姿勢の差分は下記のようになる \\
{}^{w}R_{target} = {}^{w}R_{init} * {}^{init}R_{target} より， \\
{}^{init}R_{target} = {}^{w}R_{init}^{-1} * {}^{w}R_{target} \\
{}^{w}R_{target} ... 目標姿勢 \\
{}^{w}R_{init} ... 初期姿勢 \\
{}^{init}R_{target} ... 初期姿勢と目標姿勢の差分 \\
1軸回転法で使用する\lambdaと\thetaを使用して，回転行列に変換(参照の記事に詳細を記載)して，回転行列を下記のように置いて考えると， \\
R
=
\begin{pmatrix}
r11 & r12 & r13 \\
r21 & r22 & r23 \\
r31 & r32 & r33 \\
\end{pmatrix} \\
\cos\theta = (trace(R) - 1) / 2 \\
次に，回転行列の対角成分以外を使用して，回転軸を算出する． \\
u
=
\begin{pmatrix}
r32 - r23 \\
r13 - r31 \\
r21 - r12 \\
\end{pmatrix} \\
=
2 * \sin\theta
*
\begin{pmatrix}
\lambda_{x} \\
\lambda_{y} \\
\lambda_{z} \\
\end{pmatrix} \\
両辺の大きさを取り，\sin\thetaについて解くと， \\
\sin\theta = +-|u| / 2 \\
\sin\thetaと\cos\thetaより，\thetaについて解くと， \\
\theta = atan2(\sin\theta, \cos\theta) = atan2(+-|u| / 2, (trace(R) - 1) / 2) \\
uから，\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軸回転法を使用して，初期姿勢と目標姿勢の差分を算出する．
次は，ヤコビ行列の逆行列の特異点近傍対応としてのレーベンバーグ・マルカート法に関して説明する．
特異点近傍への対応 (レーベンバーグ・マルカート法)
レーベンバーグ・マルカート法に関しては，以前に下記記事を作成しました．詳細な内容は下記記事を参照ください．
(https://qiita.com/naturemon/items/ec1154a708841f27c4d0)
ここでは，簡単な説明をします．
数式としては，下記のようになります．
\displaylines{
(J(\theta)^{T} * J(\theta) + \lambda * I) * dq = J(\theta)^{T} * dr \\
J(\theta) ... ヤコビ行列 \\
J(\theta)^{T} ... ヤコビ行列の転置行列 (J(\theta)のi行，j列のデータをj列，i行に入れ換えた行列が転置行列) \\
\lambda ... 0より大きい値で，正則化パラメータ \\
I ... 単位行列であり，行数と列数は関節数と同じとなる． \\
dq ... 関節速度 \\
dr ... 手先速度・角速度 \\
}
$dq$に関して解くと下記のようになります．
\displaylines{
dq = (J(\theta)^{T} * J(\theta) + \lambda * I))^{-1} * dr \\
}
$\lambda$の値に応じて，処理が変わる．
$\lambda$が$0$近傍の場合は，通常の逆行列と同じになるため，特異点近傍では，解が不安定になるが，目標位置への収束が早い．
$\lambda$が大きい値の場合は，特異点近傍でも解が安定するが，目標位置への収束に時間がかかる．
特異点近傍への対策として，レーベンバーグ・マルカート法を採用する．
微分逆運動学のソースコード
本記事では，ロボットアームの初期位置・姿勢と目標位置・姿勢は下表の通りとして，微分逆運動学によりロボットを動かす．
| 項目 | 位置($x, y, z$) [m] | 姿勢($Z-Y-X$オイラー角) [rad] |
|---|---|---|
| 初期値 | $( 1.0, -1.0, 1.0)$ | $(0, 0, 0)$ |
| 目標値 | $(-1.0, 1.0, 2.0)$ | $(0, \pi/2, 0)$ |
下図の青色のロボットアームが初期位置・姿勢を表し，赤色のロボットアームが目標位置・姿勢を表している．
定数を定義するファイル (constant.py)
定数を定義するファイルを下記に記す．
constant.py
# 複数ファイルで使用する定数の定義
from enum import Enum
from enum import auto
# 次元数を定義
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軸周りに逆回転
# 干渉物の名称を定義
class INTERFERENCE(Enum):
"""
干渉物の名称を定義
"""
NONE = "" # 未定義
# 2次元の干渉物 ↓
CIRCLE = "circle" # 円形の干渉物
RECTANGLE = "rectangle" # 長方形の干渉物
LINE2D = "line2D" # 2次元の直線
# 2次元の干渉物 ↑
# 3次元の干渉物 ↓
BALL = "ball" # 球の干渉物
CUBOID = "cuboid" # 直方体の干渉物
LINE3D = "line3D" # 3次元の直線
# 3次元の干渉物 ↑
# 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_from_zyz_euler(self, euler):
"""
Z(theta1)-Y(theta2)-Z(theta3)オイラー角による回転行列の計算
パラメータ
euler(float): Z(theta1)-Y(theta2)-Z(theta3)オイラー角 [rad]
戻り値
rotation(numpy.ndarray): 回転行列
"""
# 引数を分解する
theta1 = euler[0]
theta2 = euler[1]
theta3 = euler[2]
# 回転行列を計算
rot_z1 = self._rot_z(theta1)
rot_y = self._rot_y(theta2)
rot_z2 = self._rot_z(theta3)
# Z-Y-X軸に順番に回転させた時の回転行列を計算
rotation = np.dot(rot_z1, rot_y)
rotation = np.dot(rotation, rot_z2)
return rotation
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
def difference_rot_matrix(self, rot1, rot2):
"""
回転行列(rot1から見たrot2)の差分を計算
パラメータ
rot1(numpy.ndarray): 回転行列1 (3 * 3行列)
rot2(numpy.ndarray): 回転行列2 (3 * 3行列)
戻り値
diff_rot(numpy.ndarray): 回転行列の差分
"""
# 引数の確認
if rot1.shape != (3, 3):
# 回転行列ではない
raise ValueError(f"rot1.shape is not (3, 3). rot1.shape is {rot1.shape}")
if rot2.shape != (3, 3):
# 回転行列ではない
raise ValueError(f"rot2.shape is not (3, 3). rot2.shape is {rot2.shape}")
# rot1から見た4ot2の回転行列を計算
diff_rot = np.dot(rot1.T, rot2)
return diff_rot
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 = np.linalg.norm(u) / 2
# sin()，cos()からthetaを算出
rot_angle = np.arctan2(sin_rot_angle, cos_rot_angle)
# rot_angle = np.arccos(cos_rot_angle)
# 回転が0 or pi付近では，何もしない
# print(f"rot_angle = {rot_angle}")
if np.isclose(rot_angle, 0.0, atol=1e-3) or np.isclose(abs(rot_angle), np.pi, atol=1e-3):
rot_angle = 0
rot_axis = np.array([1, 0, 0])
else:
# 回転軸を算出する
# 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
6軸ロボットアームを定義するファイル (robot.py)
6軸ロボットアームを定義するファイルを下記に記す．
Roboto6DoF()クラスのdifferential_inverse_kinematics()メソッドより，微分逆運動学による目標位置までの軌道を生成する．
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 # 回転軸数
_HOMOGENEOU_MAT_ELEMENT = 4 # 同時変換行列の次元数
_ROTATION_MAT_ELEMENT = 3 # 回転行列の次元数
# ヤコビ行列に関する定数
_JACOV_DELTA_TIME = 0.2 # ヤコビの1時刻
_JACOV_NEAR_POS = 5e-3 # 目標位置との近傍距離 [m]
_JACOV_NEAR_ORI = 5e-2 # 目標位置との近傍姿勢 [rad]
_JACOV_MAX_COUNT = 200 # ヤコビの最大回数
_JACOV_POS_ERROR_WEIGHT = 1.0 # 微分逆運動学での位置誤差への重み
_JACOV_ORI_ERROR_WEIGHT = 0.2 # 微分逆運動学での姿勢誤差への重み
_LEVENBERG_MARUQUARDT_LAMBDA = 0.5 # Levenberg-Marquardt法で使用するパラメータ
# その他定数
_INITIAL_THETA = 0.0 # 初期回転角度 [rad]
_DETERMINANT_THRESHOLD = 1e-4 # 行列式の閾値
_BOX_WIDTH = 1e-2 # 各リンクの幅を定義
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
self._jacov_thetas = []
self._lambda = self._LEVENBERG_MARUQUARDT_LAMBDA
def _reset_jacov_thetas(self):
"""
_jacov_thetasプロパティのリセット
"""
if len(self._jacov_thetas) != 0:
self._jacov_thetas.clear()
@property
def links(self):
"""
_linksプロパティのゲッター
"""
return self._links
@property
def manager(self):
"""
_managerプロパティのゲッター
"""
return self._manager
@property
def jacov_thetas(self):
"""
_jacov_thetasプロパティのゲッター
"""
return self._jacov_thetas
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 _get_base_jacovian(self, rot_axis, pos_robot, homogeneou_matrix):
"""
基礎ヤコビ行列の1列分を取得
パラメータ
rot_axis(str): 回転軸
pos_robot(numpy.ndarray): ロボットの手先位置
homogeneou_matrix(numpy.ndarray): 同時変換行列 (4 * 4行列)
戻り値
base_jacovian(numpy.ndarray): 基礎ヤコビ行列の1列分(6 * 1行列)
"""
if homogeneou_matrix.shape != (4, 4):
raise ValueError(f"homogeneou_matrix is abnormal. homogeneou_matrix.shape is {homogeneou_matrix.shape}")
base_jacovian = np.zeros((DIMENTION_6D, 1))
# 回転軸の定義
if rot_axis == ROTATION_X_AXIS:
# x軸周りに正回転
axis = np.array([1, 0, 0])
elif rot_axis == ROTATION_Y_AXIS:
# y軸周りに正回転
axis = np.array([0, 1, 0])
elif rot_axis == ROTATION_Z_AXIS:
# z軸周りに正回転
axis = np.array([0, 0, 1])
else:
# 異常
raise ValueError(f"rot_axis is abnormal. rot_axis is {rot_axis}")
# ワールド座標系から見た回転行列と回転軸より，ワールド座標系から見た回転軸の取得
word_axis = np.dot(homogeneou_matrix[:self._ROTATION_MAT_ELEMENT, :self._ROTATION_MAT_ELEMENT], axis)
# 同時変換行列(ワールド座標系から見た位置・姿勢)から，回転行列・位置を取得
position = homogeneou_matrix[:self._ROTATION_MAT_ELEMENT, self._ROTATION_MAT_ELEMENT]
# 位置の差分
difference = pos_robot - position.reshape(-1)
# 位置に関する情報を保存
cross_data = np.cross(word_axis, difference)
base_jacovian[:DIMENTION_3D] = cross_data.reshape(-1, 1)
# ワールド座標系から見た姿勢に関する情報を保存
base_jacovian[DIMENTION_3D:] = word_axis.reshape(-1, 1)
return base_jacovian
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]
# 初期角度
initial_thetas = np.zeros(self._DIMENTION_THETA)
# 順運動学により，全リンク(ベースリンク，リンク1，リンク2，リンク3，リンク4，リンク5，リンク6)の位置を計算
all_link_pose = self.forward_kinematics_all_link_pos(initial_thetas)
# 1つ前のリンクの回転行列を更新
prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)
# ロボットの各リンクを直方体として定義する
for i in range(self._DIMENTION_THETA):
# 各リンクの回転行列を定義
rotation = self._rot.rot(initial_thetas[i], self._axiss[i])
rotation = np.dot(prev_rotation, rotation)
# 各リンクの中心位置 (x, y, z) を定義
center = (all_link_pose[i + 1, :DIMENTION_3D] - all_link_pose[i, :DIMENTION_3D]) / 2 + all_link_pose[i, :DIMENTION_3D]
# 直方体の定義 (x, y, zの長さを保存)
x, y, z = [self._BOX_WIDTH * 2, self._BOX_WIDTH * 2, self._links[i]]
box = fcl.Box(x, y, z)
# 直方体の中心を定義 (位置・姿勢)
translation = fcl.Transform(rotation, center)
obj = fcl.CollisionObject(box, translation)
# モデルを追加
self._objects.append(obj)
# 1つ前のリンクの回転行列を更新
prev_rotation = rotation
# 直方体をAABBとして，定義
# DynamicAABBTreeCollisionManager に登録
self._manager = fcl.DynamicAABBTreeCollisionManager()
self._manager.registerObjects(self._objects)
self._manager.setup()
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 differential_inverse_kinematics(self, thetas, target_pos):
"""
2点間の微分逆運動学
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
target_pos(numpy.ndarray): 目標位置・姿勢 (Z-Y-Xオイラー角) ([m] / [rad])
戻り値
target_thetas(numpy.ndarray): 目標位置の関節角度 [rad]
"""
# 引数の確認
if np.size(thetas) != self._DIMENTION_THETA:
# 異常
raise ValueError(f"theta's size is abnormal. theta's size is {np.size(thetas)}")
# 現在の関節角度を保存 (異なるアドレスにデータを保存するためにnp.copy()を採用)
current_thetas = np.copy(thetas)
# 微分逆行列で取得したデータを初期化
self._reset_jacov_thetas()
# 計算した角度を保存する
self._jacov_thetas.append(current_thetas)
# 目標位置・姿勢を保存
target_position = target_pos[:DIMENTION_3D]
target_rotation = self._rot.rot_from_zyx_euler(target_pos[DIMENTION_3D:])
# 位置・姿勢誤差
error_pos_rot = np.zeros(DIMENTION_6D)
# 目標位置に近づくまでループ
for i in range(self._JACOV_MAX_COUNT):
# ヤコビ行列の取得
jacovian = self._jacovian(current_thetas)
# 現在の手先位置を計算
current_pos = self.forward_kinematics(current_thetas)
# 位置の差分を計算
dP = target_position - current_pos[:DIMENTION_3D]
# 現在の姿勢(回転行列)を計算
current_rotation = self._rot.rot_from_zyx_euler(current_pos[DIMENTION_3D:])
# 1軸回転法による，姿勢の差分を計算
single_axis = self._rot.get_single_axis(current_rotation, target_rotation, t=1)
dR = single_axis[1:] * single_axis[0]
# 位置・姿勢誤差の保存
# 位置の単位は[m]，姿勢の単位は[rad]であり，単位が異なるから，係数を掛け合わせる．
error_pos_rot[:DIMENTION_3D] = dP * self._JACOV_POS_ERROR_WEIGHT
error_pos_rot[DIMENTION_3D:] = dR * self._JACOV_ORI_ERROR_WEIGHT
# 逆行列の中身を作成
new_jacovian = np.dot(jacovian.T, jacovian) + self._lambda * np.eye(jacovian.shape[0])
dTheta = np.dot(np.dot(np.linalg.inv(new_jacovian), jacovian.T), error_pos_rot)
# +=とするとcurrent_thetasが常に同じアドレスになるため，current_thetasを異なるアドレスとする (self._jacov_thetasに保存するときに不具合になるから)
current_thetas = current_thetas + dTheta * self._JACOV_DELTA_TIME
# 計算した角度を保存する
self._jacov_thetas.append(current_thetas)
# 更新後の手先位置が目標位置の近傍であれば，処理終了とする
current_pos = self.forward_kinematics(current_thetas)
# 位置誤差
pos_dist = np.linalg.norm(target_pos[:DIMENTION_3D] - current_pos[:DIMENTION_3D])
# 姿勢誤差
current_rotation = self._rot.rot_from_zyx_euler(current_pos[DIMENTION_3D:])
# current_rotation, target_rotation は 3x3 の回転行列
single_axis = self._rot.get_single_axis(current_rotation, target_rotation, t=1)
# print(f"pos_dist = {pos_dist}")
# print(f"single_axis[0] = {single_axis[0]}")
if pos_dist <= self._JACOV_NEAR_POS and single_axis[0] <= self._JACOV_NEAR_ORI:
# 位置の近傍かつ回転角度の近傍
break
target_thetas = current_thetas
return target_thetas
def _jacovian(self, thetas):
"""
ヤコビ行列の計算
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
jacovian(numpy.ndarray): ヤコビ行列
"""
# 引数の確認
if np.size(thetas) != self._DIMENTION_THETA:
# 異常
raise ValueError(f"theta's size is abnormal. theta's size is {np.size(thetas)}")
# 6(位置の3次元 + 姿勢の3次元) * ヤコビ行列は関節数
jacovian = np.zeros((DIMENTION_6D, self._DIMENTION_THETA))
# 同時変換行列 (関節数 * 4 * 4) を計算
homogeneou_matrix = self._calc_homogeneou_matrix(thetas)
# ロボットの手先位置を計算 (位置(3要素)，姿勢(3要素))
robot_pos = self.forward_kinematics(thetas)[:DIMENTION_3D]
for idx in range(self._DIMENTION_THETA):
# 各関節の基礎ヤコビ行列を計算
jacovian[:, idx] = self._get_base_jacovian(self._axiss[idx], robot_pos, homogeneou_matrix[idx]).reshape(-1)
return jacovian
def inverse_kinematics(self, pose, right=False, front=True, above=True):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
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)
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
def update(self, thetas):
"""
角度を与えて，各リンクの直方体を更新する
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
# 順運動学により，全リンク(ベースリンク，リンク1，リンク2，リンク3)の位置を計算
all_link_pose = self.forward_kinematics_all_link_pos(thetas)
# ロボットの各リンクを直方体として定義する
for i in range(self._DIMENTION_THETA):
# 各リンクの回転行列を定義
rotation = self._rot.rot_from_zyx_euler(all_link_pose[i, DIMENTION_3D:])
# 各リンクの中心位置 (x, y, z) を定義
center = (all_link_pose[i + 1, :DIMENTION_3D] - all_link_pose[i, :DIMENTION_3D]) / 2 + all_link_pose[i, :DIMENTION_3D]
# 直方体の中心を定義 (位置・姿勢)
translation = fcl.Transform(rotation, center)
# モデルの位置を更新
self._objects[i].setTransform(translation)
# AABBを更新
self._manager.update()
ロボットのアニメーション (animation.py)
ロボットのアニメーションに関する内容を下記に記す．
animation.py
# ロボットのアニメーションを実施
# ライブラリの読み込み
import numpy as np # 数値計算
import matplotlib.pyplot as plt # 描画用
import matplotlib.animation as ani # アニメーション用
import matplotlib.patches as patches # 2次元形状の描画
from mpl_toolkits.mplot3d.art3d import Poly3DCollection, Line3DCollection # 3次元形状の描画
# 自作モジュールの読み込み
from constant import * # 定数
from rotation import MyRotation # 回転行列
class RRTRobotAnimation:
"""
RRTによるロボットの経路生成アニメーション
プロパティ
_figure: 描画枠
_axis: 描画内容
publicメソッド (全てのクラスから参照可能)
plot_Animation(): アニメーション作成
protectedメソッド (自クラスまたは子クラスが参照可能)
_reset2D(): 2次元データのリセット
"""
# 定数の定義
_ANIMATION_NAME = "robot_animation.gif"
_PLOT_NAME = "robot_plot.gif"
_STRICT_POS = 0.2
def __init__(self):
"""
コンストラクタ
"""
self._rot = MyRotation()
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_circle(self, x, y, radius):
"""
円の描画
パラメータ
x(float): 中心点 (x)
y(float): 中心点 (y)
radius(float): 半径
"""
circle = patches.Circle((x, y), radius, color="gray", alpha=0.5)
self._axis.add_patch(circle)
def _plot_rectangle(self, center, width, height, angle):
"""
長方形の描画
パラメータ
center(numpy.ndarray): 中心の座標 (x, y)
width(float): 幅
height(float): 高さ
angle(float): 角度 [deg]
"""
# 左下隅の座標
xy = np.array([center[0] - width / 2, center[1] - height / 2])
rect = patches.Rectangle(xy, width, height, angle=angle, color="gray", alpha=0.5)
# 長方形を軸に追加
self._axis.add_patch(rect)
def _plot_ball(self, center, radius):
"""
円の描画
パラメータ
center(np.ndarray): 中心位置 (x, y, z)
radius(float): 半径
"""
self._axis.plot_wireframe(self._x * radius + center[0], self._y * radius + center[1], self._z * radius + center[2], color="gray", alpha=0.5)
def _plot_cuboid(self, center, x, y, z, rotation):
"""
直方体の描画
パラメータ
center(numpy.ndarray): 中心位置 (x, y, z)
x(float): 直方体のx軸の長さ
y(float): 直方体のy軸の長さ
z(float): 直方体のz軸の長さ
rotation(numpy.ndarray): 回転行列
"""
# 頂点を算出する
points = np.array([[center[0] - x / 2, center[1] - y / 2, center[2] - z / 2],
[center[0] + x / 2, center[1] - y / 2, center[2] - z / 2],
[center[0] + x / 2, center[1] + y / 2, center[2] - z / 2],
[center[0] - x / 2, center[1] + y / 2, center[2] - z / 2],
[center[0] - x / 2, center[1] - y / 2, center[2] + z / 2],
[center[0] + x / 2, center[1] - y / 2, center[2] + z / 2],
[center[0] + x / 2, center[1] + y / 2, center[2] + z / 2],
[center[0] - x / 2, center[1] + y / 2, center[2] + z / 2]])
# 頂点4点から面を算出する
verts = [[points[0], points[1], points[2], points[3]],
[points[4], points[5], points[6], points[7]],
[points[0], points[1], points[5], points[4]],
[points[2], points[3], points[7], points[6]],
[points[1], points[2], points[6], points[5]],
[points[4], points[7], points[3], points[0]]]
self._axis.add_collection3d(Poly3DCollection(verts))
def _plot_environment(self, environment):
"""
アニメーション作成
パラメータ
environment(Robot2DEnv): 経路生成時の環境
"""
if environment is None:
# 環境なし
return
for name, datas in environment.interferences.items():
# 干渉物の名称
if name == INTERFERENCE.CIRCLE:
# 円
for x, y, radius in datas:
self._plot_circle(x, y, radius)
elif name == INTERFERENCE.RECTANGLE:
# 長方形
for x, y, center_x, center_y, angle in datas:
# 中心点から左隅の点に移動させるために，-x/2，-y/2を実施
self._plot_rectangle(np.array([center_x, center_y]), x, y, angle)
elif name == INTERFERENCE.BALL:
# 球
for x, y, z, radius in datas:
self._plot_ball(np.array([x, y, z]), radius)
elif name == INTERFERENCE.CUBOID:
# 直方体
for x, y, z, center_x, center_y, center_z, angle_x, angle_y, angle_z in datas:
# 中心点から左隅の点に移動させるために，-x/2，-y/2, -z/2を実施
# 回転行列の作成
rotation_x = self._rot.rot(np.deg2rad(angle_x), ROTATION_X_AXIS)
rotation_y = self._rot.rot(np.deg2rad(angle_y), ROTATION_Y_AXIS)
rotation_z = self._rot.rot(np.deg2rad(angle_z), ROTATION_Z_AXIS)
rotation = np.dot(rotation_z, np.dot(rotation_y, rotation_x))
self._plot_cuboid(np.array([center_x, center_y, center_z]), x, y, z, rotation)
else:
# 何もしない
pass
def _plot_2DAnimation(self, robot, all_link_thetas, environment, anime_file_name=""):
"""
2次元アニメーションの作成
パラメータ
dimention(int): 次元数
robot(Robot2DoF): ロボットクラス
all_link_thetas(numpy.ndarray): 全リンクの回転角度
environment(Robot2DEnv): 経路生成時の環境
anime_file_name(str): アニメーションのファイル名
"""
# データをリセットする
self._reset2D()
# 全画像を保存する
imgs = []
# 環境の描画
self._plot_environment(environment)
# 手先位置の軌跡を保存
position_trajectory = np.zeros((all_link_thetas.shape[0], DIMENTION_2D))
# 始点と終点をプロット
# 始点位置を取得
start_pos = robot.forward_kinematics(all_link_thetas[0])
start_image = self._axis.scatter(start_pos[0], start_pos[1], color="cyan")
end_pos = robot.forward_kinematics(all_link_thetas[-1])
end_image = self._axis.scatter(end_pos[0], end_pos[1], color="red")
# 軌道生成
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)
# 始点と終点の画像を保存
path_images.extend([start_image])
path_images.extend([end_image])
# 画像を1枚にまとめて保存
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()
def _reset3D(self):
"""
3次元データのリセット
"""
self._figure = plt.figure()
self._axis = self._figure.add_subplot(111, projection="3d")
# 0 ~ 2piまでの範囲とする
theta_1_0 = np.linspace(0, np.pi * 2, 10)
theta_2_0 = np.linspace(0, np.pi * 2, 10)
theta_1, theta_2 = np.meshgrid(theta_1_0, theta_2_0)
# x, y, zの曲座標表示 (中心点が原点である半径1の球)
self._x = np.cos(theta_2) * np.sin(theta_1)
self._y = np.sin(theta_2) * np.sin(theta_1)
self._z = np.cos(theta_1)
def _set_3DAxis(self, robot):
"""
3次元データのラベルや範囲を設定
"""
# X/Y/Z軸に文字を記載
self._axis.set_xlabel("X")
self._axis.set_ylabel("Y")
self._axis.set_zlabel("Z")
self._axis.grid()
self._axis.set_aspect("equal")
def _update_3Ddata(self, i, robot, all_link_thetas, all_link_poses, environment):
"""
3D(3次元)各データの更新
パラメータ
i(int): フレーム番号
robot(Robot3DoF): ロボットクラス
all_link_thetas(numpy.ndarray): 始点から終点までの全角度
all_link_poses(numpy.ndarray): 始点から終点までの全位置
environment(Robot3DEnv): 経路生成時の環境
"""
# 以前のプロットをクリアする
self._axis.clear()
self._set_3DAxis(robot)
# 環境をプロット
self._plot_environment(environment)
# 始点と終点をプロット
# 始点位置を取得
start_pos = robot.forward_kinematics(all_link_thetas[0])
self._axis.scatter(start_pos[0], start_pos[1], start_pos[2], color="cyan")
end_pos = robot.forward_kinematics(all_link_thetas[-1])
self._axis.scatter(end_pos[0], end_pos[1], end_pos[2], color="red")
# 順運動学により，全リンク (ベースリンク, リンク1，手先位置) の位置を計算
all_link_pos = robot.forward_kinematics_all_link_pos(all_link_thetas[i])
# 線プロット
self._axis.plot(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], color="blue")
# 点プロット
self._axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], color="black", alpha=0.5)
# 手先位置の軌跡をプロット
self._axis.plot(all_link_poses[:i + 1, 0], all_link_poses[:i + 1, 1], all_link_poses[:i + 1, 2], color="lime")
def _plot_3DAnimation(self, robot, all_link_thetas, environment, anime_file_name):
"""
3次元アニメーションの作成
パラメータ
robot(Robot3DoF): ロボットクラス
all_link_thetas(numpy.ndarray): 全リンクの回転角度
environment(Robot3DEnv): 経路生成時の環境
anime_file_name(str): アニメーションのファイル名
"""
# データをリセットする
self._reset3D()
# 全位置を計算する
all_link_poses = np.zeros((all_link_thetas.shape[0], DIMENTION_3D))
for i, thetas in enumerate(all_link_thetas):
# 順運動学による位置(x, y, z)・姿勢(z-y-x)の計算
poses = robot.forward_kinematics(thetas)
# 位置だけを取得
all_link_poses[i] = poses[:DIMENTION_3D]
# アニメーションのフレーム数
n_frame = all_link_thetas.shape[0]
animation = ani.FuncAnimation(self._figure, self._update_3Ddata, fargs=(robot, all_link_thetas, all_link_poses, environment), interval=100, frames=n_frame)
# アニメーション
if anime_file_name:
animation.save(anime_file_name, writer="imagemagick")
else:
animation.save(self._ANIMATION_NAME, writer="imagemagick")
plt.show()
def plot_Animation(self, dimention, robot, all_link_thetas, environment, anime_file_name=""):
"""
アニメーション作成
パラメータ
dimention(int): 次元数
robot(Robot): ロボットクラス
all_link_thetas(numpy.ndarray): 全リンクの回転角度
environment(Robot2DEnv): 経路生成時の環境
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}")
if dimention == DIMENTION_2D:
# 2軸ロボットアームの2次元アニメーション
self._plot_2DAnimation(robot, all_link_thetas, environment, anime_file_name)
elif dimention == DIMENTION_3D:
# 3軸ロボットアームの3次元アニメーション
self._plot_3DAnimation(robot, all_link_thetas, environment, anime_file_name)
elif dimention == DIMENTION_6D:
# 6軸ロボットアームの3次元アニメーション
self._plot_3DAnimation(robot, all_link_thetas, environment, anime_file_name)
else:
# 異常
raise ValueError(f"dimention is abnormal. dimention is {dimention}")
メイン処理 (main.py)
メイン処理ファイル (main.py) を下記に記す．
main.py
# メイン処理
# ライブラリの読み込み
import numpy as np # 数値計算
# 自作モジュールの読み込み
from constant import * # 定数
from robot import Robot6DoF # ロボットクラス
from animation import RRTRobotAnimation # ロボットのアニメーション
from rotation import MyRotation
LINE_WIDTH = 3 # プロット時の線の太さ
def main():
"""
メイン処理
"""
# 6軸ロボットのリンク長
links = np.array([1.0, 1.0, 1.0, 0.1, 0.1, 0.1])
# 6軸ロボットのインスタンスを作成
robot = Robot6DoF(links)
# 始点 (位置(x, y, z), 姿勢(z, y, x))
start_pos = np.array([ 1.0, -1.0, 1.0, 0, 0 , 0])
# 終点
end_pos = np.array([-1.0, 1.0, 2.0, 0, np.pi/2, 0])
try:
# 始点と終点の逆運動学
start_theta = robot.inverse_kinematics(start_pos)
end_theta = robot.inverse_kinematics(end_pos)
except Exception as e:
# 逆運動学の解が存在しない (始点または終点が異常な値)
raise ValueError(f"please start_pos or end_pos is change. pos is singularity")
# 目標位置の角度を取得
target_theta = robot.differential_inverse_kinematics(start_theta, end_pos)
target_pos = robot.forward_kinematics(target_theta)
end_pos = robot.forward_kinematics(end_theta)
# print(f"robot.forward_kinematics(target_theta) = {target_pos}")
# print(f"robot.forward_kinematics(end_theta) = {end_pos}")
# 回転行列の計算
target_rotation = MyRotation().rot_from_zyx_euler(target_pos[3:])
end_rotation = MyRotation().rot_from_zyx_euler(end_pos[3:])
# print(f"target_rotation = {target_rotation}")
# print(f"end_rotation = {end_rotation}")
# for i in range(3):
# print(f"target_rotation[:, i].norm = {np.linalg.norm(target_rotation[:, i])}")
# print(f"end_rotation[:, i].norm = {np.linalg.norm(end_rotation[:, i])}")
# アニメーション作成
robotAnime = RRTRobotAnimation()
# ファイル名
file_name = "jacovian_robot_pos_anime.gif"
robotAnime.plot_Animation(DIMENTION_6D, robot, np.array(robot.jacov_thetas), None, file_name)
if __name__ == "__main__":
# 本ファイルがメインで呼ばれた時の処理
main()
処理結果
main.pyを実行し，初期位置から目標位置まで移動できているかのアニメーションは下図である．
アニメーションの凡例は下記の通りとなる．
・水色の点(右下側)：ロボットの初期位置 $P(1.0, -1.0)$
・赤色の点(上側)：ロボットの目標位置 $P(1.0, 1.8)$
・緑色の軌跡：始点から終点までのロボットの手先の軌跡
・青色の線：ロボットアームの各リンク
・黒色の点：ロボットアームの各リンクのジョイント
微分逆運動学により，6軸ロボットアームを初期位置から目標位置へ移動させれることが確認できた．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました．
・6軸ロボットアームの微分逆運動学 + アニメーション
次記事では，下記内容を実装していきます．
・作成したロボットアームに関する記事のまとめ