test-fetchfeus.l: add more test on *ri* :angle-vector for jsk-ros-pkg…

…#839, jsk-ros-pkg#841

jsk_fetch_robot/fetcheus/test/test-fetcheus.l

@@ -39,17 +39,69 @@
   (let (diff-av)
     (setq *ri* (instance fetch-interface :init))
     ;;
+    (format t "check reset-pose")
     (send *fetch* :reset-pose)
+    (send *ri* :robot :reset-pose)
     (send *ri* :angle-vector (send *fetch* :angle-vector))
     (send *ri* :wait-interpolation)
+    ;; do not care 360 rotaiton
     (setq diff-av (send *ri* :sub-angle-vector (send *ri* :state :potentio-vector) (send *fetch* :angle-vector)))
-    (assert (eps= (norm diff-av) 0.0 10.0) (format nil "diff-av ~A" diff-av))
+    (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send *ri* :robot :rarm :wrist-r :joint-angle) (send *fetch* :rarm :wrist-r :joint-angle)  (norm diff-av) (eps= (norm diff-av) 0.0 10.0))
+    (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":reset-pose, diff-av ~A" diff-av))
 
+    (format t "check :ik #f(800 0 1300), do not care about 360 degree rotation")
     (send *fetch* :inverse-kinematics (make-coords :pos #f(800 0 1300)))
     (send *ri* :angle-vector (send *fetch* :angle-vector))
     (send *ri* :wait-interpolation)
+    ;; do not care 360 rotaiton
     (setq diff-av (send *ri* :sub-angle-vector (send *ri* :state :potentio-vector) (send *fetch* :angle-vector)))
-    (assert (eps= (norm diff-av) 0.0 10.0) (format nil "diff-av ~A" diff-av))
+    (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send *ri* :robot :rarm :wrist-r :joint-angle) (send *fetch* :rarm :wrist-r :joint-angle)  (norm diff-av) (eps= (norm diff-av) 0.0 10.0))
+    (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":ik 800 0 1300, diff-av ~A" diff-av))
+
+    ;; do care 360 rotation
+    (format t "check :init-pose")
+    (send *fetch* :init-pose) ;; reset
+    (send *ri* :robot :init-pose)
+    (send *ri* :angle-vector (send *fetch* :angle-vector))
+    (send *ri* :wait-interpolation)
+    (setq diff-av (v- (send *ri* :state :potentio-vector) (send *fetch* :angle-vector)))
+    (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send *ri* :robot :rarm :wrist-r :joint-angle) (send *fetch* :rarm :wrist-r :joint-angle)  (norm diff-av) (eps= (norm diff-av) 0.0 10.0))
+    (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":init-pose, diff-av ~A" diff-av))
+
+    ;; 150 is 150
+    (format t "check :joint-angle 150, this should goes to 150")
+    (send *fetch* :rarm :wrist-r :joint-angle 150)
+    (send *ri* :angle-vector (send *fetch* :angle-vector))
+    (send *ri* :wait-interpolation)
+    (setq diff-av (v- (send *ri* :state :potentio-vector) (send *fetch* :angle-vector)))
+    (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send *ri* :robot :rarm :wrist-r :joint-angle) (send *fetch* :rarm :wrist-r :joint-angle)  (norm diff-av) (eps= (norm diff-av) 0.0 10.0))
+    (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":joint-angle 150, diff-av ~A" diff-av))
+
+    ;; from 150 to -150, robot moves to 210 by :angle-vector
+    (format t "check :joint-angle -150, this should goes to 210, by selecting minimum motion")
+    (send *fetch* :rarm :wrist-r :joint-angle -150)
+    (send *ri* :angle-vector (send *fetch* :angle-vector))
+    (send *ri* :wait-interpolation)
+    (setq diff-av (v- (send *ri* :state :potentio-vector) (send *fetch* :angle-vector)))
+    (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send *ri* :robot :rarm :wrist-r :joint-angle) (send *fetch* :rarm :wrist-r :joint-angle)  (norm diff-av) (eps= (norm diff-av) 0.0 10.0))
+    (assert (null (eps= (norm diff-av) 0.0 10.0)) (format nil ":joint-angle -150, diff-av ~A" diff-av))
+
+    (format t "check :joint-angle-sequence 150 0 -150, this should goes to -150")
+    ;; to send to -150, use :angle-vector-sequence
+    (send *ri* :angle-vector-sequence (list
+                                       (progn
+                                         (send *fetch* :rarm :wrist-r :joint-angle  150)
+                                         (send *fetch* :angle-vector))
+                                       (progn
+                                         (send *fetch* :rarm :wrist-r :joint-angle   0)
+                                         (send *fetch* :angle-vector))
+                                       (progn
+                                         (send *fetch* :rarm :wrist-r :joint-angle -150)
+                                         (send *fetch* :angle-vector))))
+    (send *ri* :wait-interpolation)
+    (setq diff-av (v- (send *ri* :state :potentio-vector) (send *fetch* :angle-vector)))
+    (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send *ri* :robot :rarm :wrist-r :joint-angle) (send *fetch* :rarm :wrist-r :joint-angle)  (norm diff-av) (eps= (norm diff-av) 0.0 10.0))
+    (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":angle-vector-sequence, diff-av ~A" diff-av))
     ))
 
 ;; https://github.com/jsk-ros-pkg/jsk_robot/pull/771/files