順運動学

2リンクマニピュレータの運動学を計算してみます。
今回が順運動学で、次回以降が逆運動学です。

順運動学は「関節の角度」から「手先の位置」を求めることでした。
逆運動学は、その逆です。

この図を頭に浮かべながら、考えていきます。

(x,y)を求めるのが順運動学です。
順運動学は簡単。

まず、マニピュレータの根元を原点として、1つめのリンクの先の座標(u,v)を求めます。

これは三角関数の定義のまんまなので瞬殺。
同じような手順で手先の座標を求めます。

ポイントは、角度が手前との和になることです。
それから2つめのリンクの根元は、原点でなく1つめのリンクの先であることに注意。

結局、まとめるとこういうこと。
リンクが3つになっても4つになっても、順番に計算していくだけ。
簡単です。