inverse-kinematicsのthreを小さくしすぎると解けなくなる

[pazeshun]

以下のように、大きなthreでは解けるIKが、小さなthreでは解けないことがあります。

(cobotta)
(send *cobotta* :angle-vector #f(-10.094 -16.3036 108.178 4.48656 -60.5562 0.91869))
(send *cobotta* :arm :move-end-pos #f(0 0 -30) :world :thre 0.05 :debug-view t)  ;; fail
(send *cobotta* :arm :move-end-pos #f(0 0 -30) :world :thre 0.1 :debug-view t)  ;; succeed

詳細：https://gist.github.com/pazeshun/83a468d232a68fe0c67f50c8d0650a56

cc @k-okada

[pazeshun]

以下のパッケージのように、100倍の大きさのロボットを用意して、座標値を100倍した目標位置姿勢に対してthre=0.1のIKを解いてみたところ解け、その時の関節角度列を元の大きさのロボットにコピーすると、目標位置と手先位置の誤差が0.001 (1μm) 以下になることが確認されました。
https://github.com/pazeshun/cobotta_precise_ik_test
サンプル実行結果：

1.irteusgl$ (test)
PQP Error! EndModel() called on model with no triangles
PQP Error! EndModel() called on model with no triangles

Normal IK (target-coords: #<coordinates #X55c5e99fc488  172.487 -82.762 396.376 / -0.113 0.543 0.101>, thre: 0.1, rthre: 0.017453) succeeded
Positional error between target-coords and IK result: 0.076894

Normal IK (target-coords: #<coordinates #X55c5e99fc488  172.487 -82.762 396.376 / -0.113 0.543 0.101>, thre: 0.05, rthre: 0.017453) failed

Normal IK (target-coords: #<coordinates #X55c5e99fc488  172.487 -82.762 396.376 / -0.113 0.543 0.101>, thre: 0.001, rthre: 0.017453) failed

IK with cobottax100 (target-coords: #<coordinates #X55c5e99fc488  172.487 -82.762 396.376 / -0.113 0.543 0.101>, thre: 0.1, rthre: 0.017453) succeeded
Positional error between target-coords and IK result: 0.00741

IK with cobottax100 (target-coords: #<coordinates #X55c5e99fc488  172.487 -82.762 396.376 / -0.113 0.543 0.101>, thre: 0.05, rthre: 0.017453) succeeded
Positional error between target-coords and IK result: 0.00741

IK with cobottax100 (target-coords: #<coordinates #X55c5e99fc488  172.487 -82.762 396.376 / -0.113 0.543 0.101>, thre: 0.001, rthre: 0.017453) succeeded
Positional error between target-coords and IK result: 1.654754e-07
nil

最後の例が、通常サイズのロボットでthre=0.001のIKを解くもので、100倍の大きさのロボットならthreも100倍してよいということでthre=0.1で解いています。

ただ、IKは基本的に浮動小数点数での演算のはずなので、指数部が飽和してなければ100倍してもしなくても同じはずな気がしていて、結果が変わる理由は不明です。

[Naoki-Hiraoka]

引数:p-limit, :manipulability-limit がサイズ依存です。
また、回転関節の角度が一定量変化したときのエンドエフェクタの姿勢の変化はサイズによって変わりませんが、位置の変化はサイズによって変わるので、エンドエフェクタの位置と姿勢のコスト関数の大きさの比がサイズ依存になるため、:dif-pos-ratioと:dif-rot-ratioもサイズ依存になります。

[Naoki-Hiraoka]

また、今回のケースでは関係ありませんが、並進関節がある場合サイズが大きくなると回転関節を動かす量は変わりませんが並進関節を動かす量は大きくなるので、:weightもサイズ依存になります。