From 4817782ba29528d7ed81e4db650a8896b0ade4e9 Mon Sep 17 00:00:00 2001
From: Kei Okada <kei.okada@gmail.com>
Date: Mon, 20 Jul 2015 13:14:56 +0900
Subject: [PATCH] [irteus/irtmodel.l] support :fat option in
 self-collision-check (#147)

---
 irteus/irtmodel.l               | 12 ++++++++++--
 irteus/test/robot-model-usage.l |  8 ++++++++
 2 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l
index 395b5fd55..b83e6d4cf 100644
--- a/irteus/irtmodel.l
+++ b/irteus/irtmodel.l
@@ -2396,10 +2396,18 @@
        )
      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 nil))
+   "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
+      :fat (number) if number is set, any link pair that has smaller distance than this fat threshold will regard as collided"
    (let ((cpairs) (col-count 0))
      (dolist (p pairs)
-       (let ((colp (/= (funcall collision-func (car p) (cdr p)) 0)))
+       (let ((colp
+              (cond ((numberp fat)
+                     (< (car (funcall distance-func (car p) (cdr p) :fat fat)) fat))
+                    (t
+                     (/= (funcall collision-func (car p) (cdr p)) 0)))))
 	 (when colp
 	   (incf col-count)
 	   (if (eq mode :first)
diff --git a/irteus/test/robot-model-usage.l b/irteus/test/robot-model-usage.l
index 0d9816ac6..40eda4cfa 100644
--- a/irteus/test/robot-model-usage.l
+++ b/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))) :fat 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))) :fat 51) "closest distance is 50 < 51")
+    ))
+
 
 (run-all-tests)
 (exit)