【Processing】InverseKinematicsで6軸ロボットアームを動かす

成果物

www.youtube.com
今回は、前回作ったロボットアームをInverseKinematicsで動かしてみました。6軸あるので、目標の座標と向きのとうりにアームを動かすことができます。

コード

今回は、行列の計算を書き起こしているので長くなっています。ロボットアームの表示についてはこちらをご覧ください。
nekodigi.hatenablog.com

仕組み

今回は、こちらのサイトを参考にしました。
meuse.co.jp
理解するのに苦戦したので、そこについて説明します。
$\begin{align*} P_{5x} = P_{x} - (l_{5} + l_{6})a_{x} \\ P_{5y} = P_{y} - (l_{5} + l_{6})a_{y} \\ P_{5z} = P_{z} - (l_{5} + l_{6})a_{z} \end{align*}$
まず、こちらの式で、P5の位置を求めます。
その後、こちらの式で0番関節をP5に向けます。
$\begin{align*} \theta_{1} = atan2(P_{5y}, P_{5x}) \quad or \quad atan2(P_{5y}, P_{5x}) + \pi \end{align*}$
今度は正弦定理を使って、2番関節の角度を求めます。その前に以下の式を使って。図でいうところのADの長さを求めます。
f:id:Nekodigi:20200122192652p:plain
$\begin{align*} &(P_{5x})^2 + (P_{5y})^2 + (P_{5z} - (l_{0} + l_{1}))^2 \\ &= (S_{2}l_{2} + S_{23}(l_{3} + l_{4}))^2 + (C_{2}l_{2} + C_{23}(l_{3} + l_{4}))^2\\ &= l_{2}^2 + (l_{3} + l_{4})^2 + 2l_{2}(l_{3} + l_{4})(S_{2}S_{23} + C_{2}C_{23})\\ &= l_{2}^2 + (l_{3} + l_{4})^2 + 2l_{2}(l_{3} + l_{4})C_{3} \end{align*}$
これで、三辺の長さがわかったのでそれをもとにcos(2番関節の角度)を求めます。
$\begin{align*} C_{3} = \frac{1}{2l_{2}(l_{3} + l_{4})}(P_{5x}^2 + P_{5y}^2 + (P_{5z} - (l_{0} + l_{1}))^2 - l_{2}^2 - (l_{3} + l_{4})^2) \end{align*}$
参考にしたサイトでは謎のステップが踏まれていますが、シンプルにacos関数を使えば2番関数の角度が求められました。
続いて1番関節の角度を求めるのですが、こちらは少し複雑で、行列を使って解かれています。ただ、どこがA,B,M,Nなのかは図を使うことで少しわかりやすくなるかと思います。
わかりにくいですが、1番関節の角度と同じ角度の場所があり、そのことを利用して1番関節の角度を求めているようです。
f:id:Nekodigi:20200122193822p:plain