ひとつ前の記事から続いています。
逆運動学は、
どんな命令でうごかしたいのかで臨機応変にいろんなことを考えればいいものですが、
今回は
中央の車輪あたりにあるy-z平面を使い、3ブロックあるものを真ん中で2つに分けていこうと思います。
クネクネする動作：
前の奴を右に
後ろの奴を左に
みたいな指示を出したいと思えば、中央のブロックからそれぞれの位置姿勢を命令したいということになります。
なので、
端の車輪の中心を、y-z平面に射影したものを見ていきます。
右に振りたければzを増やす、
上にあげたければyを増やすという命令をすればいいことになります。
もちろん「上」とか「右」とかっていう変数を置いたプログラムを作る方が便利かもしれません。
さて、順運動学をもう一回やります。
まんなかから考えるので、
同次変換行列は
この3つです。
先端までまとめると
私が命令したいのはy座標とz座標です。
最右列の上から2番目と3番目以外に興味はありません。
sin(theta4)*d3=y
と
-sin(theta3)*cos(theta4)*d3=z
という式です。
変数は、zとyとtheta3とtheta4です。
zとyには、どのくらい上にあげたいのか、右に曲げたいのか
ということに合わせて好きな数字を入れればいいと思います。
それでtheta3とtheta４はどうなるのかっていうところが気になります。
で、どうするのかっていうのは人の好きなのですが
theta4に関しては、変数1つの非線形方程式なのでいろんな方法が簡単に出来ます。
①arcsinをつかう
②数値計算で収束をまつ
③サインと角度のデータテーブルからそれっぽいのを選ぶ
ま、あ、すきな方法でやります。
①でやります。
何にも考えずに①でやります。
theta4 = arcsin(y/d3) です。
なぜなら、theta4の可動範囲は±90°だからです。
arcsinも同じ範囲をサポートしています。
ではtheta3をやります。
theta4が計算してからtheta3を計算すれば
cos(theta4)は定数です。
で、再びarcsinをやります。
theta3 = -arcsin(z/d3cos(theta4)) です。
theta3とtheta4が決まりました。
●theta3 = -arcsin(z/d3cos(theta4))
●theta4 = arcsin(y/d3)
この角度をサーボモータに指示すればいいわけですね。
[0回]
