Merge pull request #571 from k-okada/error-check

[:self-collision-check] ignore links which do not have :faces

Author: k-okada

irteus/CBULLET.cpp

@@ -117,6 +117,9 @@ long BT_MakeCapsuleModel(double radius, double height)
 long BT_MakeMeshModel(double *verticesPoints, long numVertices)
 {
   btConvexHullShape* pshape = new btConvexHullShape();
+  if (numVertices == 0) {
+    fprintf(stderr, "BT_MakeMeshModel with numVertices == 0\n");
+  }
 #define SHRINK_FOR_MARGIN false
   if (SHRINK_FOR_MARGIN) {
     // Shrink vertices for default margin CONVEX_DISTANCE_MARGIN,

irteus/bullet.l

@@ -55,8 +55,8 @@
            (setq m
                  (btmakemeshmodel
                   (scale 1e-3 ;; [m]
-                         (apply #'concatenate float-vector
-                                (mapcar #'(lambda (v) (send b :inverse-transform-vector v)) (send b :vertices))))
+                         (apply #'float-vector (apply #'concatenate cons
+                                (mapcar #'(lambda (v) (send b :inverse-transform-vector v)) (send b :vertices)))))
                   (length (send b :vertices))
                   ))
            (if (> margin 0) (btsetmargin m (* 1e-3 margin)))
@@ -74,8 +74,8 @@
             (setq m
                   (btmakemeshmodel
                    (scale 1e-3 ;; [m]
-                          (apply #'concatenate float-vector
-                                 (mapcar #'(lambda (v) (send self :inverse-transform-vector v)) vs)))
+                          (apply #'float-vector (apply #'concatenate cons
+                                 (mapcar #'(lambda (v) (send self :inverse-transform-vector v)) vs))))
                    (length vs)
                    ))
             (btsetmargin m fat)
@@ -89,8 +89,8 @@
             (setq m
                   (btmakemeshmodel
                    (scale 1e-3 ;; [m]
-                          (apply #'concatenate float-vector
-                                 (mapcar #'(lambda (v) (send self :inverse-transform-vector v)) vs)))
+                          (apply #'float-vector (apply #'concatenate cons
+                                 (mapcar #'(lambda (v) (send self :inverse-transform-vector v)) vs))))
                    (length vs)
                    ))
             (btsetmargin m fat)

irteus/irtmodel.l

@@ -2661,15 +2661,25 @@
      pairs))
   (:self-collision-check
    (&key (mode :all) (pairs (send self :collision-check-pairs)) (collision-func 'collision-check))
-   (let ((cpairs) (col-count 0))
+   (let ((cpairs) (col-count 0) (invalid-links))
      (dolist (p pairs)
-       (let ((colp (/= (funcall collision-func (car p) (cdr p)) 0)))
-	 (when colp
-	   (incf col-count)
-	   (if (eq mode :first)
-	       (return-from :self-collision-check p)
-	     (push p cpairs)))
-	 ))
+       (if (and (send (car p) :faces) (send (cdr p) :faces))
+           (let ((colp (/= (funcall collision-func (car p) (cdr p)) 0)))
+             (when colp
+               (incf col-count)
+               (if (eq mode :first)
+                   (return-from :self-collision-check p)
+                 (push p cpairs))))
+         (progn
+           (when (and (null (send (car p) :faces)) (null (member (car p) invalid-links)))
+             (warning-message 3 "skip collision between ~A (~A) because of no faces~%"
+                          (send (car p) :name) (length (send (car p) :faces)))
+             (push (car p) invalid-links))
+           (when (and (null (send (cdr p) :faces)) (null (member (cdr p) invalid-links)))
+             (warning-message 3 "skip collision between ~A (~A) because of no faces~%"
+                              (send (cdr p) :name) (length (send (cdr p) :faces)))
+             (push (cdr p) invalid-links)))
+         ))
      cpairs))
   (:calc-grasp-matrix
    (contact-points

irteus/test/test-collision.l

@@ -194,6 +194,22 @@
      )
     ))
 
+(load "models/darwin.l")
+;; use add no-face link for euscollada-robot, use darwin-robot to override
+(defmethod darwin-robot
+  ;; make collision model from faces or gl-vertices
+  (:make-collision-model-for-links
+   (&key (fat 0) ((:links ls) (send self :links)))
+   ;;
+   ;; for append camera links for test code
+   (let (l)
+     (setq l (instance bodyset-link :init (make-cascoords) :name "dummy-link"))
+     (send (car links) :add-child-links l)
+     (setq links (append links (list l)))
+     (setq ls (send self :links)))
+   )
+  )
+
 (when *collision-algorithm-pqp*
 
 (deftest test-collision-sphere-analytical-pqp
@@ -249,6 +265,7 @@
   (test-collision-distance-fat
    (body+ (make-cube 200 200 100) (make-cube 100 100 300))
    (body+ (make-cube 200 200 100) (make-cube 100 100 300))))
+
 )
 
 (when *collision-algorithm-bullet*
@@ -309,6 +326,24 @@
 
 )
 
+;; not sure why, but put this function within (when *collision-algorithm-pqp* causes errors...
+(deftest test-self-collision-check-pqp
+  (when *collision-algorithm-pqp*
+    (setq *collision-algorithm* *collision-algorithm-pqp*)
+    (let (robot)
+      (setq robot (instance darwin-robot :init))
+      (send robot :self-collision-check)
+      )))
+
+
+(deftest test-self-collision-check-bullet
+  (when  *collision-algorithm-bullet*
+    (setq *collision-algorithm* *collision-algorithm-bullet*)
+    (let (robot)
+      (setq robot (instance darwin-robot :init))
+      (send robot :self-collision-check)
+      )))
+
 (eval-when (load eval)
   (run-all-tests)
   (exit 0))