[irteus/irtmodel.l] support :fat option in self-collision-check (euslisp#147)

Author: k-okada

irteus/irtmodel.l

@@ -2396,10 +2396,19 @@
        )
      pairs))
   (:self-collision-check
-   (&key (mode :all) (pairs (send self :collision-check-pairs)) (collision-func 'pqp-collision-check))
+   (&key (mode :all) (pairs (send self :collision-check-pairs)) (collision-func 'pqp-collision-check) (distance-func 'pqp-collision-distance) (fat 0.0) (min-distance))
+   "calculate self collision chaeck
+      :mode (:all or :first) ; returns first collided link pair of all collided link pair
+      :pairs (list (cons <link1> <link2>) ....) ; linke pair to be checked
+      :min-distance (number) if number is set, any link pair that has smaller distance than this fat threshold will regard as collided
+      :fat : fatten collision model"
    (let ((cpairs) (col-count 0))
      (dolist (p pairs)
-       (let ((colp (/= (funcall collision-func (car p) (cdr p)) 0)))
+       (let ((colp
+              (cond ((numberp min-distance)
+                     (< (car (funcall distance-func (car p) (cdr p) :fat fat)) min-distance))
+                    (t
+                     (/= (funcall collision-func (car p) (cdr p) geo::PQP_FIRST_CONTACT :fat fat) 0)))))
 	 (when colp
 	   (incf col-count)
 	   (if (eq mode :first)

irteus/test/robot-model-usage.l

@@ -68,6 +68,14 @@
         ))
     ))
 
+(deftest test-collision-check
+  (let ()
+    (send *robot* :reset-pose)
+    (assert (null (send *robot* :self-collision-check :pairs (list (cons (send *robot* :link :lleg-link5) (send *robot* :link :rleg-link5))))) "this is saf pose")
+    (assert (null (send *robot* :self-collision-check :pairs (list (cons (send *robot* :link :lleg-link5) (send *robot* :link :rleg-link5))) :min-distance 49)) "distance value is 50 > 49")
+    (assert (send *robot* :self-collision-check :pairs (list (cons (send *robot* :link :lleg-link5) (send *robot* :link :rleg-link5))) :min-distance 51) "closest distance is 50 < 51")
+    ))
+
 
 (run-all-tests)
 (exit)