Merge pull request jsk-ros-pkg#323 from k-okada/fix_849

fix sub-angle-vector when diff is over 640
k-okada · Oct 8, 2017 · c3232bd · c3232bd
2 parents f64ac50 + 085ab8c
commit c3232bd
Show file tree

Hide file tree

Showing 2 changed files with 67 additions and 2 deletions.
diff --git a/pr2eus/robot-interface.l b/pr2eus/robot-interface.l
@@ -75,6 +75,7 @@
         (when (> current-time tm)
           (pop timer-sequence)
           (pop angle-vector-sequence)
+          (setq av (v+ previous-angle-vector scale-av))
           (setq previous-angle-vector av)
           (setq current-time 0)
           )
@@ -255,7 +256,6 @@
   (:robot-interface-simulation-callback ()
    (let* ((joint-list (send robot :joint-list))
           (all-joint-names (send-all joint-list :name)))
-     (send self :publish-joint-state)
      (dolist (param (send self controller-type)) ;; assuming all action is stored in controller-type (:default-controller)
        ;(print (list 'p param))
        (let* ((joint-names (cdr (assoc :joint-names param)))
@@ -269,6 +269,7 @@
                  (let ((i (position j all-joint-names :test #'string=)))
                    (send (elt joint-list i) :joint-angle (elt av i))))))
          ))
+     (send self :publish-joint-state)
      ;;
      (if viewer (send self :draw-objects))
      ))
@@ -892,7 +893,8 @@ Return value is a list of interpolatingp for all controllers, so (null (some #'i
 	 (joint-list (send robot :joint-list))
 	 (i 0) j)
      (while (setq j (pop joint-list))
-       (if (and (= (send j :min-angle) *-inf*) (= (send j :max-angle) *inf*))
+       (when (and (= (send j :min-angle) *-inf*) (= (send j :max-angle) *inf*))
+	   (setf (elt ret i) (mod (elt ret i) 360.0))
 	   (cond ((> (elt ret i) 180.0)
 		  (setf (elt ret i) (- (elt ret i) 360.0)))
 		 ((< (elt ret i) -180.0)

diff --git a/pr2eus/test/pr2-ri-test-simple.l b/pr2eus/test/pr2-ri-test-simple.l
@@ -93,6 +93,69 @@
     (send *pr2* :reset-pose)
     (assert (send *ri* :angle-vector-sequence (list (send *pr2* :angle-vector))))))
 
+;;
+;; https://github.com/jsk-ros-pkg/jsk_robot/pull/849#issuecomment-334906516
+;;
+;; add test to check sub-angle-vector over 620
+(deftest test-sub-angle-vector-1000
+  (let (angle diff msg)
+    ;;(setq *ri* (instance pr2-interface :init))
+    (send *pr2* :reset-manip-pose)
+    (send *ri* :robot :reset-manip-pose)
+    (dolist (inc (list 10 -10))
+      (send *ri* :robot :larm :elbow-r :joint-angle 0)
+      (setq angle 0.0)
+      (setq diff 0.0)
+      (do ((i 0 (incf i inc)))
+          ((if (> inc 0) (<= 1000 i) (>= -1000 i)))
+          (send *pr2* :larm :elbow-r :joint-angle i)
+          (setq diff (elt (send *ri* :sub-angle-vector (send *pr2* :angle-vector) (send *ri* :robot :angle-vector)) 5))
+          (setq msg (format nil ":sub-angle-vector : original angle-vector from ~A to ~A, expected ~A ~A~%" i diff angle (eps= angle diff)))
+          (warning-message 2 msg)
+          (assert (eps= angle diff) msg)
+          (incf angle inc)
+          (if (>= angle 185.0) (setq angle -170.0))
+          (if (<= angle -185.0) (setq angle 170.0))
+          ))
+    ))
+
+(deftest test-angle-vector-over-640
+  (let (msg)
+    (send *pr2* :reset-manip-pose)
+    (send *ri* :robot :reset-manip-pose)
+    (dolist (angle (list 60 420 780))
+      (send *pr2* :larm :elbow-r :joint-angle angle)
+      (send-message *ri* robot-interface :angle-vector (send *pr2* :angle-vector) 500)
+      (send *ri* :wait-interpolation)
+      (setq msg
+            (format nil "*ri* ~A, *pr2* ~A ~A~%"
+                    (elt (send *ri* :state :potentio-vector) 5)
+                    (elt (send *pr2* :angle-vector) 5)
+                    (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0)))
+      (warning-message 2 msg)
+      (assert (eps= (elt (send *ri* :state :potentio-vector) 5) 60.0) msg)
+      )
+    ))
+
+(deftest test-angle-vector-under-640
+  (let (msg)
+    (send *pr2* :reset-manip-pose)
+    (send *ri* :robot :reset-manip-pose)
+    (dolist (angle (list -60 -420 -780))
+      (send *pr2* :larm :elbow-r :joint-angle angle)
+      (send-message *ri* robot-interface :angle-vector (send *pr2* :angle-vector) 500)
+      (send *ri* :wait-interpolation)
+      (setq msg
+            (format nil "*ri* ~A, *pr2* ~A ~A~%"
+                    (elt (send *ri* :state :potentio-vector) 5)
+                    (elt (send *pr2* :angle-vector) 5)
+                    (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0)))
+      (warning-message 2 msg)
+      (assert (eps= (elt (send *ri* :state :potentio-vector) 5) -60.0) msg)
+      )
+    ))
+
+
 (run-all-tests)
 (exit)