Merge branch 'master' into pr2-launch-soundplay

.travis.rosinstall

@@ -1,8 +1,17 @@
+# see .travis.rosinstall.ROS_DISTRO
+
+# waiting for release 0.3.15
+# we need :go-pos-unsafe bug fix commit for fetch navigation test
+# see https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/425
+- git:
+    local-name: jsk-ros-pkg/jsk_pr2eus
+    uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git
+    version: 32f729d32bdb393fa325daaa0082a8a67930b2a9
 # see also .travis.rosinstall.ROS_DISTRO
 # waiting for jsk_maps release
 # add keepout map for entire eng2 building
 # https://github.com/jsk-ros-pkg/jsk_demos/pull/1300
-- git:
-    local-name: jsk-ros-pkg/jsk_demos
-    uri: https://github.com/jsk-ros-pkg/jsk_demos.git
-    version: master
+- tar:
+    local-name: jsk-ros-pkg/jsk_demos/jsk_maps
+    uri: https://github.com/tork-a/jsk_demos-release/archive/release/melodic/jsk_maps/0.0.5-1.tar.gz
+    version: jsk_demos-release-release-melodic-jsk_maps-0.0.5-1

jsk_baxter_robot/baxtereus/baxter-util.l

@@ -37,25 +37,42 @@
      (setq r (send-super* :inverse-kinematics target-coords :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args))
      (unless r ;;
        (format *error-output* "; failed for normal ik, starting from relaxed position~%")
-       (let  ((current-coords (send (cadr (memq :move-target args)) :copy-worldcoords)))
-         (send-super* :inverse-kinematics current-coords :rotation-axis nil :avoid-nspace-gain 0.1 :avoid-weight-gain 0.1 :stop 200 :avoid-collision-distance avoid-collision-distance args)
+       (let* ((move-target (cadr (memq :move-target args)))
+              (current-coords (if (listp move-target)
+                                (mapcar #'(lambda (x) (send x :copy-worldcoords)) move-target)
+                                (send move-target :copy-worldcoords)))
+              (rotation-axis (if (listp current-coords) (make-list (length current-coords)) nil)))
+         (send-super* :inverse-kinematics current-coords :move-target move-target :rotation-axis rotation-axis :avoid-nspace-gain 0.1 :avoid-weight-gain 0.1 :stop 200
+                      :avoid-collision-distance avoid-collision-distance args)
          (setq r (send-super* :inverse-kinematics target-coords :warnp warnp :dump-command dump-command args))
          (if (and (null r) (or (null (memq :revert-if-fail args)) (cadr (memq :revert-if-fail args))))
              ;; when fail and :rever-if-fail is nil
              (send self :angle-vector prev-av))
          ))
      (unless r ;;
        (format *error-output* "; failed for normal ik, try to move arms very slowly~%")
-       (let  ((step 0.0)
-              (current-coords (send (cadr (memq :move-target args)) :copy-worldcoords)))
+       (let* ((step 0.0)
+              (move-target (cadr (memq :move-target args)))
+              (current-coords (if (listp move-target)
+                                (mapcar #'(lambda (x) (send x :copy-worldcoords)) move-target)
+                                (send move-target :copy-worldcoords))))
          (setq r t)
          (while (and r (<= step 1.0))
-           (setq r (send-super* :inverse-kinematics (midcoords step current-coords target-coords) :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args))
-           (incf step 0.01))
+           (let ((mid-coords (if (listp current-coords)
+                               (mapcar #'(lambda (x) (midcoords step (car x) (cadr x)))
+                                       (mapcar #'list current-coords target-coords))
+                               (midcoords step current-coords target-coords))))
+             (setq r (send-super* :inverse-kinematics mid-coords
+                                  :move-target move-target :avoid-collision-distance avoid-collision-distance :warnp warnp :dump-command dump-command args))
+             (incf step 0.01)))
          (unless r (send self :angle-vector prev-av))))
      (unless r ;; start from ik-frendly position
        (format *error-output* "; failed for slow ik, try to start from prepared poses~%")
-       (let* ((move-joints (send-all (cadr (memq :link-list args)) :joint))
+       (let* ((link-list (cadr (memq :link-list args)))
+              (move-joints
+                (if (listp link-list)
+                  (apply #'append (mapcar #'(lambda (x) (send-all x :joint)) link-list))
+                  (send-all link-list :joint)))
               (av (mapcar #'(lambda (j) (send j :joint-angle)) (send self :joint-list)))
               (ik-prepared-poses (send self :ik-prepared-poses)))
          (dolist (pose ik-prepared-poses)

jsk_baxter_robot/baxtereus/test/test-baxter.l

@@ -50,38 +50,129 @@
 
 (setq *debug-view-p* (null (unix::getenv "TRAVIS_BRANCH")))
 
+
+(defun test-ik-bin (robot)
+  (send robot :locate #f(0 0 950) :world)
+  (when *debug-view-p*
+    (objects (list robot)))
+  (dolist (pos *pod-bin*)
+    (dolist (arm '(:larm :rarm))
+      (assert (send robot arm :inverse-kinematics
+                    (make-cascoords :pos (v+ pos #f(-90 0 0)))
+                    :rotation-axis t
+                    :debug-view (if *debug-view-p* :no-message nil))
+              "fail to solve ik")
+      (assert (send robot :inverse-kinematics
+                    (list (make-cascoords :pos (v+ pos #f(-90 0 0))))
+                    :move-target
+                    (list (send robot arm :end-coords))
+                    :link-list
+                    (list (send robot :link-list (send (send robot arm :end-coords) :parent)))
+                    :rotation-axis (list t)
+                    :debug-view (if *debug-view-p* :no-message nil))
+              "fail to solve ik"))))
+
 (deftest test-robot-ik-bin
   (let (robot)
     (setq robot (instance baxter-robot :init))
-    (send robot :locate #f(0 0 950) :world)
-    (when *debug-view-p*
-      (objects (list robot)))
-    (dolist (pos *pod-bin*)
-      (dolist (arm '(:larm :rarm))
-        (assert (send robot arm :inverse-kinematics
-                      (make-cascoords :pos (v+ pos #f(-90 0 0)))
-                      :rotation-axis t
-                      :debug-view (if *debug-view-p* :no-message nil))
-                "fail to solve ik")
-        )) ;; do list
-    )) ;; let
-
-(deftest test-robot-ik-safe-bin
+    (test-ik-bin robot)))
+
+(deftest test-robot-safe-ik-bin
+  (let (robot)
+    (setq robot (instance baxter-robot-safe :init))
+    (test-ik-bin robot)))
+
+(defun test-multi-ik-bin (robot)
+  (send robot :locate #f(0 0 950) :world)
+  (when *debug-view-p*
+    (objects (list robot)))
+  (dolist (pos *pod-bin*)
+    (assert (send robot :inverse-kinematics
+                  (list (make-cascoords :pos (v+ pos #f(-90 0 0)))
+                        (make-cascoords :pos (v+ pos #f(-90 0 0))))
+                  :move-target
+                  (list (send robot :larm :end-coords)
+                        (send robot :rarm :end-coords))
+                  :link-list
+                  (list (send robot :link-list (send (send robot :larm :end-coords) :parent))
+                        (send robot :link-list (send (send robot :rarm :end-coords) :parent)))
+                  :rotation-axis (list t t)
+                  :debug-view (if *debug-view-p* :no-message nil))
+            "fail to solve ik")))
+
+(deftest test-robot-multi-ik-bin
+  (let (robot)
+    (setq robot (instance baxter-robot :init))
+    (test-multi-ik-bin robot)))
+
+(deftest test-robot-safe-multi-ik-bin
+  (let (robot)
+    (setq robot (instance baxter-robot-safe :init))
+    (test-multi-ik-bin robot)))
+
+(defun test-ik-fail (robot)
+  (send robot :locate #f(0 0 950) :world)
+  (when *debug-view-p*
+    (objects (list robot)))
+  (dolist (arm (list :larm :rarm))
+    (assert
+      (not (send robot :inverse-kinematics
+                 (list (make-cascoords :pos #f(0 0 0)))
+                 :move-target
+                 (list (send robot arm :end-coords))
+                 :link-list
+                 (list (send robot :link-list (send (send robot arm :end-coords) :parent)))
+                 :rotation-axis (list t)
+                 :debug-view (if *debug-view-p* :no-message nil)))
+            "ik calculation failure")
+    (assert
+      (not (send robot arm :inverse-kinematics
+                 (make-cascoords :pos #f(0 0 0))
+                 :move-target
+                 (send robot arm :end-coords)
+                 :link-list
+                 (send robot :link-list (send (send robot arm :end-coords) :parent))
+                 :rotation-axis (list t)
+                 :debug-view (if *debug-view-p* :no-message nil)))
+            "ik calculation failure")))
+
+(deftest test-robot-ik-fail
+  (let (robot)
+    (setq robot (instance baxter-robot :init))
+    (test-ik-fail robot)))
+
+(deftest test-robot-safe-ik-fail
+  (let (robot)
+    (setq robot (instance baxter-robot-safe :init))
+    (test-ik-fail robot)))
+
+(defun test-multi-ik-fail (robot)
+  (send robot :locate #f(0 0 950) :world)
+  (when *debug-view-p*
+    (objects (list robot)))
+  (assert
+    (not (send robot :inverse-kinematics
+               (list (make-cascoords :pos #f(0 0 0))
+                     (make-cascoords :pos #f(0 0 0)))
+               :move-target
+               (list (send robot :larm :end-coords)
+                     (send robot :rarm :end-coords))
+               :link-list
+               (list (send robot :link-list (send (send robot :larm :end-coords) :parent))
+                     (send robot :link-list (send (send robot :rarm :end-coords) :parent)))
+               :rotation-axis (list t t)
+               :debug-view (if *debug-view-p* :no-message nil)))
+          "ik calculation failure"))
+
+(deftest test-robot-multiple-ik-fail
+  (let (robot)
+    (setq robot (instance baxter-robot :init))
+    (test-multi-ik-fail robot)))
+
+(deftest test-robot-safe-multiple-ik-fail
   (let (robot)
     (setq robot (instance baxter-robot-safe :init))
-    (send robot :locate #f(0 0 950) :world)
-    (when *debug-view-p*
-      (objects (list robot)))
-    (dolist (pos *pod-bin*)
-      (dolist (arm '(:larm :rarm))
-        (send robot :reset-pose)
-        (assert (send robot arm :inverse-kinematics
-                      (make-cascoords :pos (v+ pos #f(-90 0 0)))
-                      :rotation-axis t
-                      :debug-view (if *debug-view-p* :no-message nil))
-                "fail to solve ik")
-        )) ;; do list
-    )) ;; let
+    (test-multi-ik-fail robot)))
 
 ;; test to check the robot back to the original pose, if the ik failed
 (deftest test-robot-ik-revert
@@ -97,109 +188,65 @@
     (assert (eps-v= (send robot :angle-vector) av 5) "unable to revert after ik failed")
     ))
 
+
+(defun test-ik-bin-cube (robot)
+  (send (send robot :rarm :end-coords)
+        :newcoords (make-coords :pos #f(50 0 310)
+                                :rpy (float-vector 0 -pi/2 0)))
+  (send (send robot :larm :end-coords)
+        :newcoords (make-coords :pos #f(50 0 310)
+                                :rpy (float-vector 0 -pi/2 0)))
+  (send robot :locate #f(0 0 950) :world)
+  (when *debug-view-p*
+    (objects (append (list robot) *pod-bin-cube*)))
+  (setq step-dist 200)
+  (dolist (cube-data (mapcar #'list *pod-bin-ik-limit* *pod-bin-cube*))
+    (setq limit (car cube-data))
+    (setq cube (cadr cube-data))
+    (setq x (elt (send (send cube :worldcoords) :pos) 0))
+    (setq y (elt (send (send cube :worldcoords) :pos) 1))
+    (setq z (elt (send (send cube :worldcoords) :pos) 2))
+    (setq dx (x-of-cube cube))
+    (setq dy (y-of-cube cube))
+    (setq dz (z-of-cube cube))
+    (do ((tmp-x (+ (- x (/ dx 2)) (/ step-dist 2)) (+ tmp-x step-dist)))
+      ((> tmp-x (+ x (/ dx 2))) t)
+      (do ((tmp-y (+ (- y (/ dy 2)) (/ step-dist 2)) (+ tmp-y step-dist)))
+        ((> tmp-y (+ y (/ dy 2))) t)
+        (do ((tmp-z (+ (- z (/ dz 2)) (/ step-dist 2)) (+ tmp-z step-dist)))
+          ((> tmp-z (+ z (/ dz 2))) t)
+          (setq pos (float-vector tmp-x tmp-y tmp-z))
+          (assert
+            (or
+              (send robot :rarm :inverse-kinematics
+                    (list (make-cascoords :pos pos))
+                    :move-target (list (send robot :larm :end-coords))
+                    :link-list
+                    (list (send robot :link-list (send (send robot :larm :end-coords) :parent)))
+                    :rotation-axis (list t)
+                    :debug-view (if *debug-view-p* :no-message nil))
+              (send robot :inverse-kinematics
+                    (list (make-cascoords :pos pos))
+                    :move-target (list (send robot :rarm :end-coords))
+                    :link-list
+                    (list (send robot :link-list (send (send robot :rarm :end-coords) :parent)))
+                    :rotation-axis (list t)
+                    :debug-view (if *debug-view-p* :no-message nil))
+              (> tmp-x limit))
+            "fail to solve ik"))))))
+
+
 (deftest test-robot-ik-bin-cube
   (let (robot)
     (setq robot (instance baxter-robot :init))
-    (send (send robot :rarm :end-coords)
-          :newcoords (make-coords :pos #f(50 0 310)
-                                  :rpy (float-vector 0 -pi/2 0)))
-    (send (send robot :larm :end-coords)
-          :newcoords (make-coords :pos #f(50 0 310)
-                                  :rpy (float-vector 0 -pi/2 0)))
-    (send robot :locate #f(0 0 950) :world)
-    (when *debug-view-p*
-      (objects (append (list robot) *pod-bin-cube*)))
-    (setq step-dist 200)
-    (dolist (cube-data (mapcar #'list *pod-bin-ik-limit* *pod-bin-cube*))
-      (setq limit (car cube-data))
-      (setq cube (cadr cube-data))
-      (setq x (elt (send (send cube :worldcoords) :pos) 0))
-      (setq y (elt (send (send cube :worldcoords) :pos) 1))
-      (setq z (elt (send (send cube :worldcoords) :pos) 2))
-      (setq dx (x-of-cube cube))
-      (setq dy (y-of-cube cube))
-      (setq dz (z-of-cube cube))
-      (do ((tmp-x (+ (- x (/ dx 2)) (/ step-dist 2)) (+ tmp-x step-dist)))
-        ((> tmp-x (+ x (/ dx 2))) t)
-        (do ((tmp-y (+ (- y (/ dy 2)) (/ step-dist 2)) (+ tmp-y step-dist)))
-          ((> tmp-y (+ y (/ dy 2))) t)
-          (do ((tmp-z (+ (- z (/ dz 2)) (/ step-dist 2)) (+ tmp-z step-dist)))
-            ((> tmp-z (+ z (/ dz 2))) t)
-            (setq pos (float-vector tmp-x tmp-y tmp-z))
-            (assert
-              (or
-                (send robot :rarm :inverse-kinematics
-                  (make-cascoords :pos pos)
-                  :rotation-axis t
-                  :debug-view (if *debug-view-p* :no-message nil))
-                (send robot :larm :inverse-kinematics
-                  (make-cascoords :pos pos)
-                  :rotation-axis t
-                  :debug-view (if *debug-view-p* :no-message nil))
-                (> tmp-x limit))
-              "fail to solve ik")
-            )
-          )
-        )
-      )
-    )
-  )
+    (test-ik-bin-cube robot)))
+
 
 (deftest test-robot-ik-safe-bin-cube
   (let (robot)
     (setq robot (instance baxter-robot-safe :init))
-    (send (send robot :rarm :end-coords)
-          :newcoords (make-coords :pos #f(50 0 310)
-                                  :rpy (float-vector 0 -pi/2 0)))
-    (send (send robot :larm :end-coords)
-          :newcoords (make-coords :pos #f(50 0 310)
-                                  :rpy (float-vector 0 -pi/2 0)))
-    (send robot :locate #f(0 0 950) :world)
-    (when *debug-view-p*
-      (objects (append (list robot) *pod-bin-cube*)))
-    (setq step-dist 200)
-    (dolist (cube-data (mapcar #'list *pod-bin-ik-limit* *pod-bin-cube*))
-      (setq limit (car cube-data))
-      (setq cube (cadr cube-data))
-      (setq x (elt (send (send cube :worldcoords) :pos) 0))
-      (setq y (elt (send (send cube :worldcoords) :pos) 1))
-      (setq z (elt (send (send cube :worldcoords) :pos) 2))
-      (setq dx (x-of-cube cube))
-      (setq dy (y-of-cube cube))
-      (setq dz (z-of-cube cube))
-      (do ((tmp-x (+ (- x (/ dx 2)) (/ step-dist 2)) (+ tmp-x step-dist)))
-        ((> tmp-x (+ x (/ dx 2))) t)
-        (do ((tmp-y (+ (- y (/ dy 2)) (/ step-dist 2)) (+ tmp-y step-dist)))
-          ((> tmp-y (+ y (/ dy 2))) t)
-          (do ((tmp-z (+ (- z (/ dz 2)) (/ step-dist 2)) (+ tmp-z step-dist)))
-            ((> tmp-z (+ z (/ dz 2))) t)
-            (setq pos (float-vector tmp-x tmp-y tmp-z))
-            (assert
-              (or
-                (and
-                  (send robot :reset-pose)
-                  (send robot :rarm :inverse-kinematics
-                    (make-cascoords :pos pos)
-                    :rotation-axis t
-                    :debug-view (if *debug-view-p* :no-message nil))
-                  )
-                (and
-                  (send robot :reset-pose)
-                  (send robot :larm :inverse-kinematics
-                    (make-cascoords :pos pos)
-                    :rotation-axis t
-                    :debug-view (if *debug-view-p* :no-message nil))
-                  )
-                (> tmp-x limit)
-                )
-                "fail to solve ik"
-              )
-            )
-          )
-        )
-      )
-    )
-  )
+    (test-ik-bin-cube robot)))
+
 
 (deftest test-robot-l/r-reverse
   (let (robot reversed)
@@ -209,6 +256,7 @@
       (assert (= (elt (send robot :rarm :angle-vector) i) (elt reversed i))))
     ))
 
+
 (when (string> (unix::getenv "ROS_DISTRO") "hydro")
 (load "package://baxtereus/baxter-interface.l")
 (deftest test-baxter-interface