change how to calclate diff-av

jsk-ros-pkg · Sep 30, 2017 · 2e79cac · 2e79cac
1 parent 24cca52
commit 2e79cac
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/jsk_fetch_robot/fetcheus/test/test-fetcheus.l b/jsk_fetch_robot/fetcheus/test/test-fetcheus.l
@@ -43,13 +43,15 @@
     (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)))
-    (assert (eps= (norm diff-av) 0.0 5.0) (format nil "diff-av ~A" diff-av))
+    (setq diff-av (concatenate float-vector (subseq diff-av 0 1) (map float-vector #'(lambda (x) (cond ((> x 180) (- x 360)) ((< x -180) (+ x 360)) (t x))) (subseq diff-av 1))))
+    (assert (eps= (norm diff-av) 0.0 10.0) (format nil "diff-av ~A" diff-av))
 
     (send *fetch* :inverse-kinematics (make-coords :pos #f(800 0 1300)))
     (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)))
-    (assert (eps= (norm diff-av) 0.0 5.0) (format nil "diff-av ~A" diff-av))
+    (setq diff-av (concatenate float-vector (subseq diff-av 0 1) (map float-vector #'(lambda (x) (cond ((> x 180) (- x 360)) ((< x -180) (+ x 360)) (t x))) (subseq diff-av 1))))
+    (assert (eps= (norm diff-av) 0.0 10.0) (format nil "diff-av ~A" diff-av))
     ))
 
 ;; https://github.com/jsk-ros-pkg/jsk_robot/pull/771/files