[hrpsys_ros_bridge_tutorials][HIRONXJSK] Enable :self-collision-check on euslisp interface

[pazeshun]

Currently, :self-collision-check on HIRONXJSK always returns non-nil value:

1.irteusgl$ (hironxjsk-init)
...
2.irteusgl$ (objects (list *hironxjsk*))
;; (make-irtviewer) executed
(#<hironxjsk-sensor-robot #X5582026b6f80 HIRONXJSK  0.0 0.0 0.0 / 0.0 0.0 0.0>)
3.irteusgl$ (send *hironxjsk* :reset-manip-pose)
#f(-0.6 0.0 -100.0 15.2 9.4 3.2 0.6 0.0 -100.0 -15.2 9.4 -3.2 0.0 0.0 0.0)
4.irteusgl$ (send *hironxjsk* :self-collision-check)
((#<bodyset-link #X5582027886c0 CHEST_JOINT0_Link  0.0 0.0 0.0 / 0.0 0.0 0.0> . #<bodyset-link #X5582029ec050 HEAD_JOINT1_Link  0.0 0.0 569.5 / 0.0 0.0 0.0>))

This is because convex hull of CHEST_JOINT0_Link and HEAD_JOINT1_Link always collide:

18.irteusgl$ (send-all (remove nil (mapcar #'(lambda (l) (let ((name (send l :name))) (if (or (string= name "CHEST_JOINT0_Link") (string= name "HEAD_JOINT1_Link")) l nil))) (send *hironxjsk* :links))) :draw-on :flush t)

This collision is not checked on hrpsys CollisionDetector:

rtmros_tutorials/hrpsys_ros_bridge_tutorials/CMakeLists.txt

Line 293 in 303e2bf

--conf-file-option "collision_pair: HEAD_JOINT0:RARM_JOINT2 HEAD_JOINT0:RARM_JOINT3 HEAD_JOINT0:RARM_JOINT4 HEAD_JOINT0:RARM_JOINT5 HEAD_JOINT0:LARM_JOINT2 HEAD_JOINT0:LARM_JOINT3 HEAD_JOINT0:LARM_JOINT4 HEAD_JOINT0:LARM_JOINT5 HEAD_JOINT1:RARM_JOINT2 HEAD_JOINT1:RARM_JOINT3 HEAD_JOINT1:RARM_JOINT4 HEAD_JOINT1:RARM_JOINT5 HEAD_JOINT1:LARM_JOINT2 HEAD_JOINT1:LARM_JOINT3 HEAD_JOINT1:LARM_JOINT4 HEAD_JOINT1:LARM_JOINT5 WAIST:RARM_JOINT3 WAIST:RARM_JOINT4 WAIST:RARM_JOINT5 WAIST:LARM_JOINT3 WAIST:LARM_JOINT4 WAIST:LARM_JOINT5 CHEST_JOINT0:RARM_JOINT2 CHEST_JOINT0:RARM_JOINT3 CHEST_JOINT0:RARM_JOINT4 CHEST_JOINT0:RARM_JOINT5 CHEST_JOINT0:LARM_JOINT2 CHEST_JOINT0:LARM_JOINT3 CHEST_JOINT0:LARM_JOINT4 CHEST_JOINT0:LARM_JOINT5 RARM_JOINT0:LARM_JOINT1 RARM_JOINT0:LARM_JOINT2 RARM_JOINT0:LARM_JOINT3 RARM_JOINT0:LARM_JOINT4 RARM_JOINT0:LARM_JOINT5 RARM_JOINT1:LARM_JOINT0 RARM_JOINT1:LARM_JOINT1 RARM_JOINT1:LARM_JOINT2 RARM_JOINT1:LARM_JOINT3 RARM_JOINT1:LARM_JOINT4 RARM_JOINT1:LARM_JOINT5 RARM_JOINT2:LARM_JOINT0 RARM_JOINT2:LARM_JOINT1 RARM_JOINT2:LARM_JOINT2 RARM_JOINT2:LARM_JOINT3 RARM_JOINT2:LARM_JOINT4 RARM_JOINT2:LARM_JOINT5 RARM_JOINT3:LARM_JOINT0 RARM_JOINT3:LARM_JOINT1 RARM_JOINT3:LARM_JOINT2 RARM_JOINT3:LARM_JOINT3 RARM_JOINT3:LARM_JOINT4 RARM_JOINT3:LARM_JOINT5 RARM_JOINT4:LARM_JOINT0 RARM_JOINT4:LARM_JOINT1 RARM_JOINT4:LARM_JOINT2 RARM_JOINT4:LARM_JOINT3 RARM_JOINT4:LARM_JOINT4 RARM_JOINT4:LARM_JOINT5 RARM_JOINT5:LARM_JOINT0 RARM_JOINT5:LARM_JOINT1 RARM_JOINT5:LARM_JOINT2 RARM_JOINT5:LARM_JOINT3 RARM_JOINT5:LARM_JOINT4 RARM_JOINT5:LARM_JOINT5"

This PR enables to use :self-collision-check by ignoring collision between CHEST_JOINT0_Link and HEAD_JOINT1_Link.
The same method as jsk-ros-pkg/jsk_robot#1026 is used.

1.irteusgl$ (hironxjsk-init)
...
2.irteusgl$ (objects (list *hironxjsk*))
;; (make-irtviewer) executed
(#<hironxjsk-sensor-robot #X5600079efe98 HIRONXJSK  0.0 0.0 0.0 / 0.0 0.0 0.0>)
3.irteusgl$ (send *hironxjsk* :reset-manip-pose)
#f(-0.6 0.0 -100.0 15.2 9.4 3.2 0.6 0.0 -100.0 -15.2 9.4 -3.2 0.0 0.0 0.0)
4.irteusgl$ (send *hironxjsk* :self-collision-check)
nil
5.irteusgl$ (send *hironxjsk* :rarm :move-end-pos #f(0 300 0) :world)
#f(44.8284 -10.0014 -74.5099 10.6865 5.04297 -44.846 0.6 0.0 -100.0 -15.2 9.4 -3.2 0.0 0.0 0.0)
6.irteusgl$ (send *hironxjsk* :self-collision-check)
((#<bodyset-link #X56000793c5b0 LARM_JOINT5_Link  325.667 182.357 15.625 / 3.073 -1.569 -3.073> . #<bodyset-link #X560006e7d7f0 RARM_JOINT5_Link  325.607 117.586 15.691 / -3.071 -1.569 3.071>) (#<bodyset-link #X56000793c5b0 LARM_JOINT5_Link  325.667 182.357 15.625 / 3.073 -1.569 -3.073> . #<bodyset-link #X560006f566e0 RHAND_JOINT2_Link  325.654 150.59 -14.204 / -3.071 -1.569 3.071>) (#<bodyset-link #X560006e8bef8 LHAND_JOINT0_Link  325.714 149.353 -14.271 / 3.073 -1.569 -3.073> . #<bodyset-link #X560006e7d7f0 RARM_JOINT5_Link  325.607 117.586 15.691 / -3.071 -1.569 3.071>) (#<bodyset-link #X560006e8bef8 LHAND_JOINT0_Link  325.714 149.353 -14.271 / 3.073 -1.569 -3.073> . #<bodyset-link #X560006f566e0 RHAND_JOINT2_Link  325.654 150.59 -14.204 / -3.071 -1.569 3.071>) (#<bodyset-link #X560006e8bef8 LHAND_JOINT0_Link  325.714 149.353 -14.271 / 3.073 -1.569 -3.073> . #<bodyset-link #X560006f73930 RHAND_JOINT3_Link  325.728 150.595 -56.104 / -3.071 -1.569 3.071>) (#<bodyset-link #X5600078b4e28 LHAND_JOINT1_Link  325.788 149.348 -56.171 / 3.073 -1.569 -3.073> . #<bodyset-link #X560006f566e0 RHAND_JOINT2_Link  325.654 150.59 -14.204 / -3.071 -1.569 3.071>) (#<bodyset-link #X5600078b4e28 LHAND_JOINT1_Link  325.788 149.348 -56.171 / 3.073 -1.569 -3.073> . #<bodyset-link #X560006f73930 RHAND_JOINT3_Link  325.728 150.595 -56.104 / -3.071 -1.569 3.071>))

[pazeshun]

Not tested well, needs testing for a while.

[pazeshun]

@YUKINA-3252 Could you use this branch instead of https://github.com/YUKINA-3252/rtmros_tutorials/tree/hironxjsk-collision?
Because this branch realizes https://github.com/YUKINA-3252/rtmros_tutorials/blob/hironxjsk-collision/hrpsys_ros_bridge_tutorials/euslisp/hironxjsk-utils.l#L125.

[Naoki-Hiraoka]

This is because convex hull of CHEST_JOINT0_Link and HEAD_JOINT1_Link always collide:

細かなところで申し訳ないのですが、euslispのcollision checkは、convex hullではなく生のメッシュを使っているのではないかと思いました。

[knorth55]

PQPだと生のメッシュでBulletだと凸包に近似しているとドキュメントには書いてありますね。
私も勘違いしてました。

[pazeshun]

This is because convex hull of CHEST_JOINT0_Link and HEAD_JOINT1_Link always collide:

細かなところで申し訳ないのですが、euslispのcollision checkは、convex hullではなく生のメッシュを使っているのではないかと思いました。

打ち消し線で直しました、ありがとうございます

[pazeshun]

@k-okada This PR is used by @YUKINA-3252 for a long time and no issue has been reported.
Could you review this?

[pazeshun]

Thank you!