diff --git a/irteus/irtmodel.l b/irteus/irtmodel.l index 395b5fd55..858d46bea 100644 --- a/irteus/irtmodel.l +++ b/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 ) ....) ; 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) diff --git a/irteus/pqp.l b/irteus/pqp.l index 75ce793ac..0f16027c2 100644 --- a/irteus/pqp.l +++ b/irteus/pqp.l @@ -35,6 +35,7 @@ (let ((m (pqpmakemodel)) vs v1 v2 v3 (id 0)) (setf (get self :pqpmodel) m) + (setf (get self :pqpmodel-fat) fat) (pqpbeginmodel m) (dolist (f fs) (dolist (poly (face-to-triangle-aux f)) @@ -55,23 +56,27 @@ ) (defun pqp-collision-check (model1 model2 - &optional (flag PQP_FIRST_CONTACT) &key (fat 0) (fat2 nil)) + &optional (flag PQP_FIRST_CONTACT) &key (fat 0.0) (fat2 nil)) (let ((m1 (get model1 :pqpmodel)) (m2 (get model2 :pqpmodel)) + (f1 (get model1 :pqpmodel-fat)) + (f2 (get model2 :pqpmodel-fat)) (r1 (send model1 :worldrot)) (t1 (send model1 :worldpos)) (r2 (send model2 :worldrot)) (t2 (send model2 :worldpos))) (if (null fat2) (setq fat2 fat)) - (if (null m1) (setq m1 (send model1 :make-pqpmodel :fat fat))) - (if (null m2) (setq m2 (send model2 :make-pqpmodel :fat fat2))) + (if (not (and m1 (eps= fat f1))) (setq m1 (send model1 :make-pqpmodel :fat fat))) + (if (not (and m2 (eps= fat2 f2))) (setq m2 (send model2 :make-pqpmodel :fat fat2))) (pqpcollide r1 t1 m1 r2 t2 m2 flag) )) (defun pqp-collision-distance (model1 model2 - &key (fat 0) (fat2 nil) (qsize 2)) + &key (fat 0.0) (fat2 nil) (qsize 2)) (let ((m1 (get model1 :pqpmodel)) (m2 (get model2 :pqpmodel)) + (f1 (get model1 :pqpmodel-fat)) + (f2 (get model2 :pqpmodel-fat)) (r1 (send model1 :worldrot)) (t1 (send model1 :worldpos)) (r2 (send model2 :worldrot)) @@ -80,8 +85,8 @@ (p2 (float-vector 0 0 0)) r) (if (null fat2) (setq fat2 fat)) - (if (null m1) (setq m1 (send model1 :make-pqpmodel :fat fat))) - (if (null m2) (setq m2 (send model2 :make-pqpmodel :fat fat2))) + (if (not (and m1 (eps= fat f1))) (setq m1 (send model1 :make-pqpmodel :fat fat))) + (if (not (and m2 (eps= fat2 f2))) (setq m2 (send model2 :make-pqpmodel :fat fat2))) (setq r (pqpdistance r1 t1 m1 r2 t2 m2 p1 p2 qsize)) (list r p1 p2) )) diff --git a/irteus/test/geo.l b/irteus/test/geo.l index 5b8885f0d..04f37ebb3 100644 --- a/irteus/test/geo.l +++ b/irteus/test/geo.l @@ -61,6 +61,42 @@ bottom-frame )) +(deftest test-pqp-collision + (let (b1 b2 t0 t1 r (l 100) (tm (instance mtimer :init))) + (dolist (p (list (list t 0.0 #f(-50 50 50) (list 0.0 0.0 0.0)) (list nil 50.0 #f(-150 50 50) (list 0.0 0.0 0.0)) + (list t 0.0 #f(-50 50 50) (list 0.3 0.3 0.3)) (list nil 30.8431 #f(-150 50 50) (list 0.3 0.3 0.3)))) ;; (collidep distance pos rot) + (send tm :start) + (dotimes (i l) + (setq b1 (make-cube 100 100 100)) + (setq b2 (make-cube 100 100 100 :pos (caddr p) :rpy (cadddr p)))) + (setq t0 (send tm :stop)) + (objects (list b1 b2)) + (format t "create model (base line time) ~7,3f (~A)~%" t0 p) + ;; + (send tm :start) + (dotimes (i l) + (setq b1 (make-cube 100 100 100)) + (setq b2 (make-cube 100 100 100 :pos (caddr p) :rpy (cadddr p))) + (setq r (/= (pqp-collision-check b1 b2) 0)) ;; pqp-collision-check returns 1 if collide + (assert (equal r (car p)) (format nil "check collision check r = ~A, p = ~A" r p)) + ) + (objects (list b1 b2)) + (setq t1 (send tm :stop)) + (format t "pqp collision check ~7,3f (~A msec/check)~%" t1 (/ (- t1 t0) l)) + ;; + (send tm :start) + (dotimes (i l) + (setq b1 (make-cube 100 100 100)) + (setq b2 (make-cube 100 100 100 :pos (caddr p) :rpy (cadddr p))) + (setq r (pqp-collision-distance b1 b2)) + (assert (eps= (car r) (cadr p)) (format nil "check collision distance r = ~A, p = ~A" r p)) + ) + (objects (list b1 b2)) + (setq t1 (send tm :stop)) + (format t "pqp collision distance ~7,3f (~A msec/check)~%" t1 (/ (- t1 t0) l)) + ) ;; dolist + )) + (run-all-tests) (exit) diff --git a/irteus/test/robot-model-usage.l b/irteus/test/robot-model-usage.l index 0d9816ac6..bdc861243 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))) :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)