足郎2シミュレーターの構成

足郎2のシミュレーター作りが面倒くさい。角度と位置の関係をどのように表すかがわかりにくい。先に書いたものは複雑すぎてわかりにくい。ある論文を参考に、座標を単純にしたことによってグラフィカルに描くことができるようになった。
まず、関節角度をランダムに少しずつ動かした動画は次のようになる(クリックすると再生するはず。マックしかダメか?)。

ここまでできれば、サーボモータの動きと連動できる。基本的には、COSM言語で書かれたプログラムをシミュレートできるようにしたい。
まず、理論的な構成を記録しておこう。上記の動きが実現しているのは、以下の理論、数式が基本的に正しかったことを示している。その検証のためのものだ。
左下に描かれているように、y軸とz軸のみの表現である。
複雑そうだが、数学的には極めて単純になる。
図に向かって左側の足
A点とB点の関係:

y_{a}=y_{b}-l_{1}sin(\frac{\pi}{2}-\theta_{1})\\z_{a}=z_{b}-l_{1}cos(\frac{\pi}{2}-\theta_{1})


B点とC点の関係:

y_{b}=y_{c}+l_{2}sin(\theta_{1}+\theta_{2}-\frac{\pi}{2})\\z_{b}=z_{c}-l_{2}cos(\theta_{1}+\theta_{2}-\frac{\pi}{2})


C点と原点の関係:

y_{c}=y_{0}-\frac{l_{3}}{2}cos(\theta_{1}+\theta_{2}+\theta_{3}-\pi)\\z_{c}=z_{0}-\frac{l_{3}}{2}sin(\theta_{1}+\theta_{2}+\theta_{3}-\pi)


図に向かって右側の足
原点とD点の関係:

y_{d}=y_{0}+\frac{l_{3}}{2}cos(\theta_{4}+\theta_{5}+\theta_{6}-\pi)\\z_{d}=z_{0}+\frac{l_{3}}{2}sin(\theta_{4}+\theta_{5}+\theta_{6}-\pi)


D点とE点の関係:

y_{e}=y_{d}+l_{4}sin(\theta_{5}+\theta_{6}-\frac{\pi}{2})\\z_{e}=z_{d}-l_{4}cos(\theta_{5}+\theta_{6}-\frac{\pi}{2})


E点とF点の関係:

y_{f}=y_{e}-l_{5}sin(\frac{\pi}{2}-\theta_{6})\\z_{f}=z_{e}-l_{5}cos(\frac{\pi}{2}-\theta_{6})


見ればすぐにわかるように、極めて対称的な関係が求められる。ここで、原点の座標と6つの角度、および各長さが与えられれば、全ての点が決まる。
ただし、六つの角度は全て自由に与えることはできず、

\theta_{1}+\theta_{2}+\theta_{3}=\theta_{4}+\theta_{5}+\theta_{6}


の関係が成立していなければならない。自由度は5なのである。

また、各長さは、実際の長さが、ここでは現れていないx方向の変位によってきまる。

基本的な枠組みはできたので、さらにシミュレーションができるようにする。