Skip to content
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

Jump to bottom

[irtutil.l] add minjerk-interpolator-so3 and linear-interpolator-so3 #604

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

[irtutil.l] add minjerk-interpolator-so3 and linear-interpolator-so3 #604

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e4d3b5b
[irtutil.l] add minjerk-interpolator-so3 and linear-interpolator-so3
Naoki-Hiraoka Aug 24, 2021
0543c2b
Merge branch 'master' into PR-so3
k-okada Nov 16, 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
158 changes: 158 additions & 0 deletions irteus/irtutil.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -414,6 +414,164 @@
(while (send l :interpolatingp) (send l :pass-time 0.02) (print (send l :position)))
|#

(defclass linear-interpolator-SO3
:super interpolator)
(defmethod linear-interpolator-SO3
;;
(:interpolation
()
"Linear interpolator for SO3"
(let* ((v1 (nth segment position-list))
(v2 (nth (1+ segment) position-list))
(t1+t2 (- (nth segment time-list) (if (> segment 0) (nth (1- segment) time-list) 0))) ;; total time of segment
(t1 segment-time))
(midrot (/ t1 t1+t2) v1 v2)))
)

#| example
(setq l (instance linear-interpolator-so3 :init))
(send l :reset :position-list (list (rpy-matrix -pi/2 pi/2 2.7) (rpy-matrix -0.4 0 -pi/2) (rpy-matrix 0 0 0)) :time-list (list 3.0 6.0))
(send l :start-interpolation)
(while (send l :interpolatingp) (send l :pass-time 0.1) (print (send l :position)))
|#


;; Smooth Attitude Interpolation
;; https://github.com/scipy/scipy/files/2932755/attitude_interpolation.pdf
;;
;; +
;;
;; Minimum Jerk trajectory generation, a.k.a Hoff & Arbib, described in the documents
;; http://mplab.ucsd.edu/tutorials/minimumJerk.pdf (you can find copy of this at http://www.shadmehrlab.org/book/minimumjerk.pdf)
;;
;; Hoff B, Arbib MA (1992) A model of the effects of speed, accuracy, and
;; perturbation on visually guided reaching. In: Control of arm movement
;; in space: neurophysiological and computational approaches
;; (Caminiti R, Johnson PB, Burnod Y, eds), pp 285-306.

