diff --git a/irteus/irtutil.l b/irteus/irtutil.l index 7643dc41..2e909d56 100644 --- a/irteus/irtutil.l +++ b/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)) ) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index c5906a2f..51d23e06 100644 --- a/irteus/test/interpolator.l +++ b/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")