Merge pull request #595 from k-okada/fix_minjerk_vel_acc

euslisp · Jan 14, 2021 · 689b015 · 689b015 · k-okada · Jan 14, 2021
2 parents 6d5b288 + 0454df7
commit 689b015
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 22 deletions.
diff --git 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
@@ -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
@@ -105,26 +105,54 @@
     ;;    (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 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)) (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)))))
+      ))
+
+    ;; 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)) (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)))))
+      ))
+
+    ;; 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)))))
+
 
 #|
 (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)