Merge pull request #596 from k-okada/fix_minjerk_vel_acc

fix minjerk interpolation sometimes returns large acceleration

Author: k-okada

irteus/irtutil.l

@@ -294,11 +294,18 @@
      (setq position (send self :interpolation))
      (incf time dt)
      (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list))))
-     (when (> time (nth segment time-list))
-       (setq segment-time (- time (nth segment time-list)))
-       (incf segment))
+     (when (eps> time (nth segment time-list) (* 0.1 dt))
+       ;; if time-segment is not aligned, need to fix the data (see https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457)
+       (while (and (< segment segment-num) (eps> time (nth segment time-list) (* 0.1 dt)))
+         (setq segment-time (- time (nth segment time-list)))
+         (incf segment)))
      (when (>= segment segment-num)
-       (setq position (car (last position-list)))
+       ;; adjust time and segment-time to exact position
+       (setq segment (1- segment-num))
+       (setq time (car (last time-list)))
+       (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list))))
+       ;; re-calculate :interpolation
+       (setq position (send self :interpolation))
        (send self :reset))
      position))
   )

irteus/test/interpolator.l

@@ -57,12 +57,46 @@
         ((:interpolator ip) (instance interpolator-class :init))
         (initial-time 0.0) (neglect-first) (vel-vector-list) (acc-vector-list))
   (let* ((data-list) (tm-list) (vel-data-list) (acc-data-list))
+    (assert (= (length pos-list) (1+ (length time-list)))
+            (format nil "check length of pos-list(~A) and tm-list(~A)"
+                    (length pos-list) (length time-list)))
+    (setq vel-vector-list
+          (reverse
+           (do ((i 0 (1+ i)) (vel-list))
+               ((> i (length time-list)) vel-list)
+               (if (or (= i 0) (= i (length time-list)))
+                   (push (instantiate float-vector (length (car pos-list))) vel-list)
+                 (let* ((v0 (scale (/ 1.0  (elt time-list (1- i)))
+                                   (v- (elt pos-list i) (elt pos-list (1- i)))))
+                        (v1 (scale (/ 1.0 (elt time-list i))
+                                   (v- (elt pos-list (1+ i)) (elt pos-list i))))
+                        (v (scale 0.5 (v+ v0 v1))))
+                   (dotimes (i (length v)) (if (< (* (elt v0 i) (elt v1 i)) 0) (setf (elt v i) 0)))
+                   (push v vel-list))))))
+    (setq acc-vector-list
+          (reverse
+           (do ((i 0 (1+ i)) (acc-list))
+               ((> i (length time-list)) acc-list)
+               (if (or (= i 0) (= i (length time-list)))
+                   (push (instantiate float-vector (length (car vel-vector-list))) acc-list)
+                 (let* ((v0 (scale (/ 1.0  (elt time-list (1- i)))
+                                   (v- (elt vel-vector-list i) (elt vel-vector-list (1- i)))))
+                        (v1 (scale (/ 1.0 (elt time-list i))
+                                   (v- (elt vel-vector-list (1+ i)) (elt vel-vector-list i))))
+                        (v (scale 0.5 (v+ v0 v1))))
+                   (dotimes (i (length v)) (if (< (* (elt v0 i) (elt v1 i)) 0) (setf (elt v i) 0)))
+                   (push v acc-list))))))
+    (format t "=INPUT~%")
+    (format t "time ~A~%" time-list)
+    (format t " pos ~A~%" pos-list)
+    (format t " vel ~A~%" vel-vector-list)
+    (format t " acc ~A~%" acc-vector-list)
     (send* ip :reset
            :position-list pos-list
            :time-list (let (r) (dolist (n time-list) (push (+ n (if r (car r) 0)) r)) (nreverse r)) ;; list of time[sec] from start for each control point
            (append
-            (if vel-vector-list (list :vel-vector-list vel-vector-list))
-            (if acc-vector-list (list :acc-vector-list acc-vector-list))))
+            (if vel-vector-list (list :velocity-list vel-vector-list))
+            (if acc-vector-list (list :acceleration-list acc-vector-list))))
     (send ip :start-interpolation)
     (while (send ip :interpolatingp)
       (push (if (send ip :interpolatingp)
@@ -73,6 +107,12 @@
       (if (find-method ip :velocity) (push (send ip :velocity) vel-data-list))
       (if (find-method ip :acceleration) (push (send ip :acceleration) acc-data-list))
       )
+    (format t "=OUTPUT~%")
+    (if (and vel-data-list acc-data-list)
+        (mapcar #'(lambda (tm pos vel acc)
+                    (format t "~7,5f ~7,3f ~13,1f ~13,1f~%"
+                            tm (elt pos 0) (elt vel 0) (elt acc 0)))
+                (reverse tm-list) (reverse data-list) (reverse vel-data-list) (reverse acc-data-list)))
     (append
      (list :data (if neglect-first (cdr (reverse data-list)) (reverse data-list))
            :time (if neglect-first (cdr (reverse tm-list)) (reverse tm-list)))
@@ -99,12 +139,15 @@
                                      ip)))
            ))
          (ret-list2 (mapcar #'(lambda (x) (elt x 0)) (cadr (memq :data ret-list)))))
-    ;; (unless (or (null x::*display*) (= x::*display* 0))
-    ;;   (graph-view
-    ;;    (list ret-list2)
-    ;;    (cadr (memq :time ret-list))
-    ;;    :title (format nil "~A interpolator" (send ip-class :name))
-    ;;    :xlabel "time [s]" :keylist (list "")))
+    (when (and (not (or (null x::*display*) (= x::*display* 0)))
+               (functionp 'graph-view))
+      (let ((r-pos (mapcar #'(lambda (x) (elt x 0)) (cadr (memq :data ret-list))))
+            (r-vel (mapcar #'(lambda (x) (* 10 (elt x 0))) (cadr (memq :velocity ret-list))))
+            (r-acc (mapcar #'(lambda (x) (* 100 (elt x 0))) (cadr (memq :acceleration ret-list)))))
+        (graph-view (list r-pos r-vel r-acc) (cadr (memq :time ret-list))
+                    :keylist (list "position" "velocity" "acceleration")
+                    :mode "linespoints")
+        ))
 
     ;; check velocitiy
     (when (assoc :velocity (send ip-class :methods))
@@ -137,12 +180,83 @@
       (< (reduce #'(lambda (x y) (max x y)) ret-list2) (+ 90 *epsilon*))))
     ))
 
+;; https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457
+(defun test-interpolators-457
+  (&optional (ip-class linear-interpolator) (dt 0.0001))
+  (let* ((ret-list
+          (pos-list-interpolation
+           (list #f(0) #f(1) #f(2) #f(3) #f(4) #f(5) #f(6) #f(7) #f(8) #f(9))
+           (list 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001 0.001)
+           dt
+           :interpolator-class ip-class
+           )))
+    ;; check velocitiy
+    (let ((position (cadr (memq :data ret-list)))
+          (velocity (cadr (memq :velocity ret-list)))
+          real-vel calc-vel)
+      ;; check first and last velocity
+      (assert (eps= (norm (car velocity)) 0.0 (if (memq :word-size=64 *features*) *epsilon* 100))
+	      (format nil "vel:~A~%" (car velocity)))
+      (assert (eps= (norm (car (last velocity))) 0.0 (if (memq :word-size=64 *features*) *epsilon* 100))
+	      (format nil "vel:~A~%" (car (last velocity))))
+      ;; check velocity of stable region
+      (dotimes (i (1- (length position)))
+        (when (and (> i (max 2 (* 0.3 (length position))))
+                   (< i (min (- (length position) 3) (* 0.7 (length position)))))
+          (setq real-vel (scale (/ 1.0 dt) (v- (elt position (1+ i)) (elt position i)))
+                calc-vel (elt velocity i))
+          (assert (eps= (norm (v- real-vel calc-vel)) 0 (if (memq :word-size=64 *features*) *epsilon* 100))
+                  (format nil "~A pos: ~A, vel:~A, vel:~A, diff:~A~%" i (elt position i) real-vel calc-vel (norm (v- real-vel calc-vel)))))))
+
+    ;; check acceleration
+    (let ((velocity (cadr (memq :velocity ret-list)))
+          (acceleration (cadr (memq :acceleration ret-list)))
+          real-acc calc-acc)
+      ;; check first and last acceleration
+      (assert (eps= (norm (car acceleration)) 0.0 (if (memq :word-size=64 *features*) *epsilon* 100))
+	      (format nil "acc:~A~%" (car acceleration)))
+      (assert (eps= (norm (car (last acceleration))) 0.0 (if (memq :word-size=64 *features*) *epsilon* 100))
+	      (format nil "acc:~A~%" (car (last acceleration))))
+      ;; check maximum acceleration
+      (dotimes (i (1- (length velocity)))
+        (when (and (> i (max 2 (* 0.3 (length velocity))))
+                   (< i (min (- (length velocity) 3) (* 0.7 (length velocity)))))
+          (setq real-acc (scale (/ 1.0 dt) (v- (elt velocity (1+ i)) (elt velocity i)))
+                calc-acc (elt acceleration i))
+          (assert (eps= (norm (v- real-acc calc-acc)) 0 (if (memq :word-size=64 *features*) *epsilon* 100))
+                  (format nil "~A vel: ~A, acc:~A, acc:~A, diff:~A~%" i (elt velocity i) real-acc calc-acc (norm (v- real-acc calc-acc)))))
+        ))
+
+    (when (and (not (or (null x::*display*) (= x::*display* 0)))
+               (functionp 'graph-view))
+      (let ((r-pos (mapcar #'(lambda (x) (elt x 0)) (cadr (memq :data ret-list))))
+            (r-vel (mapcar #'(lambda (x) (* 0.001 (elt x 0))) (cadr (memq :velocity ret-list))))
+            (r-acc (mapcar #'(lambda (x) (* 0.000001 (elt x 0))) (cadr (memq :acceleration ret-list)))))
+        (graph-view (list r-pos r-vel r-acc) (cadr (memq :time ret-list))
+                    :keylist (list "position" "velocity" "acceleration")
+                    :mode "linespoints")
+        (unix:usleep (* 500 1000))
+        ))
+    ))
+
 (deftest test-linear-interpolator ()
   (let ((res (test-interpolators linear-interpolator)))))
 
 (deftest test-minjerk-absolute-interpolator ()
   (let ((res (test-interpolators minjerk-interpolator)))))
 
+(deftest test-minjerk-absolute-interpolator-457 ()
+  (let ((res (test-interpolators-457 minjerk-interpolator)))))
+
+(deftest test-minjerk-absolute-interpolator-457-00013 ()
+  (let ((res (test-interpolators-457 minjerk-interpolator 0.00013)))))
+
+(deftest test-minjerk-absolute-interpolator-457-0013 ()
+  (let ((res (test-interpolators-457 minjerk-interpolator 0.0013)))))
+
+(deftest test-minjerk-absolute-interpolator-457-0005 ()
+  (let ((res (test-interpolators-457 minjerk-interpolator 0.0005)))))
+
 
 #|
 (load "~/prog/euslib/jsk/gnuplotlib.l")