【C言語】2リンクマニピュレータの順運動学

スポンサーリンク

C言語を用いて、2リンクマニピュレータの順運動学を求めるプログラムを紹介します。

スポンサーリンク

順運動学の計算式 (2リンクマニピュレータ)

2リンクマニピュレータとは、2自由度のロボットアームのことです。
順運動学は、関節角度から手先位置を求めることです。
2リンクマニピュレータの順運動学の計算式は次のようになります。

(1) \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は第1関節と第2関節のリンク角度、l_1, l_2は第1リンクと第2リンクの長さです。

【参考文献】
2リンクマニピュレータの順運動学

今回はこれをC言語で計算してみました。

ソースコード

プログラムのソースコードは下記の通りです。

実行結果

プログラムの実行結果は下記の通りです。

(x, y)=(1.208046, 1.439693)

リンク1の長さ0.5、リンク2の長さ0.6、関節1の角度30[deg]、関節2の角度40[deg]としたときの
手先位置を求めています。

【関連記事】
C言語でロボット制御プログラミング
C言語入門 (サンプルプログラム集)

スポンサーリンク

シェア&フォローお願いします!