前回までは2Dアームでしたが、今回は3Dアームにおける逆運動学です。
環境：Python3.8.5、Jupyter Notebook
これまでの2Dアーム逆運動学：
- Inverse Kinematics 逆運動学：Backward Shift、FABRIK、CCD
- Jacobian Inverse Kinematics :ヤコビ行列を用いた逆運動学（その１）
- IK（逆運動学）：同次変換行列、クロス積によるヤコビ行列（その２）
- IK（逆運動学）：アーム可動域制限（角度制限）CCDとFABRIKの場合
- IK（逆運動学）：同次変換行列/ヤコビ行列(クロス積)/角度制限
- IK（逆運動学）：複数の円の交点から求める
今回の特長：
- 3Dアーム逆運動学
- 同次変換行列
- ヤコビ行列（クロス積）
- 角度制限
- インタラクティブ操作
上図：
- 左上：3D表示
- 右上：真上からの視点（X-Y平面）
- 右下：正面からの視点（X-Z平面）
- 赤い×は目標座標
アームの初期設定は、
- Joint0[0,0,0]はZ軸を中心にLink0（Joint1）を回転させる
- Joint1[0,0,0.5]はY軸を中心にLink1（Joint2）を回転させる
- Joint2[1,0,0]はY軸を中心にLink2（Joint3）を回転させる
- Joint3[1,0,0]はY軸を中心にLink3（End-Effector）を回転させる
要するに、Link0のZ軸を中心した回転によってLink1〜3はX-Z平面上を回転し、アーム先端（EE：End-Effector）は目標値Targetに近似していきます。
同次変換行列：
3Dなので、同次変換行列は以下のように３種類用意しました。引数1の'X'、'Y'、'Z'によって、その軸回りでの回転になります（今回の4リンクアームの例では、'Y'、'Z'だけ使用）。引数2は平行移動ベクトル[x,y,z]、引数3は回転角。
def H(axis, vec, theta):
if axis == 'X':
return np.array([[1, 0, 0, vec[0]],
[0, cos(theta), -sin(theta), vec[1]],
[0, sin(theta), cos(theta), vec[2]],
[0, 0, 0, 1]])
elif axis == 'Y':
return np.array([[cos(theta), 0, -sin(theta), vec[0]],
[ 0, 1, 0, vec[1]],
[sin(theta), 0, cos(theta), vec[2]],
[ 0, 0, 0, 1]])
elif axis == 'Z':
return np.array([[cos(theta), -sin(theta), 0, vec[0]],
[sin(theta), cos(theta), 0, vec[1]],
[ 0, 0, 1, vec[2]],
[ 0, 0, 0, 1]])
else:
return np.array([[1, 0, 0, vec[0]],
[0, 1, 0, vec[0]],
[0, 0, 1, vec[0]],
[0, 0, 0, 1]])
4つ目の行列は単位行列を返します（今回は不使用）。
FK（運動学）：
FKにおいては、上記の同次変換行列を順次掛け合わせることで各ジョイントとEnd-Effectorのベクトルを計算しています。Tは角度変換行列、Vで変換後の各ジョイントのベクトルをその都度取得しnp.arrayに格納。
def FK(L, TH):
N = len(L)
T = H('Z', L[0], TH[0])
V = np.array(T[:3,-1])
for i in range(1, N-1):
T = T @ H('Y', L[i], TH[i])
V = np.c_[V, T[:3,-1]]
EE = T @ np.array([[1,0,0,1]]).T
V = np.c_[V, EE[:3, -1]]
return V
今回の場合は、
T = H('Z', [0,0,0], 0) @ H('X', [0,0,0.5], pi/2) @ H('X', [1,0,0], 0) @ H('X', [1,0,0], 0) @ EE
という順番で掛け合わせています（@はドット積）。
最後のEEはEnd-Effector=np.array([1,0,0,1]).T
IK（逆運動学）とヤコビ行列：
今回のヤコビ行列はクロス積で求めています（クロス積によるヤコビ行列についてはこちらを参照）。
np.cross(回転軸ベクトル, 各ジョイントベクトル)
回転軸がZ軸の場合は[0,0,1]、Y軸の場合は[0,-1,0]で反転させています。
角度制限：
以前の方法と同様、各ジョイントにおいて下限角度と上限角度を設定しておき、角度更新後にその角度を-180〜180度の範囲に変換してから制限値範囲外の場合は抑制し再更新します。
def angleLimit(TH, MinA, MaxA):
THCOPY = TH.copy()
for i in range(len(TH)):
THCOPY[i] = TH[i] % tau
if THCOPY[i] > pi:
THCOPY[i] -= tau
if THCOPY[i] < MinA[i]:
THCOPY[i] = MinA[i]
if THCOPY[i] > MaxA[i]:
THCOPY[i] = MaxA[i]
return THCOPY
インタラクティブモード：
右２つのグラフ（X-Y平面上かX-Z平面上の任意の座標）をマウスクリックすると、アーム先端（End-Effector）がその座標に移動します（まだ多少バグがあるかも）。左側3D表示内の座標をクリックすることはできませんが、視点の向きを変えることができます。
どのグラフ（X-Y平面上かX-Z平面上）をクリックしたかは、event.inaxesで判定しています。
def click(event):
global TH, mx, my, mz
if event.inaxes == A10.axes:
mx = event.xdata
my = event.ydata
elif event.inaxes == A20.axes:
mx = event.xdata
mz = event.ydata
else:
pass
# 以下省略
コード：
