PR2でOptmotiongenの障害物回避IKが上手くいかない

[Kanazawanaoaki]

Optmotiongenのsample-arm-reach-ik-obstacleに使われている障害物回避IKをPR2で試してみようとした(Kanazawanaoaki/oneweek_kanazawa#1 )ところ以下のような問題が発生しました。

:collision-avoidance-link-pair を指定する必要がある

sample-arm-reach-ik-obstacleと同じように:obstacles (list *obstacle*)として障害物を指定してもPR2の場合は障害物回避をすることが出来ませんでした。

(https://gist.github.com/Kanazawanaoaki/1810958f83c44c037b9cb286a047b385#file-obstacle-ik-test-01-l )
@pazeshunに:obstacles (list *obstacle*)の代わりに:collision-avoidance-link-pair (mapcar #'(lambda (l) (list l *obstacle*)) (send *robot* :links))とする必要があることを教えていただきましたが、READMEから辿るにはPR2のregraspのサンプルのコードを注意深く読む以外にない気がします。

障害物を回避しきれていない

(https://gist.github.com/Kanazawanaoaki/1810958f83c44c037b9cb286a047b385#file-obstacle-ik-test-02-l )のように:collision-avoidance-link-pairを指定して実行してみたのですが、障害物を回避しきれていなくて障害物と重なってしまっているように見えます。

(normal-ik)として:inverse-kinematicsを使った時と、(obstacle-ik)として:inverse-kinematics-optmotiongenを使った時の挙動を比べると障害物を指定できていて回避していようとしているように見えます。完全には回避しきれない状態で解き終わってしまうこともあるのでしょうか。

[Naoki-Hiraoka]

PR2は特殊なロボットで，:linksや:link-listに全リンクが含まれていないため，それらのリンクの干渉が考慮されないのが原因ではないかと思います．

(objects (send *robot* :link-list (send *robot* :rarm :end-coords :parent)))

(objects (send *robot* :links))

:collision-avoidance-link-pairにしっかりリンクを指定してやるとうまく行くのではないかと思いますが，PR2のリンク構造によってはそのままだとうまく動かないかもしれません．

今回の原因と直接関係はありませんが，現在のoptmotiongenが使用しているPQPによる干渉チェックは，物体同士が既に干渉している時に離れる方向を正しく出力しないため，一度干渉すると干渉したままになってしまいやすいという問題があります．これは，euslisp/jskeus#555 がリリースされると改善できるのですが，今のところは初期姿勢で物体と干渉していないように注意する必要があります．

[Kanazawanaoaki]

ありがとうございます。
本当ですね、、(objects (send *robot* :link-list (send *robot* :rarm :end-coords :parent)))を見て驚きました。考慮したいリンクが全部指定されているようにしないといけないみたいですね。

初期姿勢で物体と干渉していないかのチェックもするようにしようと思います。

[Kanazawanaoaki]

PR2はぶつけるオプションもあるということでしたが、

:collision-avoidance-link-pair (mapcar #'(lambda (l) (list l *obstacle*)) (append (send *robot* :link-list (send *robot* :rarm :end-coords :parent)) (list (send *robot* :r_forearm_link_lk) (send *robot* :r_upper_arm_link_lk) (send *robot* :r_gripper_palm_link_lk) (send *robot* :r_gripper_r_finger_link_lk) (send *robot* :r_gripper_l_finger_link_lk) (send *robot* :r_gripper_r_finger_tip_link_lk)

と考慮したいリンクをappendすることで回避する事ができました。

(objects (append (send *robot* :link-list (send *robot* :rarm :end-coords :parent)) (list (send *robot* :r_forearm_link_lk) (send *robot* :r_upper_arm_link_lk) (send *robot* :r_gripper_palm_link_lk) (send *robot* :r_gripper_r_finger_link_lk) (send *robot* :r_gripper_l_finger_link_lk) (send *robot* :r_gripper_r_finger_tip_link_lk) (send *robot* :r_gripper_l_finger_tip_link_lk))))

[k-okada]

メモ：jsk-ros-pkg/jsk_model_tools#171
に議論があるもので．jsk-ros-pkg/jsk_model_tools#171 (comment) に汚いけど，プログラマブルにcollision-linkを正しく（見えるように）する方法が書いてあります．他の人やロボットでもこれが必要な状況が増えたら，各自がコードを書かずに，上流で対応したいともうので，ココにやってきた人は足跡を残していてください．

ちなみにこれはURDFがassocの代わりにfixed jointで別リンクをつなげていく，というのが原因だともいます．

[pazeshun]

@Kanazawanaoaki
まだ
https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/optmotiongen/README.md
に情報が載ってないので、少なくとも、このissueは開けておいた方がよいと思いました。

[Kanazawanaoaki]

そうですね、開けておくことにします。