C言語を用いて、2リンクマニピュレータの順運動学を求めるプログラムを紹介します。
[latexpage]
## 順運動学の計算式 (2リンクマニピュレータ)
2リンクマニピュレータとは、2自由度のロボットアームのことです。
順運動学は、関節角度から手先位置を求めることです。
2リンクマニピュレータの順運動学の計算式は次のようになります。
\begin{eqnarray}
x=l_1cos\theta_1 + l_2cos(\theta_1+\theta_2)\
y=l_1sin\theta_1 + l_2sin(\theta_1+\theta_2)
\end{eqnarray}
ここで、$x,y$は手先位置、 $\theta_1, \theta_2$は第１関節と第２関節のリンク角度、$l_1, l_2$は第1リンクと第２リンクの長さです。
【参考文献】
・2リンクマニピュレータの順運動学
今回はこれをC言語で計算してみました。
## ソースコード
プログラムのソースコードは下記の通りです。
## 実行結果
プログラムの実行結果は下記の通りです。
(x, y)=(1.208046, 1.439693)
リンク1の長さ0.5、リンク２の長さ0.6、関節１の角度30[deg]、関節2の角度40[deg]としたときの
手先位置を求めています。
【関連記事】
・C言語でロボット制御プログラミング
・C言語入門 (サンプルプログラム集)
コメント