ロボット座標の再構成(修正)

いざプログラムに書こうと思ったら、先の記事に書いた正面図が、ひざ関節が左右に曲がるというものになっているのに気づいた。足郎2は、ひざは前後にしか曲がらない(人間と同様)ので、修正した。

正面図は、変な格好だが、図は角度の正の向きを確認し、関節位置の座標を確定するためのものだから問題ない。

《正面図》
A点とB点の関係

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


B点とC点の関係

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


C点と原点の関係

y_{c}=y_{0}-\frac{l_{3}}{2}\\z_{c}=z_{0}


D点と原点の関係

y_{d}=y_{0}+\frac{l_{3}}{2}\\z_{d}=z_{0}


D点とE点の関係

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


E点とF点の関係

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

《側面図》
A点とB点の関係

x_{a}=x_{b}-m_{1}sin(\phi_{2}+\phi_{3}-\frac{\pi}{2})\\z_{a}=z_{b}-m_{1}cos(\phi_{2}+\phi_{3}-\frac{\pi}{2})


B点とC点の関係

x_{b}=x_{c}+m_{2}sin(\frac{\pi}{2}-\phi_{3})\\z_{b}=z_{c}-m_{2}cos(\frac{\pi}{2}-\phi_{3})


C点とD点と原点の関係

x_{c}=x_{d}=x_{0}\\z_{c}=z_{d}=z_{0}


D点とE点の関係

x_{e}=x_{d}-m_{4}sin(\frac{\pi}{2}-\phi_{4})\\z_{e}=z_{d}-m_{4}cos(\frac{\pi}{2}-\phi_{4})


E点とF点の関係

x_{f}=x_{e}-m_{5}sin(\frac{\pi}{2}-\phi_{4}+\phi_{5})\\z_{f}=z_{e}-m_{5}cos(\frac{\pi}{2}-\phi_{4}+\phi_{5})