revert codes for collision model making according to euslisp/jskeus#93 …

…and jsk-ros-pkg#46
snozawa · Jun 10, 2014 · 26f1434 · 26f1434
1 parent 33c47d5
commit 26f1434
Show file tree

Hide file tree

Showing 2 changed files with 40 additions and 0 deletions.
diff --git a/euscollada/src/euscollada-robot.l b/euscollada/src/euscollada-robot.l
@@ -53,6 +53,26 @@
                       ))
                     args)
      ))
+  ;; make collision model from faces or gl-vertices
+  (:make-collision-model-for-links
+   (&key (fat 0) (collision-func 'pqp-collision-check) ((:links ls) (send self :links)))
+   (dolist (ll ls)
+     (unless (send ll :get (read-from-string (format nil ":~Amodel"
+                                                     (string-right-trim "-COLLISION-CHECK" (string collision-func)))))
+       (send ll
+             (read-from-string
+              (format nil ":make-~Amodel"
+                      (string-right-trim "-COLLISION-CHECK" (string collision-func))))
+             :fat fat
+             :faces (flatten (mapcar #'(lambda (x)
+                                         (cond
+                                          ((find-method x :def-gl-vertices)
+                                           (send (x . glvertices) :convert-to-faces :wrt :world))
+                                          (t
+                                           (send x :faces))))
+                                     (send ll :bodies)))))
+     )
+   )
   )
 
 ;; copy euscollada-body class definition from euscollada/src/euscollada-robot.l

diff --git a/euscollada/src/euscollada-robot_urdfmodel.l b/euscollada/src/euscollada-robot_urdfmodel.l
@@ -42,6 +42,26 @@
                       ))
                     args)
      ))
+  ;; make collision model from faces or gl-vertices
+  (:make-collision-model-for-links
+   (&key (fat 0) (collision-func 'pqp-collision-check) ((:links ls) (send self :links)))
+   (dolist (ll ls)
+     (unless (send ll :get (read-from-string (format nil ":~Amodel"
+                                                     (string-right-trim "-COLLISION-CHECK" (string collision-func)))))
+       (send ll
+             (read-from-string
+              (format nil ":make-~Amodel"
+                      (string-right-trim "-COLLISION-CHECK" (string collision-func))))
+             :fat fat
+             :faces (flatten (mapcar #'(lambda (x)
+                                         (cond
+                                          ((find-method x :def-gl-vertices)
+                                           (send (x . glvertices) :convert-to-faces :wrt :world))
+                                          (t
+                                           (send x :faces))))
+                                     (send ll :bodies)))))
+     )
+   )
   )
 
 (defclass collada-body