Skip to content

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

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
+66 −8
Open

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

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4a6c4ee
[irteus/irtmodel.l] support :fat option in self-collision-check (#147)
k-okada Jul 20, 2015
a762650
[irteus/pqp.l] store fat value to pslot (https://github.com/euslisp/j…
k-okada Jul 20, 2015
4ab94ab
[test/geo.l] add test co check pqp collision/distance calculation time
k-okada Jul 20, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 11 additions & 2 deletions irteus/irtmodel.l
View file
Original file line number Diff line number Diff line change
@@ -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
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, I understand the motivation of this PR.
For this purpose, it would be better to another name for fat as you said.,
e.g., fat => min-distance, because fatting collision shape is not theoretically same as changing distance.

(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)
17 changes: 11 additions & 6 deletions irteus/pqp.l
View file
Original file line number Diff line number Diff line change
@@ -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)
))
36 changes: 36 additions & 0 deletions irteus/test/geo.l
View file
Original file line number Diff line number Diff line change
@@ -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)

8 changes: 8 additions & 0 deletions irteus/test/robot-model-usage.l
View file
Original file line number Diff line number Diff line change
@@ -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)