From 534029bd97c3af4ae67fa3fecf2949e2d93ffb04 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Jan 2021 23:24:12 +0900 Subject: [PATCH 1/7] run graph-view if the function is defined --- irteus/test/interpolator.l | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index c5906a2f..601858ac 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -99,12 +99,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)) From 76ee76120dd1c2bfdd29ff75c87346bc541bfce3 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Jan 2021 23:25:11 +0900 Subject: [PATCH 2/7] add test to cehck https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457 --- irteus/test/interpolator.l | 67 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index 601858ac..f0540caa 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -140,12 +140,79 @@ (< (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) (format nil "vel:~A~%" (car velocity))) + (assert (eps= (norm (car (last velocity))) 0.0) (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) + (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) (format nil "acc:~A~%" (car acceleration))) + (assert (eps= (norm (car (last acceleration))) 0.0) (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) + (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.01 (elt x 0))) (cadr (memq :velocity ret-list)))) + (r-acc (mapcar #'(lambda (x) (* 0.00001 (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 (* 100 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") From 117f33471dd09856f469f151bbcc3b6c321b594d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Jan 2021 23:33:04 +0900 Subject: [PATCH 3/7] add more comment on pos-list-interpolation --- irteus/test/interpolator.l | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index f0540caa..7617472c 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -57,6 +57,11 @@ ((: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)) + (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 @@ -73,6 +78,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))) From ed3a36b63790e2bc13e34659027b9bb00f5345ba Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Jan 2021 23:41:12 +0900 Subject: [PATCH 4/7] fix graph-veiw parameters --- irteus/test/interpolator.l | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index 7617472c..e6ad5f43 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -197,12 +197,12 @@ (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.01 (elt x 0))) (cadr (memq :velocity ret-list)))) - (r-acc (mapcar #'(lambda (x) (* 0.00001 (elt x 0))) (cadr (memq :acceleration 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 (* 100 1000)) + (unix:usleep (* 500 1000)) )) )) From 7309fa24ff16cec872c9a3a044b599f1fcf1a18f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Jan 2021 23:44:32 +0900 Subject: [PATCH 5/7] create velociy-list and acceleration-list for interpolator, see https://github.com/fkanehiro/hrpsys-base/blob/3a83667cc76554bc86c473b00b1dc666f96e7ad7/rtc/SequencePlayer/seqplay.cpp#L243-L247 --- irteus/test/interpolator.l | 33 +++++++++++++++++++++++++++++++-- 1 file changed, 31 insertions(+), 2 deletions(-) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index e6ad5f43..c7ea8232 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -57,6 +57,35 @@ ((: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) @@ -66,8 +95,8 @@ :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) From 22c98803aabdc2b0f1fedb6ac435fdb837d43bfb Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 21 Jan 2021 23:53:20 +0900 Subject: [PATCH 6/7] if time-segment is not aligned, need to fix the data, adjust time and segment-time to exact position --- irteus/irtutil.l | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) 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)) ) From 602907e5b8d2f23516cdbdb4859ea355a690751c Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 22 Jan 2021 16:49:21 +0900 Subject: [PATCH 7/7] add large epsilon for :word-size 32 --- irteus/test/interpolator.l | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/irteus/test/interpolator.l b/irteus/test/interpolator.l index c7ea8232..51d23e06 100644 --- a/irteus/test/interpolator.l +++ b/irteus/test/interpolator.l @@ -195,15 +195,17 @@ (velocity (cadr (memq :velocity ret-list))) real-vel calc-vel) ;; check first and last velocity - (assert (eps= (norm (car velocity)) 0.0) (format nil "vel:~A~%" (car velocity))) - (assert (eps= (norm (car (last velocity))) 0.0) (format nil "vel:~A~%" (car (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) + (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 @@ -211,15 +213,17 @@ (acceleration (cadr (memq :acceleration ret-list))) real-acc calc-acc) ;; check first and last acceleration - (assert (eps= (norm (car acceleration)) 0.0) (format nil "acc:~A~%" (car acceleration))) - (assert (eps= (norm (car (last acceleration))) 0.0) (format nil "acc:~A~%" (car (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) + (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))))) ))