euslisp / jskeus

This repository contains EusLisp software developed and used by JSK at The University of Tokyo
23 stars 55 forks source link

fix velocity and acecleration of minjerk-interpolator #595

Closed k-okada closed 3 years ago

k-okada commented 3 years ago

the velocity and acceleration of minjerk-interpolator are not correct. The output of

(load "gnuplotlib.l") ;; download from https://raw.githubusercontent.com/k-okada/jskeus/add_gnuplotlib/irteus/gnuplotlib.l, see   https://github.com/euslisp/jskeus/pull/261                                                                                   
(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 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)) 

is illustrated below. Screenshot from 2020-12-31 15-07-29.

And this should be Screenshot from 2020-12-31 15-54-43

Add test to this smoothness.

The root cause is typo on irtutil, the velocity should be a1 + 2*a2*t+..., but it returns a1 + a2*t + ...

https://github.com/euslisp/jskeus/blob/6d5b288d37ada3b47dcc94108ba551b63f4189fc/irteus/irtutil.l#L391-L406