(defclass minjerk-interpolator-SO3
:super interpolator
:slots (velocity acceleration velocity-list acceleration-list))
(defmethod minjerk-interpolator-SO3
(:velocity () "returns current velocity" velocity)
(:velocity-list () "returns velocity list" velocity-list)
(:acceleration () "returns current acceleration" acceleration)
(:acceleration-list () "returns acceleration list" acceleration-list)
(:reset
(&rest args
&key
((:velocity-list vl) (send self :velocity-list))
((:acceleration-list al) (send self :acceleration-list))
&allow-other-keys)
"minjerk interopolator-SO3
position-list : list of control point (SO3)
velocity-list : list of velocity in each control point represented in local frame
acceleration-list : list of acceleration in each control point represented in local frame"
(send-super* :reset args)
(setq velocity-list (if vl vl (make-list (1+ segment-num) :initial-element (instantiate float-vector 3))))
(setq acceleration-list (if al al (make-list (1+ segment-num) :initial-element (instantiate float-vector 3))))
)
(:interpolation
()
"Example code is:
(setq l (instance linear-interpolator-so3 :init))
(send l :reset :position-list (list (rpy-matrix -pi/2 pi/2 2.7) (rpy-matrix -0.4 0 -pi/2) (rpy-matrix 0 0 0)) :time-list (list 3.0 6.0)
:velocity-list (list (float-vector 0 0 0) (float-vector 0 0.9 0) (float-vector 0 0 0))
:acceleration-list (list (float-vector 0 0 0) (float-vector 0 0 0) (float-vector 0 0 0)))
(send l :start-interpolation)
(while (send l :interpolatingp) (send l :pass-time 0.1) (print (send l :position)))"
(let* ((Ri (nth segment position-list))
(Rf (nth (1+ segment) position-list))
(wi (nth segment velocity-list))
(wf (nth (1+ segment) velocity-list))
(dwi (nth segment acceleration-list))
(dwf (nth (1+ segment) acceleration-list))
;;
;; x := theta
(xi (float-vector 0 0 0))
(xf (matrix-log (m* (transpose Ri) Rf)))
(Af_ (if (equal xi xf)
(unit-matrix 3)
(reduce #'m+ (list (unit-matrix 3)
(scale-matrix 1/2 (outer-product-matrix xf))
(scale-matrix (/ (- 1 (/ (/ (norm xf) 2) (tan (/ (norm xf) 2)))) (norm2 xf)) (m* (outer-product-matrix xf) (outer-product-matrix xf)))))))
(vi wi)
(vf (transform Af_ wf))
(dAf_ (if (equal xi xf)
(scale-matrix 0.5 (outer-product-matrix vf))
(reduce #'m+ (list (scale-matrix 1/2 (outer-product-matrix vf))
(scale-matrix (+ (/ (/ (norm vf) (tan (/ (norm xf) 2))) (* 2 (norm2 xf)))
(/ (norm vf) (* 4 (norm xf) (expt (sin (/ (norm xf) 2)) 2)))
(- (/ (* 2 (norm vf)) (expt (norm xf) 3))))
(m* (outer-product-matrix xf) (outer-product-matrix xf)))
(scale-matrix (/ (- 1 (/ (/ (norm xf) 2) (tan (/ (norm xf) 2)))) (norm2 xf))
(m+ (m* (outer-product-matrix vf) (outer-product-matrix xf)) (m* (outer-product-matrix xf) (outer-product-matrix vf))))))))
(ai dwi)
(af (v+ (transform dAf_ wf) (transform Af_ dwf)))
;;
(t1+t2 (- (nth segment time-list) (if (> segment 0) (nth (1- segment) time-list) 0))) ;; total time of segment
;; A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t)
;; B=(gv-(v+a*t))/(t*t)
;; C=(ga-a)/t
(A (scale (/ 1.0 (* t1+t2 t1+t2 t1+t2)) (v- xf (reduce #'v+ (list xi (scale t1+t2 vi) (scale (* t1+t2 t1+t2) (scale 0.5 ai)))))))
(B (scale (/ 1.0 (* t1+t2 t1+t2)) (v- vf (v+ vi (scale t1+t2 ai)))))
(C (scale (/ 1.0 (* t1+t2)) (v- af ai)))
;; a0=x
;; a1=v
;; a2=a/2.0
;; a3=10*A-4*B+0.5*C
;;; a4=(-15*A+7*B-C)/t
;; a5=(6*A-3*B+0.5*C)/(t*t)
(a0 xi)
(a1 vi)
(a2 (scale 0.5 ai))
(a3 (v+ (v- (scale 10 A) (scale 4 B)) (scale 0.5 C)))
(a4 (scale (/ 1.0 t1+t2) (v- (v+ (scale -15 A) (scale 7 B)) C)))
(a5 (scale (/ 1.0 t1+t2 t1+t2) (v+ (v+ (scale 6 A) (scale -3 B)) (scale 0.5 C))))

;; x=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t
;; v=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t
;; a=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t
(x_ (reduce #'v+ (list a0
(scale (expt segment-time 1) a1) (scale (expt segment-time 2) a2)
(scale (expt segment-time 3) a3) (scale (expt segment-time 4) a4)
(scale (expt segment-time 5) a5))))
(v_ (reduce #'v+ (list a1
(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))))
(a_ (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))))
(A-1_ (if (= (norm x_) 0)
(unit-matrix 3)
(reduce #'m+ (list (unit-matrix 3)
(scale-matrix (- (/ (- 1 (cos (norm x_))) (norm2 x_))) (outer-product-matrix x_))
(scale-matrix (/ (- (norm x_) (sin (norm x_))) (expt (norm x_) 3)) (m* (outer-product-matrix x_) (outer-product-matrix x_)))))))
(dA-1v_ (if (= (norm x_) 0)
(float-vector 0 0 0)
(reduce #'v+ (list (scale (* (/ (- (+ (* (norm x_) (sin (norm x_))) (* 2 (- (cos (norm x_)) 1)))) (expt (norm x_) 4)) (v. x_ v_))
(v* x_ v_))
(scale (* (/ (- (+ (* 2 (norm x_)) (* (norm x_) (cos (norm x_))) (* -3 (sin (norm x_))))) (expt (norm x_) 5)) (v. x_ v_))
(v* x_ (v* x_ v_)))
(scale (/ (- (norm x_) (sin (norm x_))) (expt (norm x_) 3))
(v* v_ (v* x_ v_)))))))
)
(setq position (m* Ri (matrix-exponent x_))
velocity (transform A-1_ v_)
acceleration (v+ (transform A-1_ a_) dA-1v_))
position))
)

