Debug like fullbodyik

[YuOhara]

• すべてthreshold以内に入ったらループが終わるように
• ikが解けなかったらwarnpの文章が適切に出るように

  503

[mmurooka]

ありがとう．

・確認する中でも立ち位置が変わってしまうけれど大丈夫か． ・目標手先位置軌道の点が離れている場合に，IKのイテレーション3回で，ある点から次の点まで手先が動いてくれないと収束しないことになりそうに見えるけれど，それで大丈夫か， などが気になるので， とりあえずcheck-convergence引数を追加して， nilだったらこれまで通りにしておいてデフォルトnilにしてもらってもいいでしょうか．

[YuOhara]

これですが、そもそもIKはstop 2だと一周だけ解く. stop1だと一周も解かない、になると思います。

https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L2260 で

(while (< (incf loop) stop)

という記述があり、初期値が(局所変数)loop=0で与えてるなので、stopが1の場合

11.E3-irteusgl$ setq loop 0
0
12.E3-irteusgl$ < (incf loop) 1
nil

となって抜けてしまうので、ややこしいですが3回解くのを意図してなくて、今まで通り1回だけ解いてます

[YuOhara]

でも確かに、立ち位置の方にthreshouldがないと、数センチの範囲ですが誤差が乗りそうです。

[mmurooka]

・目標手先位置軌道の点が離れている場合に，IKのイテレーション3回で，ある点から次の点まで手先が動いてくれないと収束しないことになりそうに見えるけれど，それで大丈夫か，

は，各目標手先位置ごとに別で，angle-vectorを保持しているので大丈夫そうでした．

・確認する中でも立ち位置が変わってしまうけれど大丈夫か．

も，ループの中でfullbody-inverse-kinematicsを2回呼んでいるうち， 後者は

これですが、そもそもIKはstop 2だと一周だけ解く. stop1だと一周も解かない、になると思います。

なので動かず， 前者で動く分は，

立ち位置の方にthreshouldがないと、数センチの範囲ですが誤差が乗りそうです。

が気になるかもしれませんが，とりあえず使ってみて様子見でよさそうです．

[garaemon]

が気になるかもしれませんが，とりあえず使ってみて様子見でよさそうです．

ループの最後の方は重み0にするとか

[mmurooka]

ループの最後の方は重み0にするとか

ループの最初のほうで手先位置が収束したら，そこで抜けてしまうので，大丈夫なことを保証してはくれなさそうです． 手先位置が収束しそうになったら，立ち位置の重みを0に近づけるとするのもありかもしれませんが， 収束が遅くなったりするのかもしれない気もします．

もしやるとしたら， 立ち位置の動きが小さいという条件をループを抜ける条件にandで加えるとかのように思います．