[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: https://github.com/start-jsk/rtmros_tutorials/blob/303e2bf87d1f6582017c4fe2a145db5d1ce99bca/hrpsys_ros_bridge_tutorials/CMakeLists.txt#L293

This PR enables to use :self-collision-check by ignoring collision between CHEST_JOINT0_Link and HEAD_JOINT1_Link. The same method as https://github.com/jsk-ros-pkg/jsk_robot/pull/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!