はじめに
私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
前記事では，干渉物が存在する環境下での2軸ロボットアームの逆運動学を用いた経路生成を説明した．
(https://qiita.com/haruhiro1020/items/b42725df00e13ddcb5af)
本記事では，干渉物が存在しない環境下での2軸ロボットアームの微分逆運動学を用いた軌道生成を説明する．
微分逆運動学(順運動学にて算出した手先位置を微分して，逆運動学を解く)については，後ほど説明する．
また，2軸ロボットアームの順運動学 (関節角度から手先位置を算出する) と逆運動学 (手先位置から関節角度を算出する) は以下で説明した．
(・https://qiita.com/haruhiro1020/items/4529e648161beac7754f
・https://qiita.com/haruhiro1020/items/9416bea4b5b0757f4f36)
本記事で実装すること
・2軸ロボットアームで，微分逆運動学を使用した軌道生成を実装
本記事では実装できないこと (将来実装したい内容)
・3軸ロボットアームで，微分逆運動学を使用した軌道生成を実装
・2軸ロボットアームで，干渉物が存在する環境下での微分逆運動学と経路生成(RRT)
・特異点(逆運動学の解が一意に決まらない)対応
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)
2軸ロボットアームに関して
2軸ロボットアームに関して説明する．
今回は下図のような2軸ロボットアームを考える．
以前の記事(https://qiita.com/haruhiro1020/items/4529e648161beac7754f) にて，順運動学(関節角度からロボットの手先位置を算出)による手先位置を計算した．計算結果は下式の通りとなる．
\displaylines{
x(t) = l_{1} * \cos\theta_{1}(t) + l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t) ) ... ① \\
y(t) = l_{1} * \sin\theta_{1}(t) + l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t) ) ... ② \\
}
上式を微分して，手先速度と関節速度の関係性を算出して，ロボットアームを動かすのが本記事の目標となる．
微分逆運動学に関して
下記のように，手先位置を$P(x, y)$から$P(x+dx, y+dy)$に移動させることを考えてみる．
移動させる時に，関節角度も変更する必要がある．手先速度と関節速度の関係性を算出することで，関節速度の解を使用して，目標位置へ移動させれるようになることが目標である．
手先速度を算出するために，①の$x(t)$と②の$y(t)$を$t$で微分すると下式の通りとなる．
\displaylines{
はじめにx(t)をtで微分する．\\
dx(t)/dt = dx(t)/d\theta_{1} * d\theta_{1}/dt + dx(t)/d\theta_{2} * d\theta_{2}/dt \\
dx(t)/d\theta_{1}とdx(t)/d\theta_{2}は下式の通りとなる． \\
dx(t)/d\theta_{1} = l_{1} * (-\sin\theta_{1}(t) + l_{2} * (-\sin(\theta_{1}(t) + \theta_{2}(t)) \\
dx(t)/d\theta_{2} = l_{2} * (-\sin(\theta_{1}(t) + \theta_{2}(t)) \\
上記より，dx(t)/dtが求まる．\\
dx(t)/dt = ( -l_{1} * \sin\theta_{1}(t) - l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t)) ) * d\theta_{1}/dt - l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t) ) * d\theta_{2}/dt \\
次にy(t)をtで微分する．\\
dy(t)/dt = dy(t)/d\theta_{1} * d\theta_{1}/dt + dy(t)/d\theta_{2} * d\theta_{2}/dt \\
dy(t)/d\theta_{1}とdy(t)/d\theta_{2}は下式の通りとなる． \\
dy(t)/d\theta_{1} = l_{1} * (\cos\theta_{1}(t) + l_{2} * (\cos(\theta_{1}(t) + \theta_{2}(t)) \\
dy(t)/d\theta_{2} = l_{2} * (\cos(\theta_{1}(t) + \theta_{2}(t)) \\
上記より，dy(t)/dtが求まる．\\
dy(t)/dt = ( l_{1} * \cos\theta_{1}(t) + l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t)) ) * d\theta_{1}/dt + l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t) ) * d\theta_{2}/dt \\
最後にdx(t)/dtとdy(t)/dtを行列形式で表す．\\
dP(t)/dt
=
\begin{pmatrix}
dx(t)/dt \\
dy(t)/dt \\
\end{pmatrix}
=
\begin{pmatrix}
( -l_{1} * \sin\theta_{1}(t) - l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t)) ) * d\theta_{1}/dt - l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t) ) * d\theta_{2}/dt \\
( l_{1} * \cos\theta_{1}(t) + l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t)) ) * d\theta_{1}/dt + l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t) ) * d\theta_{2}/dt \\
\end{pmatrix} \\
dP(t)/dt
=
\begin{pmatrix}
dx(t)/dt \\
dy(t)/dt \\
\end{pmatrix}
=
\begin{pmatrix}
-l_{1} * \sin\theta_{1}(t) - l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t)) & - l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t) ) \\
l_{1} * \cos\theta_{1}(t) + l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t)) & l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t) ) \\
\end{pmatrix}
\begin{pmatrix}
d\theta_{1}/dt \\
d\theta_{2}/dt \\
\end{pmatrix} \\
よって，手先速度と関節速度には以下のような関係式となる．\\
dP(t)/dt = J(\theta(t)) * d\theta/dt \\
ここで，各行列/ベクトルは以下のように定義される．\\
dP(t)/dt = \begin{pmatrix}
dx(t)/dt \\
dy(t)/dt \\
\end{pmatrix} \\
J(\theta(t)) = \begin{pmatrix}
-l_{1} * \sin\theta_{1}(t) - l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t)) & - l_{2} * \sin(\theta_{1}(t) + \theta_{2}(t) ) \\
l_{1} * \cos\theta_{1}(t) + l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t)) & l_{2} * \cos(\theta_{1}(t) + \theta_{2}(t) ) \\
\end{pmatrix} \\
d\theta/dt = \begin{pmatrix}
d\theta_{1}/dt \\
d\theta_{2}/dt \\
\end{pmatrix} \\
}
$J(\theta(t))$を(解析的)ヤコビ行列と呼ぶ．
上式より，手先速度と関節速度の関係を式で表すことができる．
上関係より，手先速度が求まっていたら，関節速度を計算することが可能となる．
関節速度の算出方法は下式の通りとなる．
(2 * 2行列の逆行列は下記リンクを参考とした．
https://risalc.info/src/inverse-cofactor-ex2.html )
\displaylines{
dP(t)/dt = J(\theta) * d\theta/dt \\
d\theta/dt = J^{-1}(\theta) * dP(t)/dt \\
J(\theta)より，J^{-1}(\theta)は下記のようになる．\\
J^{-1}(\theta)
=
1/\det J(\theta)
\begin{pmatrix}
l_{2} * \cos(\theta_{1} + \theta_{2}) & l_{2} * \sin(\theta_{1} + \theta_{2}) \\
-(l_{1} * \cos\theta_{1} + l_{2} * \cos(\theta_{1} + \theta_{2})) & -l_{1} * \sin\theta_{1} - l_{2} * \sin(\theta_{1} + \theta_{2}) \\
\end{pmatrix} \\
\det J(\theta) = ( -l_{1} * \sin\theta_{1} - l_{2} * \sin(\theta_{1} + \theta_{2}) ) * l_{2} * \cos(\theta_{1} + \theta_{2} ) + (l_{2} * \sin(\theta_{1} + \theta_{2} ) ) * (l_{1} * \cos\theta_{1} + l_{2} * \cos(\theta_{1} + \theta_{2})) \\
\det J(\theta) = -l_{1} * l_{2} * \sin\theta_{1} * \cos(\theta_{1} + \theta_{2}) - l^{2}_{2} * \sin(\theta_{1} + \theta_{2}) * \cos(\theta_{1} + \theta_{2}) + l_{1} * l_{2} * \cos\theta_{1} * \sin(\theta_{1} + \theta_{2}) + l^{2}_{2} * \sin(\theta_{1} + \theta_{2}) * \cos(\theta_{1} + \theta_{2}) \\
\det J(\theta) = l_{1} * l_{2} * (-\sin\theta_{1} * (\cos\theta_{1} * \cos\theta_{2} - \sin\theta_{1} * \sin\theta_{2}) + \cos\theta_{1} * (\sin\theta_{1} * \cos\theta_{2} + \cos\theta_{1} * \sin\theta_{2}) \\
\det J(\theta) = l_{1} * l_{2} * (-\sin\theta_{1} * \cos\theta_{1} * \cos\theta_{2} + \sin^{2}\theta_{1} * \sin\theta_{2} + \sin\theta_{1} * \cos\theta_{1} * \cos\theta_{2} + \cos^{2}\theta_{1} * \sin\theta_{2}) \\
\det J(\theta) = l_{1} * l_{2} * \sin\theta_{2} * (\sin^{2}\theta_{1} + \cos^{2}\theta_{1}) = l_{1} * l_{2} * \sin\theta_{2} \\
}
行列式である$\det J(\theta(t))$が$0$以外であれば，$J^{-1}(\theta(t))$を算出することが可能となる．
ヤコビ行列の逆行列を算出が可能であれば，関節速度も求めることができる．
逆行列が算出できない時は特異点と呼ばれる．
ヤコビ行列の逆行列が算出できない場合を考えてみよう．
$\det J(\theta(t)) = l_{1} * l_{2} * \sin\theta_{2}$ が $0$ の時に，逆行列が算出できなくなる．
要するに，$\sin\theta_{2} = 0$の時に特異点となる．($\theta_{2} = 0, \pi$の時に特異点となる．)
ロボットアームが伸び切った状態または，折り畳まれた状態が特異点となる．
今回は特異点に対しての対応はしない．
他記事にて，特異点対応の方法を説明する．
微分逆運動学のソースコード
本記事では，ロボットアームの初期位置を$P(1.0, -1.0)$，目標位置を$P(0.0, 1.8)$として，微分逆運動学によりロボットを動かす．
定数を定義するファイル (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
回転行列を定義するファイル (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
2軸ロボットアームを定義するファイル (robot.py)
2軸ロボットアームを定義するファイルを下記に記す．
Roboto2DoF()クラスの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): 干渉判定クラス
_jacov_thetas(list): 微分逆行列で取得した角度を保存
メソッド
public
forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
update(): 角度を与えて，各リンクの直方体を更新する
differential_inverse_kinematics(): 微分逆運動学
links(): _linksプロパティのゲッター
manager(): _managerプロパティのゲッター
jacov_thetas(): _jacov_thetasプロパティのゲッター
protected
_calc_homogeneou_matrix(): 同時変換行列の計算
_jacovian(): ヤコビ行列
"""
# 定数の定義
_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 # 回転行列の次元数
_DETERMINANT_THRESHOLD = 1e-4 # 行列式の閾値
_BOX_WIDTH = 1e-2 # 各リンクの幅を定義
_JACOV_DELTA_TIME = 0.10 # ヤコビの1時刻
_JACOV_NEAR_POS = 1e-6 # 目標位置との近傍距離 [m]
_JACOV_MAX_COUNT = 100 # ヤコビの最大回数
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 = []
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 differential_inverse_kinematics(self, thetas, target_pos):
"""
2点間の微分逆運動学
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
target_pos(numpy.ndarray): 目標位置 (位置[m]・姿勢[rad])
戻り値
target_thetas(numpy.ndarray): 目標位置の関節角度 [rad]
"""
raise NotImplementedError("differential_inverse_kinematics() is necessary override.")
def _jacovian(self, thetas):
"""
ヤコビ行列の計算
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
jacovian(numpy.ndarray): ヤコビ行列
"""
raise NotImplementedError("_jacovian() 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 Robot2DoF(Robot):
"""
2軸ロボットクラス
プロパティ
_links(numpy.ndarray): ロボットのリンク長
_rot(Rotation): 回転行列クラス
_axiss(list): 関節の回転軸
メソッド
public
forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_2D # 手先位置の次元数
_DIMENTION_THETA = DIMENTION_2D # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_2D # リンク数
def __init__(self, links):
"""
コンストラクタ
パラメータ
links(numpy.ndarray): ロボットのリンク長 [m]
"""
# 親クラスの初期化
super().__init__(links)
# ロボットの各リンクを直方体として定義する
self._objects = []
# (全リンクの角度を0とした時の) 各リンク間の相対位置
self._relative_positions = np.zeros((self._DIMENTION_POSE + 1, 3))
self._relative_positions[0] = np.array([0, 0, 0])
self._relative_positions[1] = np.array([self._links[0], 0, 0])
self._relative_positions[2] = np.array([self._links[1], 0, 0])
# リンク1とリンク2は回転軸がz軸である
self._axiss = [ROTATION_Z_AXIS, ROTATION_Z_AXIS]
# 初期角度
initial_thetas = np.zeros(self._DIMENTION_THETA)
# 順運動学により，全リンク(ベースリンク，リンク1，リンク2)の位置を計算
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 = np.zeros(DIMENTION_3D)
center[:self._DIMENTION_POSE] = all_link_pose[i + 1] / 2 + all_link_pose[i]
# 直方体の定義 (x, y, zの長さを保存)
box = fcl.Box(self._relative_positions[i + 1, 0], 2 * self._BOX_WIDTH, 2 * self._BOX_WIDTH)
# 直方体の中心を定義 (位置・姿勢)
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()
self._jacov_thetas = []
def forward_kinematics(self, thetas):
"""
順運動学 (ロボットの関節角度からロボットの手先位置を算出)
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
# 同時変換行列(2 * 4 * 4)を計算する
homogeneou_matrix = self._calc_homogeneou_matrix(thetas)
# 最終リンクの同時変換行列(最終リンク座標の位置・姿勢)より，手先位置を計算する
final_link_matrix = homogeneou_matrix[self._DIMENTION_LINK - 1]
# 最終リンクから手先位置までの早退位置(4ベクトル)を定義
relative_pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
relative_pos[:self._HOMOGENEOU_MAT_ELEMENT - 1] = self._relative_positions[-1]
pose = np.dot(final_link_matrix, relative_pos)
# 手先位置(x, y)を取得
pose = pose[:self._DIMENTION_POSE]
return pose
def differential_inverse_kinematics(self, thetas, target_pos):
"""
2点間の微分逆運動学
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
target_pos(numpy.ndarray): 目標位置 (位置[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)
# 目標値の関節角度が見つかったどうか
success_flg = False
# 微分逆行列で取得したデータを初期化
self._reset_jacov_thetas()
# 計算した角度を保存する
self._jacov_thetas.append(current_thetas)
# 目標位置に近づくまでループ
for i in range(self._JACOV_MAX_COUNT):
# ヤコビ行列の取得
jacovian = self._jacovian(current_thetas)
# dP(位置の差分) = J(ヤコビ行列) * dTheta(角度の差分)
# dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
# 行列式が0近傍であるかの確認(ヤコビ行列の逆行列の存在確認)
det = np.linalg.det(jacovian)
print(f"det = {det}")
if abs(det) <= self._DETERMINANT_THRESHOLD:
# 0近傍であるため，逆行列が存在しない
raise ValueError(f"abs(det) is near 0. abs(det) is {abs(det)}")
# 現在の手先位置を計算
current_pos = self.forward_kinematics(current_thetas)
# 位置の差分を計算
dP = target_pos - current_pos
# dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
dTheta = np.dot(np.linalg.inv(jacovian), dP)
# 関節角度の更新 (+=とするとcurrent_thetasが常に同じアドレスになるため，current_thetasを異なるアドレスとしたい)
# print(f"id(current_thetas) = {id(current_thetas)}") # アドレス確認
# current_thetas = current_thetas + dTheta * self._JACOV_DELTA_TIME
# 位置の差分が大きいほど，角度の更新量を小さくしたい
coefficient = 1 / max(0.1, np.linalg.norm(dP))
current_thetas = current_thetas + dTheta * coefficient * self._JACOV_DELTA_TIME
# print(f"id(current_thetas) = {id(current_thetas)}") # アドレス確認
# 計算した角度を保存する
self._jacov_thetas.append(current_thetas)
# 更新後の手先位置が目標位置の近傍であれば，処理終了とする
current_pos = self.forward_kinematics(current_thetas)
distance = np.linalg.norm(target_pos - current_pos)
# print(f"distance = {distance}")
if distance <= self._JACOV_NEAR_POS:
# 近傍のため，処理終了
success_flg = True
break
if not success_flg:
# 目標位置の関節角度が見つからない
raise ValueError(f"target_pos's theta is not found. please change target_pos")
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)}")
# jacovian = [[-l1 * sin(theta1) - l2 * sin(theta12), -l2 * sin(theta12)], [l1 * cos(theta1) + l2 * cos(theta12), l2 * cos(theta12)]]
# 各リンクの長さをローカル変数に保存
l1 = self._links[0]
l2 = self._links[1]
# 三角関数の計算
sin1 = np.sin(thetas[0])
cos1 = np.cos(thetas[0])
sin12 = np.sin(thetas[0] + thetas[1])
cos12 = np.cos(thetas[0] + thetas[1])
# ヤコビ行列
jacovian = np.array([[-l1 * sin1 - l2 * sin12, -l2 * sin12],
[ l1 * cos1 + l2 * cos12, l2 * cos12]])
return jacovian
def inverse_kinematics(self, pose, upper=False):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
upper(bool): 腕が上向かどうか
戻り値
thetas(numpy.ndarray): ロボットの関節角度 [rad]
"""
# パラメータの次元数を確認
if np.size(pose) != self._DIMENTION_POSE:
raise ValueError(f"parameter pose's size is abnormal. pose's size is {np.size(pose)}")
# c2 = {(px ** 2 + py ** 2) - (l1 ** 2 + l2 ** 2)} / (2 * l1 * l2)
px = pose[0]
py = pose[1]
l1 = self._links[0]
l2 = self._links[1]
cos2 = ((px ** 2 + py ** 2) - (l1 ** 2 + l2 ** 2)) / (2 * l1 * l2)
# cosの範囲は-1以上1以下である
if cos2 < -1 or cos2 > 1:
# 異常
raise ValueError(f"cos2 is abnormal. cos2 is {cos2}")
# sinも求めて，theta2をatan2()より算出する
sin2 = np.sqrt(1 - cos2 ** 2)
theta2 = np.arctan2(sin2, cos2)
if not upper:
# 下向きの角度のため，三角関数も更新
theta2 = -theta2
sin2 = np.sin(theta2)
cos2 = np.cos(theta2)
# 行列計算
# [c1, s1] = [[l1 + l2 * c2, -l2 * s2], [l2 * s2, l1 + l2 * c2]] ** -1 * [px, py]
element1 = l1 + l2 * cos2
element2 = -l2 * sin2
matrix = np.array([[ element1, element2],
[-element2, element1]])
# 行列式を計算
det = np.linalg.det(matrix)
# 0近傍の確認
if det <= self._DETERMINANT_THRESHOLD and det >= -self._DETERMINANT_THRESHOLD:
# 0近傍 (異常)
raise ValueError(f"det is abnormal. det is {det}")
# [c1, s1]の計算
cos1_sin1 = np.dot(np.linalg.inv(matrix), pose)
# theta1をatan2()より算出する
theta1 = np.arctan2(cos1_sin1[1], cos1_sin1[0])
thetas = np.array([theta1, theta2])
return thetas
def forward_kinematics_all_link_pos(self, thetas):
"""
順運動学で全リンクの位置を取得 (グラフの描画で使用する)
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
all_link_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
"""
# パラメータの次元数を確認
if np.size(thetas) != self._DIMENTION_THETA:
raise ValueError(f"thetas's size is abnormal. thetas's size is {np.size(thetas)}")
# 同時変換行列(2 * 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):
# 同時変換行列から位置を取得
pos = matrix[:self._DIMENTION_POSE, self._ROTATION_MAT_ELEMENT].reshape(1, -1)
all_link_pose[i] = pos
# 最後のリンクの座標系の原点から，手先の位置を計算する
pos = np.ones(self._HOMOGENEOU_MAT_ELEMENT)
pos[:DIMENTION_3D] = self._relative_positions[-1]
all_link_pose[-1] = np.dot(homogeneou_matix[-1], pos)[:self._DIMENTION_POSE].reshape(1, -1)
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)の位置を計算
all_link_pose = self.forward_kinematics_all_link_pos(thetas)
# 1つ前のリンクの回転行列を定義
prev_rotation = np.eye(self._ROTATION_MAT_ELEMENT)
# ロボットの各リンクを直方体として定義する
for i in range(self._DIMENTION_THETA):
# 各リンクの回転行列を定義
rotation = self._rot.rot(thetas[i], self._axiss[i])
# 1つ前のリンクの回転も考慮する
rotation = np.dot(prev_rotation, rotation)
# 各リンクの中心位置 (x, y, z) を定義
center = np.zeros(DIMENTION_3D)
center[:self._DIMENTION_POSE] = (all_link_pose[i + 1] - all_link_pose[i]) / 2 + all_link_pose[i]
# 直方体の中心を定義 (位置・姿勢)
translation = fcl.Transform(rotation, center)
# モデルの位置を更新
self._objects[i].setTransform(translation)
# 1つ前のリンクの回転行列を更新
prev_rotation = rotation
# 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 constant import * # 定数
class RobotAnimation:
"""
ロボットのアニメーション作成
プロパティ
_figure: 描画枠
_axis: 描画内容
publicメソッド (全てのクラスから参照可能)
plot_Animation(): アニメーション作成
protectedメソッド (自クラスまたは子クラスが参照可能)
_reset2D(): 2次元データのリセット
"""
# 定数の定義
_ANIMATION_NAME = "robot_animation.gif"
_PLOT_NAME = "robot_plot.gif"
def __init__(self):
"""
コンストラクタ
"""
pass
def _reset2D(self):
"""
2次元データのリセット
"""
self._figure = plt.Figure()
self._axis = self._figure.add_subplot(111)
# X/Y軸に文字を記載
self._axis.set_xlabel("X")
self._axis.set_ylabel("Y")
self._axis.grid()
self._axis.set_aspect("equal")
def _plot_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, x, y, width, height, angle):
"""
長方形の描画
パラメータ
x(float): 左下隅の座標 (x)
y(float): 左下隅の座標 (y)
width(float): 幅
height(float): 高さ
angle(float): 角度 [deg]
"""
rect = patches.Rectangle((x, y), width, height, angle=angle, color="gray", alpha=0.5)
# 長方形を軸に追加
self._axis.add_patch(rect)
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(center_x - x / 2, center_y - y / 2, x, y, angle)
else:
# 何もしない
pass
def plot_Animation(self, robot, all_link_thetas, environment, anime_file_name=""):
"""
アニメーション作成
パラメータ
robot(Robot2DoF): ロボットクラス
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}")
# データをリセットする
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()
メイン処理ファイルを下記に記す．
main.py
# メイン処理
# ライブラリの読み込み
import numpy as np # 数値計算
import matplotlib.pyplot as plt # 描画
# 自作モジュールの読み込み
from constant import * # 定数
from robot import Robot2DoF # ロボットクラス
from animation import RobotAnimation # ロボットのアニメーション
def main():
"""
メイン処理
"""
# 2軸ロボットのリンク長
links = np.array([1.0, 1.0])
# 2軸ロボットのインスタンスを作成
robot = Robot2DoF(links)
# 始点
# start_pos = np.array([1.0, 0.0])
start_pos = np.array([1.0, -1.0])
# 終点
# end_pos = np.array([0.0, 1.0])
end_pos = np.array([0.0, 1.8])
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)
print(f"target_theta = {target_theta}")
print(f"end_theta = {end_theta}")
print(f"robot.jacov_thetas[-1] = {robot.jacov_thetas[-1]}")
print(f"forwrad_kinematics = {robot.forward_kinematics(target_theta)}")
# アニメーション作成
robotAnime = RobotAnimation()
# 関節空間による RRT 経路生成
# ファイル名
file_name = "jacov_robot_anime.gif"
# robot.jacov_thetasはlist型であり，numpy.ndarray型に変換するためにnp.array()を採用
robotAnime.plot_Animation(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)$
・緑色の軌跡：始点から終点までのロボットの手先の軌跡
・青色の線：ロボットアームの各リンク
・黒色の点：ロボットアームの各リンクのジョイント
微分逆運動学により，2軸ロボットアームを初期位置から目標位置へ移動させれることが確認できた．
今回は2軸ロボットアームであったが，3軸ロボットアームでの微分逆運動学を実行したい．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました．
・2軸ロボットアームの微分逆運動学 + アニメーション
次記事では，下記内容を実装していきます．
・3軸ロボットアームの微分逆運動学 + アニメーション