Skip to content

fix minjerk interpolation sometimes returns large acceleration #596

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 7 commits into from
Nov 24, 2021
Merged

fix minjerk interpolation sometimes returns large acceleration #596

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
534029b
run graph-view if the function is defined
k-okada Jan 21, 2021
76ee761
add test to cehck https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457
k-okada Jan 21, 2021
117f334
add more comment on pos-list-interpolation
k-okada Jan 21, 2021
ed3a36b
fix graph-veiw parameters
k-okada Jan 21, 2021
7309fa2
create velociy-list and acceleration-list for interpolator, see https…
k-okada Jan 21, 2021
22c9880
if time-segment is not aligned, need to fix the data, adjust time and…
k-okada Jan 21, 2021
602907e
add large epsilon for :word-size 32
k-okada Jan 22, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 11 additions & 4 deletions irteus/irtutil.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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))
)
Expand Down
130 changes: 122 additions & 8 deletions irteus/test/interpolator.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)))
Expand All @@ -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))
Expand Down Expand Up @@ -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")
Expand Down