はじめに
私がロボットに関して興味を持っている．特にロボットの経路生成に興味がある．
前記事では，3軸ロボットアームの順運動学(ロボットの関節角度からロボットの手先位置を算出する) を説明した．
(https://qiita.com/haruhiro1020/items/27c0cf098056dc792ab9)
本記事では，3軸ロボットアームの逆運動学を説明する．
逆運動学とは，手先位置から関節角度を計算することです．
関節角度を算出するために，三角関数(sin, cos, tan)を使用する．以下サイトはわかりやすいです．
・https://www.meikogijuku.jp/meiko-plus/study/trigonometric-function.html
本記事で実装すること
・3軸ロボットアームの逆運動学
本記事では実装できないこと (将来実装したい内容)
・3軸ロボットアームに経路生成手法のRRT を組み合わせる
動作環境
・macOS Sequoia (バージョン15.5)
・Python3 (3.10.9)
・Numpy (1.23.5) (数値計算用ライブラリ)
・Matplotlib (3.7.0) (アニメーション作成用ライブラリ)
逆運動学
逆運動学について説明する．
今回は下図のような3軸ロボットアームを考える．
$l_{1}$はリンク1の長さ[m]，$l_{2}$はリンク2の長さ[m]，$l_{3}$はリンク3の長さ[m]です．
$\theta_{1}$は関節1の角度[rad]であり，z軸方向に回転する．
$\theta_{2}$は関節2の角度[rad]であり，y軸方向に回転する．
$\theta_{3}$は関節3の角度[rad]であり，y軸方向に回転する．
各関節角度($\theta_{1}$，$\theta_{2}$, $\theta_{3}$)を手先位置$P(x, y, z)$から計算する．
前記事にて，手先位置$P(x, y, z)$を各関節角度($\theta_{1}$，$\theta_{2}$, $\theta_{3}$)から計算した．
その結果は下記の通りとなった．
\displaylines{
x = \cos\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) ) \\
y = \sin\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) ) \\
z = l_{1} + l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) \\
}
逆運動学で関節角度を算出するは，以下の順番で求める．
1：$\theta_{3}$
2：$\theta_{1}$
3：$\theta_{2}$
theta3 の算出方法
$\theta_{3}$の算出方法を説明する．
(1)：$x$，$y$，$z - l_{1}$を二乗して足し合わせる．
\displaylines{
x^{2} = \cos^{2}\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} \\
y^{2} = \sin^{2}\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} \\
(z - l_{1})^{2} = (l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) )^{2} \\
x^{2} + y^{2} + (z - l_{1})^{2} = (\cos^{2}\theta_{1} + \sin^{2}\theta_{1}) * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} + (l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) )^{2} \\
}
(2)：三角関数の公式を使用する．
\displaylines{
\sin^{2}\theta + \cos^{2}\theta = 1
\sin(\theta_{1} + \theta_{2}) = \sin\theta_{1} * \cos\theta_{2} + \cos\theta_{1} * \sin\theta_{2} \\
\sin(\theta_{1} - \theta_{2}) = \sin\theta_{1} * \cos\theta_{2} - \cos\theta_{1} * \sin\theta_{2} \\
\cos(\theta_{1} + \theta_{2}) = \cos\theta_{1} * \cos\theta_{2} - \sin\theta_{1} * \sin\theta_{2} \\
\cos(\theta_{1} - \theta_{2}) = \cos\theta_{1} * \cos\theta_{2} + \sin\theta_{1} * \sin\theta_{2} \\
}
(3)：(1)の結果に(2)を代入する．
\displaylines{
x^{2} + y^{2} + (z - l_{1})^{2} = (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} + (l_{2} * \sin\theta_{2} + l_{3} * \sin(\theta_{2} + \theta_{3}) )^{2} \\
x^{2} + y^{2} + (z - l_{1})^{2} = (l^{2}_{2} * \cos^{2}\theta_{2} + 2 * l_{2} * l_{3} * \cos\theta_{2} * \cos(\theta_{2} + \theta_{3}) + l^{2}_{3} * \cos^{2}(\theta_{2} + \theta_{3}) ) + (l^{2}_{2} * \sin^{2}\theta_{2} + 2 * l_{2} * l_{3} * \sin\theta_{2} * \sin(\theta_{2} + \theta_{3}) + l^{2}_{3} * \sin^{2}(\theta_{2} + \theta_{3}) ) \\
x^{2} + y^{2} + (z - l_{1})^{2} = (\cos^{2}\theta_{2} + \sin^{2}\theta_{2}) * l^{2}_{2} + (\cos^{2}(\theta_{2} + \theta_{3}) + \sin^{2}(\theta_{2} + \theta_{3}) ) * l^{2}_{3} + 2 * l_{2} * l_{3} * (\cos\theta_{2} * \cos(\theta_{2} + \theta_{3}) + \sin\theta_{2} * \sin(\theta_{2} + \theta_{3})) \\
x^{2} + y^{2} + (z - l_{1})^{2} = l^{2}_{2} + l^{2}_{3} + 2 * l_{2} * l_{3} * (\cos\theta_{2} * (\cos\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \sin\theta_{3}) ) + sin\theta_{2} * (sin\theta_{2} * \cos\theta_{3} + \cos\theta_{2} * \sin\theta_{3} ) ) \\
x^{2} + y^{2} + (z - l_{1})^{2} = l^{2}_{2} + l^{2}_{3} + 2 * l_{2} * l_{3} * (\cos^{2}\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \cos\theta_{2} * \sin\theta_{3}) + sin^{2}\theta_{2} * \cos\theta_{3} + \sin\theta_{2} * \cos\theta_{2} * \sin\theta_{3} ) \\
x^{2} + y^{2} + (z - l_{1})^{2} = l^{2}_{2} + l^{2}_{3} + 2 * l_{2} * l_{3} * (\cos^{2}\theta_{2} + sin^{2}\theta_{2}) * \cos\theta_{3} \\
x^{2} + y^{2} + (z - l_{1})^{2} = l^{2}_{2} + l^{2}_{3} + 2 * l_{2} * l_{3} * \cos\theta_{3} \\
2 * l_{2} * l_{3} * \cos\theta_{3} = (x^{2} + y^{2} + (z - l_{1})^{2}) - (l^{2}_{2} + l^{2}_{3}) \\
\cos\theta_{3} = ( (x^{2} + y^{2} + (z - l_{1})^{2}) - (l^{2}_{2} + l^{2}_{3}) ) / (2 * l_{2} * l_{3}) \\
\theta_{3} = \arccos( (x^{2} + y^{2} + (z - l_{1})^{2}) - (l^{2}_{2} + l^{2}_{3}) ) / (2 * l_{2} * l_{3}) \\
}
上記計算により，$\theta_{3}$が算出される．しかし，算出されるnumpy.arccos()で取得できる角度は$0$から$\pi$の範囲である．そのため，$-\pi$から$0$の範囲は算出できない．
今回は，$-\pi$から$\pi$の範囲の角度が欲しいため，np.arctan2()を採用する．
np.arctan2()の引数には，$\sin\theta$と$\cos\theta$の情報が必要である．
$\cos\theta_{3}$は(3)で算出されているため，(2)より以下のように$\sin\theta_{3}$と$\theta_{3}$を算出する．
\displaylines{
\sin\theta_{3} = \sqrt(1 - \cos^{2}\theta_{3}) \\
\theta_{3} = arctan2(\sin\theta_{3}, \cos\theta_{3}) \\
}
ここで，$\theta_{3}$は下図のような2通りの解が得られる．
$+\theta_{3}$は上向きで，$-\theta_{3}$は下向きの角度となる．
要するに逆運動学を解くと，2つの解が得られる．上向きの解か下向きの解かを選択する必要がある．
theta1 の算出方法
$\theta_{1}$の算出方法を説明する．
ロボットをz軸から見ると，下図のようになる．
$\theta_{1}$はロボットの手先位置$P$の$(x, y)$で以下の通りに，算出することができる．
\displaylines{
\theta_{1} = arctan2(y, x) \\
\theta_{1} = arctan2(-y, -x) \\
}
ロボット正面に手先位置が存在するときは$\theta_{1} = arctan2(y, x)$となり，ロボット背面に手先位置が存在するときは$\theta_{1} = arctan2(-y, -x)$となる．
ロボット背面に位置が存在する時は，ロボットの座標系から180度回転するから，$arctan2(-y, -x)$を使用する．
ただし，$(x, y) = (0, 0)$では，解が不定となることに注意すること．
($(x, y) = (0, 0)$では\theta_{1}にどのような値も入れることが可能となる)
theta2 の算出方法
$\theta_{2}$の算出方法を説明する．
(1)：$x$，$y$を二乗して足し合わせる．
\displaylines{
x^{2} = \cos^{2}\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} \\
y^{2} = \sin^{2}\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2} \\
x^{2} + y^{2} = (\cos^{2}\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2}) + (\sin^{2}\theta_{1} * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2}) \\
x^{2} + y^{2} = (\cos^{2}\theta_{1} + \sin^{2}\theta_{1}) * (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2}) \\
x^{2} + y^{2} = (l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) )^{2}) \\
}
(2)：(1)の両辺の平方根を算出する．
\displaylines{
+-\sqrt(x^{2} + y^{2}) = l_{2} * \cos\theta_{2} + l_{3} * \cos(\theta_{2} + \theta_{3}) \\
}
(3)：(2)と$z$を行列形式として，$\cos(\theta_{2})と\sin(\theta_{2})$を算出し，$\theta_{2}$を求める．
\displaylines{
\begin{pmatrix}
l_{2} * \cos\theta_{2} + l_{3} * (\cos\theta_{2} * \cos\theta_{3} - \sin\theta_{2} * \sin\theta_{3} ) \\
l_{2} * \sin\theta_{2} + l_{3} * (\sin\theta_{2} * \cos\theta_{3} + \cos\theta_{2} * \sin\theta_{3})
\end{pmatrix}
=
\begin{pmatrix}
+-\sqrt(x^{2} + y^{2}) \\
z - l_{1} \\
\end{pmatrix} \\
\begin{pmatrix}
l_{2} + l_{3} * \cos\theta_{3} & -l_{3} * \sin\theta_{3} \\
l_{3} * \sin\theta_{3} & l_{2} + l_{3} * \cos\theta_{3} \\
\end{pmatrix}
\begin{pmatrix}
\cos\theta_{2} \\
\sin\theta_{2} \\
\end{pmatrix}
=
\begin{pmatrix}
+-\sqrt(x^{2} + y^{2}) \\
z - l_{1} \\
\end{pmatrix} \\
\begin{pmatrix}
\cos\theta_{2} \\
\sin\theta_{2} \\
\end{pmatrix}
=
\begin{pmatrix}
l_{2} + l_{3} * \cos\theta_{3} & -l_{3} * \sin\theta_{3} \\
l_{3} * \sin\theta_{3} & l_{2} + l_{3} * \cos\theta_{3} \\
\end{pmatrix} ^{-1}
\begin{pmatrix}
+-\sqrt(x^{2} + y^{2}) \\
z - l_{1} \\
\end{pmatrix} \\
\theta_{2} = \arctan2(\sin\theta_{2}, \cos\theta_{2}) \\
}
上記では，逆行列を使用している．逆行列に関しては，下記が参考になります．
(https://qiita.com/nognog/items/efae10aceef1ac9d1d09)
これにより，逆運動学によって手先位置から関節角度を算出することができる．
逆運動学のソースコード
定数を定義するファイル (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軸周りに回転
3軸ロボットアームを定義するファイル (robot.py)
3軸ロボットアームを定義するファイルを下記に記す．
robot.py
# ロボットアームの運動学を記載
# ライブラリの読み込み
import numpy as np
# サードパーティーの読み込み
# 自作モジュールの読み込み
from constant import * # 定数
from rotation import Rotation # 回転行列
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プロパティのゲッター
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_NONE # 手先位置の次元数
_DIMENTION_THETA = DIMENTION_NONE # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_NONE # リンク数
_DIMENTION_AXIS = DIMENTION_NONE # 回転軸数
_INITIAL_THETA = 0.0 # 初期回転角度 [rad]
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 = Rotation()
self._objects = []
self._manager = None
@property
def links(self):
"""
_linksプロパティのゲッター
"""
return self._links
@property
def manager(self):
"""
_managerプロパティのゲッター
"""
return self._manager
def forward_kinematics(self, thetas):
"""
順運動学 (ロボットの関節角度からロボットの手先位置を算出)
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
"""
raise NotImplementedError("forward_kinematics() is necessary override.")
def inverse_kinematics(self, pose):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
pose(numpy.ndarray): ロボットの手先位置 (位置 + 姿勢) [m] + [rad]
戻り値
thetas(numpy.ndarray): ロボットの関節角度 [rad]
"""
raise NotImplementedError("inverse_kinematics() is necessary override.")
def forward_kinematics_all_pos(self, thetas):
"""
順運動学で全リンクの位置を取得
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
戻り値
all_pose(numpy.ndarray): ロボットの全リンク位置 (位置 + 姿勢) [m] + [rad]
"""
raise NotImplementedError("forward_kinematics() is necessary override.")
def update(self, thetas):
"""
角度を与えて，各リンクの直方体を更新する
パラメータ
thetas(numpy.ndarray): ロボットの関節角度 [rad]
"""
raise NotImplementedError("update() is necessary override.")
class Robot3DoF(Robot):
"""
3軸ロボットクラス
プロパティ
_links(numpy.ndarray): ロボットのリンク長
_rot(Rotation): 回転行列クラス
_axiss(list): 関節の回転軸
メソッド
public
forward_kinematics(): 順運動学 (ロボットの関節角度からロボットの手先位置を算出)
inverse_kinematics(): 逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
forward_kinematics_all_pos(): 順運動学で全リンクの位置を取得
update(): 角度を与えて，各リンクの直方体を更新する
"""
# 定数の定義
_DIMENTION_POSE = DIMENTION_3D # 手先位置の次元数
_DIMENTION_THETA = DIMENTION_3D # 関節角度の次元数
_DIMENTION_LINK = DIMENTION_3D # リンク数
_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)
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)}")
# あらかじめ三角関数を算出する
sin1 = np.sin(thetas[0])
sin2 = np.sin(thetas[1])
sin23 = np.sin(thetas[1] + thetas[2])
cos1 = np.cos(thetas[0])
cos2 = np.cos(thetas[1])
cos23 = np.cos(thetas[1] + thetas[2])
# ロボットの手先位置を算出
pxy_common = self._links[1] * cos2 + self._links[2] * cos23
px = cos1 * pxy_common
py = sin1 * pxy_common
pz = self._links[0] + self._links[1] * sin2 + self._links[2] * sin23
pose = np.array([px, py, pz])
return pose
def inverse_kinematics(self, pose, upper=False, front=True):
"""
逆運動学 (ロボットの手先位置からロボットの関節角度を算出)
パラメータ
pose(numpy.ndarray): ロボットの手先位置 (位置) [m]
upper(bool): 腕が上向かどうか
front(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)}")
# はじめにtheta3を算出する
# c3 = {(px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l3 ** 2)} / (2 * l2 * l3)
px = pose[0]
py = pose[1]
pz = pose[2]
l1 = self._links[0]
l2 = self._links[1]
l3 = self._links[2]
cos3 = ((px ** 2 + py ** 2 + (pz - l1) ** 2) - (l2 ** 2 + l3 ** 2)) / (2 * l2 * l3)
# cosの範囲は-1以上1以下である
if abs(cos3) > 1:
# 異常
raise ValueError(f"cos3 is abnormal. cos3 is {cos3}")
# sinも求めて，theta3をatan2(sin2, cos2)より算出する
sin3 = np.sqrt(1 - cos3 ** 2)
theta3 = np.arctan2(sin3, cos3)
if not upper:
# 下向きの角度のため，三角関数も更新
theta3 = -theta3
sin3 = np.sin(theta3)
cos3 = np.cos(theta3)
# 次にtheta1を算出する
# theta1 = atan2(py, px)
# py, pxが0近傍なら失敗
if abs(py) <= self._THETA1_XY_THRESHOLD and abs(px) <= self._THETA1_XY_THRESHOLD:
# 近傍のため，エラー
raise ValueError(f"abs(py) and abs(px) is abnormal. py is {py}, px is {px}")
if front:
# 位置(pose)に対して正面向き
theta1 = np.arctan2( py, px)
else:
# 位置(pose)に対して後ろ向き
theta1 = np.arctan2(-py, -px)
# 最後にtheta2を算出する
# 行列計算
# [c2, s2] = [[l2 + l3 * c3, -l3 * s3], [l3 * s3, l2 + l3 * c3]] ** -1 * [root(px ** 2 + py ** 2), pz - l1]
element1 = l2 + l3 * cos3
element2 = -l3 * 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])
# [c2, s2]の計算
cos2_sin2 = np.dot(np.linalg.inv(matrix), position)
# theta2をatan2()より算出する
theta2 = np.arctan2(cos2_sin2[1], cos2_sin2[0])
thetas = np.array([theta1, theta2, theta3])
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)}")
# 回転角度とリンク長をローカル変数に保存
theta1 = thetas[0]
theta2 = thetas[1]
theta3 = thetas[2]
link1 = self._links[0]
link2 = self._links[1]
link3 = self._links[2]
# 三角関数を計算
s1 = np.sin(theta1)
c1 = np.cos(theta1)
s2 = np.sin(theta2)
c2 = np.cos(theta2)
s3 = np.sin(theta3)
c3 = np.cos(theta3)
s12 = np.sin(theta1 + theta2)
c12 = np.cos(theta1 + theta2)
s23 = np.sin(theta2 + theta3)
c23 = np.cos(theta2 + theta3)
# 各リンクの位置を算出
base_pos = np.zeros(self._DIMENTION_POSE)
link1_pos = np.array([0, 0, link1])
link2_xy_common = link2 * c2
link2_pos = np.array([c1 * link2_xy_common, s1 * link2_xy_common, link1 + link2 * s2])
link3_xy_common = link2_xy_common + link3 * c23
link3_pos = np.array([c1 * link3_xy_common, s1 * link3_xy_common, link1 + link2 * s2 + link3 * s23])
# 全リンクの位置を算出
all_link_pose = np.array([base_pos, link1_pos, link2_pos, link3_pos])
return all_link_pose
メイン(プロットなど)処理 (main.py)
メイン処理ファイルを下記に記す．
main.py
# メイン処理
# ライブラリの読み込み
import numpy as np # 数値計算
import matplotlib.pyplot as plt # 描画
# 自作モジュールの読み込み
from constant import * # 定数
from robot import Robot3DoF # ロボットクラス
FORWARD_KINE = False # 順運動学の計算フラグ (True / False = 順運動学 / 逆運動学)
LINE_WIDTH = 3 # プロット時の線の太さ
GRAPH_FORWARD_FILE_NAME = "plot_3drob_forward_kinematics.png"
GRAPH_INVERSE_FILE_NAME = "plot_3drob_inverse_kinematics.png"
def forward(robot, thetas, axis, plot_flg=True, label=True):
"""
順運動学
パラメータ
robot(Robot3DoF): ロボットクラス
thetas(numpy.ndarray): 関節角度 [rad]
axis(figure.add_subplot): 3次元の描画軸
plot_flg(bool): 描画の有無 (True / False = 描画する / しない)
label(bool): 描画時のラベルの有無
戻り値
pose(numpy.ndarray): 手先位置
"""
# 順運動学により，位置を算出
pose = robot.forward_kinematics(thetas)
if plot_flg:
plot(robot, thetas, axis, label=label)
else:
print(f"forward theta = {thetas}")
print(f"forward pose = {pose}")
pass
return pose
def inverse(robot, pose, axis, plot_flg=True, upper=True, front=True, label=True):
"""
逆運動学
パラメータ
robot(Robot3DoF): ロボットクラス
pose(numpy.ndarray): 手先位置 [m]
axis(figure.add_subplot): 3次元の描画軸
plot_flg(bool): 描画の有無 (True / False = 描画する / しない)
upper(bool): 関節が上向きとするかどうか (True / False = Yes / No)
front(bool): 正面向きとするかどうか (True / False = Yes / No)
label(bool): 描画時のラベルの有無
戻り値
thetas(list): 関節角度
"""
# 順運動学により，位置を算出
thetas = robot.inverse_kinematics(pose, upper=upper, front=front)
if plot_flg:
plot(robot, thetas, axis)
else:
print(f"inverse pose = {pose}")
print(f"inverse theta = {thetas}")
pass
return thetas
def main():
"""
メイン処理
"""
# 3軸ロボットのリンク長
links = np.array([1.0, 1.0, 1.0])
# 3軸ロボットのインスタンスを作成
robot = Robot3DoF(links)
# プロット有無
plot_flg = True
# 逆運動学の上向き解とするかの有無
uppers = [True, False]
# 順運動学で使用する関節角度 [deg] を定義
deg_thetas = [0, 150]
candidate = np.deg2rad(deg_thetas)
all_thetas = np.array([[theta1, theta2, theta3] for theta1 in candidate for theta2 in candidate for theta3 in candidate])
# 3次元グラフの作成
fig = plt.figure()
axis = fig.add_subplot(111, projection="3d")
if FORWARD_KINE:
# 順運動学
file_name = GRAPH_FORWARD_FILE_NAME
for thetas in all_thetas:
pose = forward(robot, thetas, axis, plot_flg=plot_flg)
else:
# 逆運動学
file_name = GRAPH_INVERSE_FILE_NAME
for thetas in all_thetas:
for upper in uppers:
# 順運動学で位置を取得
pose1 = forward(robot, thetas, axis, plot_flg=False)
# 取得した位置で逆運動学
theta = inverse(robot, pose1, axis, plot_flg=plot_flg, upper=upper)
# 逆運動学の結果が正しいかを順運動学で計算
pose2 = forward(robot, theta, axis, plot_flg=False)
print()
print()
print()
if plot_flg:
show(axis, file_name)
def show(axis, graph_file_name):
"""
描画する
パラメータ
axis(figure.add_subplot): 3次元の描画軸
graph_file_name(str): 描画ファイル名
"""
# X軸，Y軸，Z軸の名称
axis.set_xlabel("X [m]")
axis.set_ylabel("Y [m]")
axis.set_zlabel("Z [m]")
axis.grid()
axis.set_aspect("equal")
plt.legend(fontsize=5, loc='center left', bbox_to_anchor=(0.8, .5))
# # 凡例を見切れないようにするために，plt.tight_layout()
plt.tight_layout()
plt.savefig(graph_file_name)
plt.show()
def plot(robot, thetas, axis, label=True):
"""
ロボットをプロット (将来的にはクラス化する)
パラメータ
robot(Robot3DoF): ロボット
thetas(numpy.ndarray): 回転角度
axis(figure.add_subplot): 3次元の描画軸
label(bool): 描画時のラベルの有無
"""
# 全リンクの位置を算出
all_link_pos = robot.forward_kinematics_all_link_pos(thetas)
# 線プロット
if label:
axis.plot(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], linewidth=LINE_WIDTH, label=f"theta1={np.rad2deg(thetas[0]):.1f} [deg], theta2={np.rad2deg(thetas[1]):.1f} [deg], theta3={np.rad2deg(thetas[2]):.1f} [deg]")
else:
axis.plot(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2], linewidth=LINE_WIDTH)
# 点プロット
axis.scatter(all_link_pos[:, 0], all_link_pos[:, 1], all_link_pos[:, 2])
if __name__ == "__main__":
# 本ファイルがメインで呼ばれた時の処理
main()
処理結果
main.pyを実行し，順運動学と逆運動学のグラフを下図に記す．
下図は順運動学のグラフである．
下図は逆運動学のグラフである．
右に判例を記載してある．グラフを見る限り，逆運動学の処理は正常であることがわかる．
おわりに
本記事では，Pythonを使用して，下記内容を実装しました．
・3軸ロボットアームの逆運動学 (逆運動学 ... ロボットの手先位置からロボットの関節角度を算出する)
次記事では，下記内容を実装していきます．
・干渉物が存在する環境下で，3軸ロボットアームの経路生成