[irteus/irtmodel.l] check collision if colision-pair is not included in link-list

[knorth55]

When collision-pair is not included in link-list, its collision data (elt col-list i) is set as nil.

(if (not (memq (car pair) union-link-list))
    (progn
      (warn ";; ERROR : (car pair) is not included in link-list ~A~%" (send-all pair :name))
      (setf (elt col-list i) nil)
)

https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L1323-L1327

This cause errors in https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L1348 and https://github.com/euslisp/jskeus/blob/master/irteus/irtmodel.l#L1429-L1434, because of doing (elt nil 0).

When I came into debugging this problem, I did not understand why there is no collision check for collision-pair which are not in link-list.
I thought that even if collision-pair is not in link-list, we should check its collision distance` .

[k-okada]

is this related to colladaeus problem, where link-list does not include all link body objects?
jsk-ros-pkg/jsk_model_tools#171
jsk-ros-pkg/jsk_pr2eus#206

[knorth55]

I made PR in jsk_apc start-jsk/jsk_apc#2624, and I added custom gripper for collision links.
In that case, right_gripper_tube, left_gripper_tube, right_gripper_pad_with_base and left_gripper_pad_with_base are not in link-list.

How to reproduce is as follow;

roscd jsk_arc2017_baxter
git remote add knorth55 https://github.com/knorth55/jsk_apc.git
git fetch knorth55
git checkout knorth55/add-avoid-collision-ik
cd euslisp/lib/
roseus baxter.l

1.irteusgl$ (setq *baxter* (instance jsk_arc2017_baxter::baxter-robot-safe :init))
#<jsk_arc2017_baxter::baxter-robot-safe #X74fad00 baxter  0.0 0.0 0.0 / 0.0 0.0 0.0>
2.irteusgl$ (setq larm-coords (send *baxter* :larm :end-coords))
#<cascaded-coords #X7071450 :larm-end-coords  768.695 313.691 62.507 / 0.642 0.307 -0.979>
3.irteusgl$ (send *baxter* :rarm :inverse-kinematics larm-coords :rotation-axis nil :revert-if-fail nil :debug-view t)
loop:   1
union-link-list: (right_upper_shoulder right_lower_shoulder right_upper_elbow right_lower_elbow right_upper_forearm right_lower_forearm right_wrist)
move-target: (#<cascaded-coords #X62d03d8 :rarm-end-coords  768.695 -313.691 62.507 / -0.642 0.307 0.979>)
targe-coordst: (#<cascaded-coords #X7071450 :larm-end-coords  768.695 313.691 62.507 / 0.642 0.307 -0.979>)
vel-pos :-57.117  29.600 -76.561
vel-rot :
vel-pos-norm : 100.000/  1.000
vel-rot-norm :   0.000/  0.175
angle :  -20.0   -25.0    40.0    60.0    20.0    80.0     0.0
 min  :  -97.5  -123.0  -175.0    -2.9  -175.3   -90.0  -175.3
 max  :   97.5    60.0   175.0   150.0   175.3   120.0   175.3
usrwei:  1.000   1.000   1.000   1.000   1.000   1.000   1.000
ocost :100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000 100000000000000000000.000
cost  :  0.263   0.090   0.167   0.284   0.077   1.778   0.000
weight:  1.000   1.000   1.000   1.000   1.000   1.000   1.000
k     :  0.000 (manipulability:  0.510, gain:  0.001, limit:  0.100, len:7)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base right_gripper_l_finger)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base right_gripper_r_finger)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube right_gripper_l_finger)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube right_gripper_r_finger)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base right_gripper_l_finger_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube right_gripper_l_finger_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base right_gripper_palm_endpoint)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube right_gripper_palm_endpoint)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base right_gripper_r_finger_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube right_gripper_r_finger_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base right_upper_shoulder)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube right_upper_shoulder)
PQP Error! EndModel() called on model with no triangles
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_gripper_pad_with_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_gripper_pad_with_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_gripper_l_finger)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_gripper_l_finger)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_gripper_r_finger)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_gripper_r_finger)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_gripper_tube)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_gripper_tube)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_gripper_l_finger_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_gripper_l_finger_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_gripper_palm_endpoint)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_gripper_palm_endpoint)
PQP Error! EndModel() called on model with no triangles
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_gripper_r_finger_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_gripper_r_finger_base)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_upper_shoulder)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_upper_shoulder)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_lower_shoulder)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_lower_shoulder)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_upper_elbow)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_upper_elbow)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_upper_forearm)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_upper_forearm)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_lower_elbow)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_lower_elbow)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_lower_forearm)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_lower_forearm)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base left_wrist)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube left_wrist)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base head)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube head)
;; ERROR : (car pair) is not included in link-list (right_gripper_pad_with_base screen)
;; ERROR : (car pair) is not included in link-list (right_gripper_tube screen)
Call Stack (max depth: 20):
  0: at #<compiled-code #X52f13e0>
  1: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  2: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  3: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  4: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  5: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  6: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  7: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  8: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  9: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  10: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  11: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  12: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  13: at (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)
  14: at (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args))
  15: at (let ((r) (prev-av (send self :angle-vector))) (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)) (unless r (format *error-output* "; failed for normal ik, starting from relaxed position~%") (let ((current-coords (send (cadr (memq :move-target args)) :copy-worldcoords))) (send-super* :inverse-kinematics current-coords :rotation-axis nil :avoid-nspace-gain 0.1 :avoid-weight-gain 0.1 :stop 200 :avoid-collision-distance avoid-collision-distance :debug-view nil args) (setq r (send-super* :inverse-kinematics target-coords :warnp warnp :dump-command dump-command args)) (if (and (null r) (or (null (memq :revert-if-fail args)) (cadr (memq :revert-if-fail args)))) (send self :angle-vector prev-av)))) (unless r (format *error-output* "; failed for normal ik, try to move arms very slowly~%") (let ((step 0.0) (current-coords (send (cadr (memq :move-target args)) :copy-worldcoords))) (setq r t) (while (and r (<= step 1.0)) (setq r (send-super* :inverse-kinematics (midcoords step current-coords target-coords) :avoid-collision-distance avoid-collision-distance :debug-view nil :warnp warnp :dump-command dump-command args)) (incf step 0.01)) (unless r (send self :angle-vector prev-av)))) (unless r (format *error-output* "; failed for slow ik, try to start from prepared poses~%") (let* ((move-joints (send-all (cadr (memq :link-list args)) :joint)) (av (mapcar #'(lambda (j) (send j :joint-angle)) (send self :joint-list))) (ik-prepared-poses (send self :ik-prepared-poses))) (dolist (pose ik-prepared-poses) (unless r (format *error-output* "; starting from prepared pose '~A'~%" pose) (send self pose) (mapcar #'(lambda (j a) (if (not (memq j move-joints)) (send j :joint-angle a))) (send self :joint-list) av) (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args)))) (when (and (null r) (or (null (memq :revert-if-fail args)) (cadr (memq :revert-if-fail args)))) (when dump-command (format *error-output* "; failed all ik attempts, dump all data to /tmp/irtmodel-ik-~A~%" (unix:getpid)) (format *error-output* ";      base coords ~A~%" (send self :worldcoords)) (format *error-output* ";    target coords ~A~%" target-coords)) (send self :angle-vector prev-av)))) r)
  16: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :move-target move-target :link-list link-list :rthre rthre args)
  17: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :move-target move-target :link-list link-list :rthre rthre args)
  18: at (apply #'send-message self (class . super) :inverse-kinematics target-coords :move-target move-target :link-list link-list :rthre rthre args)
  19: at (send-super* :inverse-kinematics target-coords :move-target move-target :link-list link-list :rthre rthre args)
  And more...
/home/shingo/jskeus/eus/Linux64/bin/irteusgl 0 error: number expected in #<compiled-code #X52f13e0>
4.E1-irteusgl$

[knorth55]

gripper_tube

gripper_pad_with_base

[knorth55]

jsk_arc2017_baxter uses custom :inverse-kinematics defined in https://github.com/start-jsk/jsk_apc/blob/master/jsk_arc2017_baxter/euslisp/lib/baxter.l#L87-L113.
This errors occurs when I set :use-gripper nil arg for :inverse-kinematics.
When :use-gripper nil is set, link-list remove grippers to solve IK with no gripper, which causes this error.

[k-okada]

c.f. you can set custom move target https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/pr2-utils.l#L86