Collision avoidance IK easily failed on HIRONXJSK

[YUKINA-3252]

I want to try collision avoidance IK on HIRONXJSK with the following modification to the model:
master...YUKINA-3252:rtmros_tutorials:hironxjsk-collision-ik
But the following code

(hironxjsk-init)
(objects (list *hironxjsk*))
(send *hironxjsk* :reset-manip-pose)
(send *hironxjsk* :rarm :move-end-pos #f(0 150 0) :world :debug-view t :check-collision t)

returned IK failure:
https://gist.github.com/YUKINA-3252/2a82d117c721e070c80d1db12a45e22f

This code succeeded when I did not use collision avoidance IK (set no collision-avoidance-links):
master...YUKINA-3252:rtmros_tutorials:hironxjsk-collision
log: https://gist.github.com/YUKINA-3252/363559b66b0e4b10f950ff78b4d4638e

cc @pazeshun @k-okada

[knorth55]

can you check this PR to show and visualize the default collision pairs?
jsk-ros-pkg/jsk_robot#895

[Naoki-Hiraoka]

これは、https://github.com/euslisp/jskeus/blob/3040a1aa1cce1326c2e79890111d099b07207479/irteus/irtmodel.l#L1838-L1862 の手法を使う限り避けられない問題で、以下のようなパラチューンでなんとかするしかないと思います。

(send *robot* :rarm :move-end-pos #f(0 150 0) :world :debug-view t :check-collision t :avoid-collision-joint-gain 0.0) 

(send *robot* :rarm :move-end-pos #f(0 150 0) :world :debug-view t :check-collision t :avoid-collision-distance 100) 

• Should Null space be computed using SR-Inverse? fkanehiro/hrpsys-base#1322 の修正をirteusに施すと若干改善すると思います.
• https://github.com/jsk-ros-pkg/jsk_control/tree/master/eus_qp/optmotiongen 等の不等式制約が扱えるソルバを利用した逆運動学を使うと、パラチューンが必要な度合いが減ります.

[YUKINA-3252]

@knorth55
The two photos below show how the following code was sent.

(hironxjsk-init)
(objects (list *hironxjsk*))
(send *hironxjsk* :reset-manip-pose)
(send-all (send *hironxjsk* :collision-avoidance-links) :draw-on :flush t)

The three photos below show how the following code was sent.

(hironxjsk-init)
(objects (list *hironxjsk*))
(send *hironxjsk* :reset-manip-pose)
(send *hironxjsk* :rarm :move-end-pos #f(0 150 0) :world :debug-view t :check-collision t :revert-if-fail t)
(send-all (send *hironxjsk* :collision-avoidance-links) :draw-on :flush t)

@Naoki-Hiraoka
I made this issue because I am wondering if there are any issues related to HIRO links, etc., before I solve the problem with the ik methodology itself.
I will try to use the method you introduced for solving ik hereafter. Thank you very much.

[k-okada]

@YUKINA-3252
I assume that you want to use collision avoidance IK because the robot hit its hand? arm? to the base body ("WAIST?"). If so, check collision between these links seems fine. For example, setting collision-avoidance-links as follows.

(defmethod hironxjsk-sensor-robot-safe
  (:init (&rest args)
    (send-super* :init args)
    (setq collision-avoidance-links (send self :links))
    (setq larm-collision-avoidance-links collision-avoidance-links)
    (setq rarm-collision-avoidance-links collision-avoidance-links)
    (setq collision-avoidance-links (list (send self :link "WAIST")
                                          (send self :link "LARM_JOINT1_Link")
                                          (send self :link "LARM_JOINT5_Link")
                                          (send self :link "RARM_JOINT1_Link")
                                          (send self :link "RARM_JOINT5_Link")
                                          ))
    (setq larm-collision-avoidance-links collision-avoidance-links
          rarm-collision-avoidance-links collision-avoidance-links)
    )
  )

Or, if you just do not want to send collided angle-vector to real robot, solve ik without :collision t but check (send *robot* :check-self-collision) before (send *ri* :angle-vector).
Because hiro robot only have 6 DoF, so I think advantage of avoiding collision during IK is not so much.

[YUKINA-3252]

issueの目的がわかりにくくすみません。
もともと私の目的は、一連の動作のikを解く過程でcollisionしているかどうかを確認したいというもので、
その意味では岡田先生のおっしゃるとおり:self-collision-checkや:check-collisionを使用することが解決策となるのだと思います。
ただ、ikを解く過程で衝突回避をするような計算があると教わり試してみたのですがうまくいかず、報告としてこのissueをたてました。
現状では私の最初のコメントにあるようにcollision-avoidanceを使うとikが解けない、という問題がありましたが、
これについてはみなさんが指摘されているようにhttps://github.com/jsk-ros-pkg/jsk_robot/pull/895のように
ペアを厳選していったり、 @Naoki-Hiraoka さんが示してくださったようにパラメータを変えたりソルバを用いたりすればうまくいくことがあるということがわかりました。
必要になった場合はこれらを使いたいと思います。
みなさんありがとうございました。