はじめに
私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
前記事では，2軸ロボットアームの微分逆運動学を用いた軌道生成を説明した．
(・https://qiita.com/haruhiro1020/items/0f8c372cfb2988e47cf4)
本記事では，2軸ロボットアームの微分逆運動学の特異点近傍への対処方法を説明する(特異点近傍への対処を実施しないと，実際のロボットを動かそうとすると，安定しなくなる)．
微分逆運動学(順運動学にて算出した手先位置を微分して，逆運動学を解く)については，後ほど説明する．
本記事では，特異点近傍への対処方法として，レーベンバーグ・マルカート法を採用する．
レーベンバーグ・マルカート法は下記サイトを参考にした．
・https://qiita.com/naturemon/items/ec1154a708841f27c4d0
特異点近傍への対処方法にはレーベンバーグ・マルカート法以外も存在する．興味がありましたら，下記の論文を拝読ください．
参考文献：
杉原 “逆運動学の数値解法.”, 日本ロボット学会誌 2016
本記事で実装すること
・2軸ロボットアームで，微分逆運動学の特異点近傍への対処方法を実装
本記事では実装できないこと (将来実装したい内容)
・3軸，6軸ロボットアームで，微分逆運動学による軌道生成
・レーベンバーグ・マルカート法のパラメータを特異点近傍であるかによって(可操作度により，特異点近傍であるかの確認ができる)変更していく
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)
微分逆運動学に関して
ロボットの手先位置・姿勢と各関節角度は下記の数式で表現することができる(下式を順運動学という)．
\displaylines{
r = f(q) \\
r ... ロボットの手先位置・姿勢 \\
f() ... 関節角度から手先位置・姿勢に変換する関数 \\
q ... ロボットの各関節角度 \\
}
上式より，関節角度から手先位置・姿勢を算出できるため，手先位置・姿勢を微分してみると，下式の通りとなる．
\displaylines{
dr/dt = J(q) * dq/dt \\
J(q) = df(q)/dq \\
J(q) ... ヤコビ行列 \\
}
上式では，ヤコビ行列が算出できれば，関節速度を与えることで，手先速度・角速度を算出することが可能となる．
しかし，現実では関節速度を与えることは困難である．手先速度・角速度を与える方が容易である．
そのため，上式を$dq/dt$に関して解いてみると下式のようになる．
\displaylines{
dq/dt = J^{-1}(q) * dr/dt \\
J^{-1}(q) ... ヤコビ行列の逆行列 \\
}
上式より，手先速度・角速度を与えることで，関節速度を算出することが可能となる．
しかしながら，厄介なことは$J^{-1}(q)$の算出失敗(0割)や数値誤差(0近傍での割り算)である．
逆行列が安定しないことで，微分逆運動学の解も安定しなくなってしまう(特異点や特異点近傍では，逆行列が安定しなくなる)．
解を安定させるために，レーベンバーグ・マルカート法を使用する．
レーベンバーグ・マルカート法
レーベンバーグ・マルカート法に関しては，下記サイトを参考にさせていただきました．
(https://qiita.com/naturemon/items/ec1154a708841f27c4d0)
数式としては，下記のようになります．
\displaylines{
(J(q)^{T} * J(q) + \lambda * I) * dq = J(q)^{T} * dr \\
J(q) ... ヤコビ行列 \\
J(q)^{T} ... ヤコビ行列の転置行列 (J(q)のi行，j列のデータをj列，i行に入れ換えた行列が転置行列) \\
\lambda ... 0より大きい値で，正則化パラメータ \\
I ... 単位行列であり，行数と列数は関節数と同じとなる． \\
dq ... 関節速度 \\
dr ... 手先速度・角速度 \\
}
$dq$に関して解くと下記のようになります．
\displaylines{
dq = (J(q)^{T} * J(q) + \lambda * I))^{-1} * dr \\
}
$\lambda$の値に応じて，処理が変わる．
$\lambda$が$0$近傍の場合は，通常の逆行列と同じになるため，特異点近傍では，解が不安定になるが，目標位置への収束が早い．
$\lambda$が大きい値の場合は，特異点近傍でも解が安定するが，目標位置への収束に時間がかかる．
特異点近傍かどうかに応じて，$\lambda$の値を変更していくのが良い案である．
しかしながら，本記事では$\lambda$の値は固定値とする．
将来的には，特異点近傍かどうかを判定するために"可操作度"と呼ばれる値を使用して，$\lambda$の値を変更しようと考えている．
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) ) \\
}
2軸ロボットアームのヤコビ行列
以前の記事(https://qiita.com/haruhiro1020/items/0f8c372cfb2988e47cf4) にて，ヤコビ行列を計算した．計算結果は下式の通りとなる(下式は上記の手先位置を微分すると算出できる)．
\displaylines{
J(\theta) = \begin{pmatrix}
-l_{1} * \sin\theta_{1} - l_{2} * \sin(\theta_{1} + \theta_{2}) & - l_{2} * \sin(\theta_{1} + \theta_{2} ) \\
l_{1} * \cos\theta_{1} + l_{2} * \cos(\theta_{1} + \theta_{2}) & l_{2} * \cos(\theta_{1} + \theta_{2} ) \\
\end{pmatrix} \\
}
微分逆運動学のソースコード
本記事では，ロボットアームの初期位置を$P(-1.9, 0.1)$，目標位置を$P(1.9, 0.1)$として，微分逆運動学によりロボットを動かす．
定数を定義するファイル (constant.py)
定数を定義するファイルを下記に記す．
constant.py
# 複数ファイルで使用する定数の定義
# 次元数を定義
DIMENTION_NONE = -1 # 未定義
DIMENTION_2D = 2 # 2次元
DIMENTION_3D = 3 # 3次元
# 回転軸
ROTATION_X_AXIS = "rot_x" # x軸周りに回転
ROTATION_Y_AXIS = "rot_y" # y軸周りに回転
ROTATION_Z_AXIS = "rot_z" # z軸周りに回転
ROTATION_X_NEGATIVE_AXIS = "rot_neg_x" # x軸周りに逆回転
ROTATION_Y_NEGATIVE_AXIS = "rot_neg_y" # y軸周りに逆回転
ROTATION_Z_NEGATIVE_AXIS = "rot_neg_z" # z軸周りに逆回転
# 0割を防ぐための定数
EPSILON = 1e-6
回転行列を定義するファイル (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-6 # 行列式の閾値
_BOX_WIDTH = 1e-2 # 各リンクの幅を定義
_JACOV_DELTA_TIME = 0.1 # ヤコビの1時刻
_JACOV_NEAR_POS = 1e-10 # 目標位置との近傍距離 [m]
_JACOV_MAX_COUNT = 100 # ヤコビの最大回数
_LEVENBERG_MARUQUARDT_LAMBDA = 1 # Levenberg-Marquardt法で使用するパラメータ
def __init__(self, links, levenberg=True):
"""
コンストラクタ
パラメータ
links(numpy.ndarray): ロボットのリンク長 [m]
levenberg(bool): レーベンバーグ・マルカート法の実装有無
"""
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
self._levenberg_maruquardt = levenberg
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, levenberg=True):
"""
コンストラクタ
パラメータ
links(numpy.ndarray): ロボットのリンク長 [m]
levenberg(bool): レーベンバーグ・マルカート法の実装有無
"""
# 親クラスの初期化
super().__init__(links, levenberg)
# ロボットの各リンクを直方体として定義する
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]
sin1 = np.sin(thetas[0])
cos1 = np.cos(thetas[0])
cos1_sin1 = np.array([cos1, sin1])
sin12 = np.sin(thetas[0] + thetas[1])
cos12 = np.cos(thetas[0] + thetas[1])
cos12_sin12 = np.array([cos12, sin12])
pose = self._links[0] * cos1_sin1 + self._links[1] * cos12_sin12
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)
# 微分逆行列で取得したデータを初期化
self._reset_jacov_thetas()
# 計算した角度を保存する
self._jacov_thetas.append(current_thetas)
# 目標位置に近づくまでループ
for _ in range(self._JACOV_MAX_COUNT):
# ヤコビ行列の取得
jacovian = self._jacovian(current_thetas)
# 現在の手先位置を計算
current_pos = self.forward_kinematics(current_thetas)
# 位置の差分を計算
dP = target_pos - current_pos
if self._levenberg_maruquardt:
# 特異点対応として，レーベンバーグ・マルカート法を実施
# 逆行列の中身を作成
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), dP)
else:
# 特異点対応の未実施
# dP(位置の差分) = J(ヤコビ行列) * dTheta(角度の差分)
# dTheta = J^(-1)(ヤコビ行列の逆行列) * dP
# 行列式が0近傍であるかの確認(ヤコビ行列の逆行列の存在確認)
det = np.linalg.det(jacovian)
if abs(det) <= self._DETERMINANT_THRESHOLD:
# 0近傍であるため，逆行列が存在しない
raise ValueError(f"abs(det) is near 0. abs(det) is {abs(det)}")
# 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.5, np.linalg.norm(dP))
coefficient = 1
current_thetas = current_thetas + dTheta * coefficient * self._JACOV_DELTA_TIME
# 範囲 [-π, π] に正規化（任意）
current_thetas = (current_thetas + np.pi) % (2 * np.pi) - np.pi
# 計算した角度を保存する
self._jacov_thetas.append(current_thetas)
# 現在位置の更新
current_pos = self.forward_kinematics(current_thetas)
distance = np.linalg.norm(target_pos - current_pos)
if distance <= self._JACOV_NEAR_POS:
# 近傍のため，処理終了
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)}")
# 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 mpl_toolkits.mplot3d.art3d import Poly3DCollection # 3次元形状の描画
# 自作モジュールの読み込み
from constant import * # 定数
from rotation import MyRotation # 回転行列
class RobotAnimation:
"""
ロボットのアニメーション
プロパティ
_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, facecolors='gray', edgecolors='gray', alpha=0.5))
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 = []
# 手先位置の軌跡を保存
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, 20)
theta_2_0 = np.linspace(0, np.pi * 2, 20)
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)
# 始点と終点をプロット
# 始点位置を取得
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):
# 順運動学による位置の計算
poses = robot.forward_kinematics(thetas)
all_link_poses[i] = poses
# アニメーションのフレーム数
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(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}")
if dimention == DIMENTION_2D:
# 2次元アニメーション
self._plot_2DAnimation(robot, all_link_thetas, environment, anime_file_name)
elif dimention == DIMENTION_3D:
# 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
# メイン処理
# ライブラリの読み込み
import numpy as np # 数値計算
import matplotlib.pyplot as plt # 描画
# 自作モジュールの読み込み
from constant import * # 定数
from robot import Robot2DoF # ロボットクラス
from animation import RobotAnimation # ロボットのアニメーション
def main():
"""
メイン処理
"""
# レーベンバーグ・マルカート法の実装有無
# levenberg = True
levenberg = False
# 2軸ロボットのリンク長
links = np.array([1.0, 1.0])
# 2軸ロボットのインスタンスを作成
robot = Robot2DoF(links, levenberg=levenberg)
# 始点
start_pos = np.array([-1.9, 0.1])
# start_theta = np.array([0.0, 0.0])
# 終点
end_pos = np.array([ 1.9, 0.1])
try:
# 始点の逆運動学
start_theta = robot.inverse_kinematics(start_pos)
except Exception as e:
# 逆運動学の解が存在しない
raise ValueError(f"please start_pos is change. start_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(DIMENTION_2D, robot, np.array(robot.jacov_thetas), None, file_name)
if __name__ == "__main__":
# 本ファイルがメインで呼ばれた時の処理
main()
特異点近傍対応の有無による処理結果
特異点近傍対応の有無により，解が安定するかどうかを確認した結果を説明する．
下記に特異点近傍対応のあり/なしのアニメーションを記す．
アニメーションの凡例は下記の通りとなる．
・水色の点(左側)：ロボットの初期位置 $P(-1.9, 0.1)$
・赤色の点(上側)：ロボットの目標位置 $P( 1.9, 0.1)$
・緑色の軌跡：始点から終点までのロボットの手先の軌跡
・青色の線：ロボットアームの各リンク
・黒色の点：ロボットアームの各リンクのジョイント
下図は特異点近傍対応が未実施のアニメーションである．
特異点$(x, y) = (0, 0)$近傍で，解が不安定となったことを確認できる．
下図は特異点近傍対応が実施済みのアニメーションである．
特異点$(x, y) = (0, 0)$近傍で，解が安定となるが，目標位置への収束に時間がかかるし，ロボットの軌跡も遠回りになった．
レーベンバーグ・マルカート法のパラメータである$\lambda$の値を特異点近傍かどうかで，変更することで，対処できるのではないかと考えられる．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました．
・2軸ロボットアームの微分逆運動学の特異点近傍への対応 (レーベンバーグ・マルカート法) + アニメーション
次記事では，下記内容を実装していきます．
・6軸ロボットアームの微分逆運動学 + アニメーション
・特異点近傍かどうかで，レーベンバーグ・マルカート法のパラメータである$\lambda$の値を変更