#はじめに
「初めて学ぶ基礎ロボット工学」小川鑛一, 加藤了三共著（東京電機大学出版局, 1998.1）を図書館で借りてきて読んでいたら、偶然にもMS3Lと同じ平面３リンクマニピュレータの関節座標$(\phi_1,\phi_2,\phi_3)$の解を求める式が説明されていたので、pythonでも計算してみました。
####以下、計算方法の説明を同書から一部引用します。
「マニピュレータの手先の位置$(x,y)$および姿勢角 $\alpha$と関節角度$\phi_1,\phi_2,\phi_3$の関係は、幾何学的な条件を考えると，
\begin{eqnarray} x &=& L_1\cos(\phi_1)+L_2\cos(\phi_1+\phi_2)+L_3\cos(\phi_1+\phi_2+\phi_3) \\
y &=& L_1\sin(\phi_1)+L_2\sin(\phi_1+\phi_2)+L_3\sin(\phi_1+\phi_2+\phi_3) \\ \alpha &=& \phi_1+\phi_2+\phi_3 \end{eqnarray}
となる。」「例えば，$\ l_1,l_2,l_3=1$ であるとすると，関節座標が$(\phi_1,\phi_2,\phi_3)=(30^\circ,60^\circ,-60^\circ)$の時，手先座標$(x,y,\alpha)^T$は，$(\sqrt{3},2,30^\circ)^T$と求まり，これで順運動問題が解けたことになる。」「一方、逆運動学問題は、（略）$(x,y,\alpha)^T$が与えられたとすると、式(1.1)，(1.2)，(1.3)の右辺第３項はいずれも既知である。そこで，これらの式の右辺第１項と第３項を左辺に移行して両辺を２乗し，辺々を足し合わせると，
\begin{eqnarray}
A^2+B^2+l_1^2-2l_1(Acos\phi_1+Bcos\phi_1)=l_2^2
\end{eqnarray}
ここに，
\begin{eqnarray}
A=x-l_3\cos\alpha \hspace{10pt}, \hspace{10pt}B=y-l_3\sin\alpha
\end{eqnarray}
となる。加法定理により，左辺第４項は，
\begin{eqnarray}
A\cos\phi_1+B\sin\phi_1=\sqrt{A^2+B^2}\cos(\phi_1-\gamma)\hspace{5pt}, \hspace{5pt}\gamma=\tan^{-1}(B/A)
\end{eqnarray}
と変形できるので，
\begin{eqnarray}
\phi_1=\gamma\pm\cos^{-1} \frac{A^2+B^2+l_1^1+l_2^2}{2l_1\sqrt{A^2+B^2}}
\end{eqnarray}
となる。[<---ここでの式の展開がまだ理解できておりません(汗)]」「$\phi_1$が求まると，式(1.1)，(1.2)の右辺第１項，第３項は既知となるので，式(1.1)，(1.2)より，
\begin{eqnarray}
\tan(\phi_1+\phi_2)=\frac{B-l_1\sin\phi_1}{A-l_1\cos\phi_1}
\end{eqnarray}
となり，
\begin{eqnarray}
\phi_2=-\phi_1+\tan^{-1}\frac{B-l_1\sin\phi_1}{A-l_1\cos\phi_1}
\end{eqnarray}
により$\phi_2$が求まる。そして，以上で求めた$\phi_1$と$\phi_2$と式(1.3)により，$\phi_3$が求まり，関節変数$\phi_1,\phi_2,\phi_3$のすべてが求まる。」（同書pp.123-126)
###MS3L
狙いは、こんな感じのデバイスを操作します。
###pythonで書いたコード（ROS用）
サンプル用の手先座標のデータは、同書に記載されているもの(p.126)と同じです。pythonのコードとしてはあまり工夫はありませんが、$\gamma$値と$\phi_2$値のアークタンジェントの計算にはmathのatan2関数を使用しています。
#!/usr/bin/env python
import rospy
import math as mt
from geometry_msgs.msg import Twist
# calc test data set
#----------------------------------
# point/position data set
x = mt.sqrt(3)
y = 2
alpha = mt.radians(30)
#----------------------------------
# arm lengths set
l = [1, 1, 1]
#----------------------------------
theta0 = [0] * 3
theta1 = [0] * 3
A = x - l[2] * mt.cos(alpha)
B = y - l[2] * mt.sin(alpha)
C = (A ** 2 + B ** 2 + l[0] ** 2 - l[1] ** 2) / (2 * l[0])
gamma = mt.atan2(B, A)
theta0[0] = gamma + mt.acos(C / mt.sqrt(A ** 2 + B ** 2))
theta0[1] = mt.atan2(B - l[0] * mt.sin(theta0[0]), A - l[0] * mt.cos(theta0[0])) - theta0[0]
theta0[2] = alpha - theta0[0] - theta0[1]
theta1[0] = gamma - mt.acos(C / mt.sqrt(A ** 2 + B ** 2))
theta1[1] = mt.atan2(B - l[0] * mt.sin(theta1[0]), A - l[0] * mt.cos(theta1[0])) - theta1[0]
theta1[2] = alpha - theta1[0] - theta1[1]
#ROS topic set
planar_3_link = Twist()
planar_3_link.linear.x = x
planar_3_link.linear.y = y
planar_3_link.linear.z = mt.degrees(alpha)
planar_3_link.angular.x = theta0[0]
planar_3_link.angular.y = theta0[1]
planar_3_link.angular.z = theta0[2]
#debug purpose only
print mt.degrees(theta0[0]), mt.degrees(theta0[1]), mt.degrees(theta0[2])
print mt.degrees(theta1[0]), mt.degrees(theta1[1]), mt.degrees(theta1[2])
print planar_3_link
最後のデータ・セット部分は、ROSのメッセージ型に合わせてトピックを出力できるようにしているだけなので、計算とは関係ありません。
###実行結果
$ python planar_3_links.py
90.0 -60.0 0.0
30.0 60.0 -60.0
linear:
x: 1.73205080757
y: 2
z: 30.0
angular:
x: 1.57079632679
y: -1.0471975512
z: 0.0
#最後に
pythonだと、本当に文字通り、計算式そのままのようなプログラムが書けるのは本当に楽で良いと思います。サーボモータの動作速度と実際にROSを動かすプラットフォームの処理能力により計算/動作速度には不安はありますが、もうしばらくはPythonで実験して、本当に必要になったらC++に移行しようと思います。