バイオロイドの関節角が欲しかったのだが、とりあえず、関節角から腕の先のXYZ座標を出してみた。
肩から肘にかけてサーボの角度をθ1、θ2、θ3とし、腕を伸ばして垂直に垂らした状態をθ1=0、θ2=0、θ3=π（パイ）とした。
また、前方をY、上をZ、肩から外に向かう方向をXと考える。
これを連立して解けばいいのだが、θ1、θ2、θ3が一様に定まらない気がする。
とくにXYZ＝(0,0,0)のときとか分かりやすく無理そう。なので、計算ヤメ・・・。
XYZからθを求められないものか・・・。
あと、今、サークルのマシンが２点間の座標から補完するプログラムで動いている。
今まで速度一定で補完していたのだが。（↓）
ちなみにx1がもともとの座標、x2が目標座標。
iがループカウンタで、nがループ数である。（for文で回して補完して使う）
それをsinカーブで補完してみた。単純なsinカーブのθ=-π/2からθ=π/2の領域を指定しただけである。
加速しながら移動して、減速しながら座標で止まる。
少しの工夫なのだが、実装してみたところ、ふんわりした動作でなかなかいい感じになった。
とくにバックラッシュの大きいサーボがいい感じに動きました。
また、動き始めが加速度∞ではないので、サーボに与える衝撃も少ないんじゃないかと期待してます。
ちなみにsinで補完したのはなんとなく。
歩行等のモーションを作る段階で、これがホントにいいのかは謎・・・。