#| example
(setq l (instance linear-interpolator-so3 :init))
(send l :reset :position-list (list (rpy-matrix -pi/2 pi/2 2.7) (rpy-matrix -0.4 0 -pi/2) (rpy-matrix 0 0 0)) :time-list (list 3.0 6.0)
:velocity-list (list (float-vector 0 0 0) (float-vector 0 0.9 0) (float-vector 0 0 0))
:acceleration-list (list (float-vector 0 0 0) (float-vector 0 0 0) (float-vector 0 0 0)))
(send l :start-interpolation)
(while (send l :interpolatingp) (send l :pass-time 0.1) (print (send l :position)))
|#


;; color utils
(defun his2rgb (h &optional (i 1.0) (s 1.0) ret)
"convert his to rgb (0 <= h <= 360, 0.0 <= i <= 1.0, 0.0 <= s <= 1.0)"
Expand Down
92 changes: 92 additions & 0 deletions irteus/test/interpolator.l
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,50 @@
(assert (null (send l :interpolatingp)))
))

(deftest linear-interpolator-SO3
(let* ((l (instance linear-interpolator-SO3 :init))
(p0 (rpy-matrix -pi/2 0 -pi/2)) (t0 0.10)
(p1 (rpy-matrix 0 pi/2 pi/2)) (t1 0.18))
(send l :reset :position-list (list p0 p1 p0) :time-list (list t0 t1))
(send l :start-interpolation)
(do ((i 0 (+ i 0.02)))
((eps>= i t0))
(send l :pass-time 0.02)
(assert (eps= 0 (matrix-log (m* (transpose (send l :position)) (midrot (/ i t0) p0 p1)))))
(print (list i (send l :position) (midrot (/ i t0) p0 p1)))
)
(do ((i t0 (+ i 0.02)))
((eps> i t1))
(send l :pass-time 0.02)
(assert (eps= 0 (matrix-log (m* (transpose (send l :position)) (midrot (/ (- i t0) (- t1 t0)) p0 p1)))))
(print (list i (send l :position) (midrot (/ (- i t0) (- t1 t0)) p0 p1)))
)
(assert (null (send l :interpolatingp)))
))

(deftest minjerk-interpolator-SO3
(let* ((l (instance minjerk-interpolator-SO3 :init))
(p0 (rpy-matrix -pi/2 0 -pi/2)) (t0 0.10)
(p1 (rpy-matrix 0 pi/2 pi/2)) (t1 0.18))
(send l :reset :position-list (list p0 p1 p0) :time-list (list t0 t1))
(send l :start-interpolation)
(do ((i 0 (+ i 0.02)))
((eps> i t0))
(send l :pass-time 0.02)
)
(assert (eps= 0 (matrix-log (m* (transpose (send l :position)) p1))))
(print (list (send l :position) p1))
;;
(do ((i t0 (+ i 0.02)))
((eps> i t1))
(send l :pass-time 0.02)
)
(assert (eps= 0 (matrix-log (m* (transpose (send l :position)) p0))))
(print (list (send l :position) p0))
;;
(assert (null (send l :interpolatingp)))
))

;; copy from https://github.com/jsk-ros-pkg/euslib/blob/master/tests/test-virtual-interpolator.l
;; pos interpolation function
;; sample : euslib/test/test-virtual-interpolator.l
Expand Down Expand Up @@ -143,6 +187,54 @@
(deftest test-minjerk-absolute-interpolator ()
(let ((res (test-interpolators minjerk-interpolator)))))

(defun test-interpolators-SO3
(&optional (ip-class linear-interpolator-SO3) (mode))
(let* ((dt 0.01) ;; step time [s]
(time-len 10.0) ;; total time [s]
(ret-list
(pos-list-interpolation
(list (rpy-matrix 0 0 0) (rpy-matrix 0 0.7 0) (rpy-matrix 0 pi/2 0) (rpy-matrix -pi/2 0 pi/2) (rpy-matrix 0 0 0))
(list (/ time-len 4) (/ time-len 4) (/ time-len 4) (/ time-len 4))
dt
:interpolator-class ip-class
)))
;; (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 "")))

;; 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) (matrix-log (m* (transpose (elt position i)) (elt position (1+ i)))))
calc-vel (elt velocity i))
(assert (< (norm (v- real-vel calc-vel)) (if (memq :word-size=64 *features*) 0.1 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.1 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)))))
))
))

(deftest test-linear-interpolator-SO3 ()
(let ((res (test-interpolators-SO3 linear-interpolator-SO3)))))

(deftest test-minjerk-interpolator-SO3 ()
(let ((res (test-interpolators-SO3 minjerk-interpolator-SO3)))))

#|
(load "~/prog/euslib/jsk/gnuplotlib.l")
Expand Down