From b7fa9f39293f3bd161f67ecfc48a850d86f4c38a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 31 Dec 2020 15:25:41 +0900 Subject: [PATCH 1/4] use :acceleration and :velocity if defined --- irteus/test/interpolator.l | 23 +++++++++++++---------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index 89e15f259..c13e8989e 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -70,16 +70,16 @@ (+ dt (car tm-list))) tm-list) (send ip :pass-time dt) (push (send ip :position) data-list) - (if (find-method ip :vel) (push (send ip :vel) vel-data-list)) - (if (find-method ip :acc) (push (send ip :acc) acc-data-list)) + (if (find-method ip :velocity) (push (send ip :velocity) vel-data-list)) + (if (find-method ip :acceleration) (push (send ip :acceleration) 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))) - (if (find-method ip :vel) - (list :vel (if neglect-first (cdr (reverse vel-data-list)) (reverse vel-data-list)))) - (if (find-method ip :acc) - (list :acc (if neglect-first (cdr (reverse acc-data-list)) (reverse acc-data-list)))) + (if (find-method ip :velocity) + (list :velocity (if neglect-first (cdr (reverse vel-data-list)) (reverse vel-data-list)))) + (if (find-method ip :acceleration) + (list :acceleration (if neglect-first (cdr (reverse acc-data-list)) (reverse acc-data-list)))) ))) (defun test-interpolators @@ -118,13 +118,16 @@ (let ((res (test-interpolators minjerk-interpolator))) (assert res))) + #| (load "~/prog/euslib/jsk/gnuplotlib.l") (setq r (pos-list-interpolation (list #f(0) #f(30) #f(90) #f(-90) #f(0)) (list 0.25 0.25 0.25 0.25) 0.01)) -(setq r2 (mapcar #'(lambda (x) (elt x 0)) (cadr (memq :data r)))) (graph-view (list r2) (cadr (memq :time r)))) -(graph-view (list r2) (cadr (memq :time r))) -(print r2) -(print (length r2)) +(setq r-pos (mapcar #'(lambda (x) (elt x 0)) (cadr (memq :data r)))) +(setq r-vel (mapcar #'(lambda (x) (/ (elt x 0) 10)) (cadr (memq :velocity r)))) +(setq r-acc (mapcar #'(lambda (x) (/ (elt x 0) 100)) (cadr (memq :acceleration r)))) +(graph-view (list r-pos r-vel r-acc) (cadr (memq :time r)) :keylist (list "position" "velocity" "acceleration")) +(print r-pos) +(print (length r-pos)) |# (eval-when (load eval) From 614a1d3ca1421db5c96a8614d0a6efb339af265d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 31 Dec 2020 15:36:12 +0900 Subject: [PATCH 2/4] check overshoot within test-interpolators function --- irteus/test/interpolator.l | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index c13e8989e..f929e16f7 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -105,18 +105,18 @@ ;; (cadr (memq :time ret-list)) ;; :title (format nil "~A interpolator" (send ip-class :name)) ;; :xlabel "time [s]" :keylist (list ""))) - (and - (> (reduce #'(lambda (x y) (min x y)) ret-list2) (- -90 *epsilon*)) - (< (reduce #'(lambda (x y) (max x y)) ret-list2) (+ 90 *epsilon*))) + ;; check overshoot + (assert + (and + (> (reduce #'(lambda (x y) (min x y)) ret-list2) (- -90 *epsilon*)) + (< (reduce #'(lambda (x y) (max x y)) ret-list2) (+ 90 *epsilon*)))) )) (deftest test-linear-interpolator () - (let ((res (test-interpolators linear-interpolator))) - (assert res))) + (let ((res (test-interpolators linear-interpolator))))) (deftest test-minjerk-absolute-interpolator () - (let ((res (test-interpolators minjerk-interpolator))) - (assert res))) + (let ((res (test-interpolators minjerk-interpolator))))) #| From a1d50bc4150552e020cb97fc6b7544d6691086f9 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 31 Dec 2020 16:31:47 +0900 Subject: [PATCH 3/4] add test to check smoothness of minjerk-interpolator --- irteus/test/interpolator.l | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index f929e16f7..c28946c98 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -105,6 +105,31 @@ ;; (cadr (memq :time ret-list)) ;; :title (format nil "~A interpolator" (send ip-class :name)) ;; :xlabel "time [s]" :keylist (list ""))) + + ;; check velocitiy + (when (assoc :velocity (send ip-class :methods)) + (let ((position (cadr (memq :data ret-list))) + (velocity (cadr (memq :velocity ret-list))) + real-vel calc-vel) + (dotimes (i (1- (length position))) + (setq real-vel (scale (/ 1.0 dt) (v- (elt position (1+ i)) (elt position i))) + calc-vel (elt velocity i)) + (assert (< (norm (v- real-vel calc-vel)) 0.01) + (format nil "pos: ~A, vel:~A, vel:~A, diff:~A~%" (elt position i) real-vel calc-vel (norm (v- real-vel calc-vel))))) + )) + + ;; check acceleration + (when (assoc :acceleration (send ip-class :methods)) + (let ((velocity (cadr (memq :velocity ret-list))) + (acceleration (cadr (memq :acceleration ret-list))) + real-acc calc-acc) + (dotimes (i (1- (length velocity))) + (setq real-acc (scale (/ 1.0 dt) (v- (elt velocity (1+ i)) (elt velocity i))) + calc-acc (elt acceleration i)) + (assert (< (norm (v- real-acc calc-acc)) 0.02) + (format nil "vel: ~A, acc:~A, acc:~A, diff:~A~%" (elt velocity i) real-acc calc-acc (norm (v- real-acc calc-acc))))) + )) + ;; check overshoot (assert (and From 0454df76848d6fa5ec725b51df43e4c7ad4ad339 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 4 Jan 2021 10:59:54 +0900 Subject: [PATCH 4/4] fix :veocity/acceleration of minjerk-interpolation --- irteus/irtutil.l | 10 +++++----- irteus/test/interpolator.l | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/irteus/irtutil.l b/irteus/irtutil.l index e69131f99..7643dc419 100644 --- a/irteus/irtutil.l +++ b/irteus/irtutil.l @@ -398,12 +398,12 @@ (scale (expt segment-time 5) a5))) velocity (reduce #'v+ (list a1 - (scale (expt segment-time 1) a2) (scale (expt segment-time 2) a3) - (scale (expt segment-time 3) a4) (scale (expt segment-time 4) a5))) + (scale (* 2 (expt segment-time 1)) a2) (scale (* 3 (expt segment-time 2)) a3) + (scale (* 4 (expt segment-time 3)) a4) (scale (* 5 (expt segment-time 4)) a5))) acceleration - (reduce #'v+ (list a2 - (scale (expt segment-time 1) a3) (scale (expt segment-time 2) a4) - (scale (expt segment-time 3) a5)))) + (reduce #'v+ (list (scale 2 a2) + (scale (* 6 (expt segment-time 1)) a3) (scale (* 12 (expt segment-time 2)) a4) + (scale (* 20 (expt segment-time 3)) a5)))) position)) ; ) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index c28946c98..c5906a2fe 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -114,7 +114,7 @@ (dotimes (i (1- (length position))) (setq real-vel (scale (/ 1.0 dt) (v- (elt position (1+ i)) (elt position i))) calc-vel (elt velocity i)) - (assert (< (norm (v- real-vel calc-vel)) 0.01) + (assert (< (norm (v- real-vel calc-vel)) (if (memq :word-size=64 *features*) 0.01 0.15)) (format nil "pos: ~A, vel:~A, vel:~A, diff:~A~%" (elt position i) real-vel calc-vel (norm (v- real-vel calc-vel))))) )) @@ -126,7 +126,7 @@ (dotimes (i (1- (length velocity))) (setq real-acc (scale (/ 1.0 dt) (v- (elt velocity (1+ i)) (elt velocity i))) calc-acc (elt acceleration i)) - (assert (< (norm (v- real-acc calc-acc)) 0.02) + (assert (< (norm (v- real-acc calc-acc)) (if (memq :word-size=64 *features*) 0.01 0.15)) (format nil "vel: ~A, acc:~A, acc:~A, diff:~A~%" (elt velocity i) real-acc calc-acc (norm (v- real-acc calc-acc))))) ))