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

[Naoki-Hiraoka]

回転行列のためのinterpolatorです。
回転行列は特異点等の問題があるので、今あるlinear-inerpoilatorやminjerk-inerpoilatorを使うことができませんでした。

下図の3つの姿勢を補間する場合。

今あるlinear-interpolator
特異点周辺のため無駄に大きく動く

今あるminjerk-interpolator
特異点周辺のため無駄に大きく動く

今回追加したlinear-interpolator-SO3
まっすぐ補間してくれる. キーポーズの箇所で角速度が不連続に変わる。

今回追加したminjerk-interpolator-SO3
まっすぐ補間しつつ、始点と終点の角速度・角加速度を考慮して補間することで、軌道を滑らかにつなげることができる。

gif動画のコード

;; linear-interpolator
(setq *cube* (make-cube 200 100 400))
(objects (list *cube*))
(let ((vs nil))
  (setq l (instance linear-interpolator :init))
  (send l :reset
        :position-list (list (float-vector -pi/2 pi/2 2.7) (float-vector -0.4 0 -pi/2) (float-vector 0 0 0))
        :time-list (list 3.0 6.0))
  (send l :start-interpolation)
  (while (send l :interpolatingp)
    (send l :pass-time 0.1)
    (send *cube* :newcoords (rpy-matrix (elt (send l :position) 0) (elt (send l :position) 1) (elt (send l :position) 2)) (float-vector 0 0 0))
    (send *irtviewer* :draw-objects :flush nil)
    (push (send (make-coords :pos (float-vector 0 0 200)) :transform *cube* :world) vs)
    (dolist (v vs)
      (send v :draw-on :flush nil))
    (send *irtviewer* :viewer :flush)
    (unix::usleep 100000)
    (x::window-main-one)
    )
  )

;;minjerk-interpolator
(setq *cube* (make-cube 200 100 400))
(objects (list *cube*))
(let ((vs nil))
  (setq l (instance minjerk-interpolator :init))
  (send l :reset
        :position-list (list (float-vector -pi/2 pi/2 2.7) (float-vector -0.4 0 -pi/2) (float-vector 0 0 0))
        :velocity-list (list (float-vector 0 0 0) (float-vector 0.9 0 0) (float-vector 0 0 0))
        :acceleration-list (list (float-vector 0 0 0) (float-vector 0 0 0) (float-vector 0 0 0))
        :time-list (list 3.0 6.0))
  (send l :start-interpolation)
  (while (send l :interpolatingp)
    (send l :pass-time 0.1)
    (send *cube* :newcoords (rpy-matrix (elt (send l :position) 0) (elt (send l :position) 1) (elt (send l :position) 2)) (float-vector 0 0 0))
    (send *irtviewer* :draw-objects :flush nil)
    (push (send (make-coords :pos (float-vector 0 0 200)) :transform *cube* :world) vs)
    (dolist (v vs)
      (send v :draw-on :flush nil))
    (send *irtviewer* :viewer :flush)
    (unix::usleep 100000)
    (x::window-main-one)
    )
  )

;; linear-interpolator-SO3
(setq *cube* (make-cube 200 100 400))
(objects (list *cube*))
(let ((vs nil))
    (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)
      (send *cube* :newcoords (copy-object (send l :position)) (float-vector 0 0 0))
      (send *irtviewer* :draw-objects :flush nil)
      (push (send (make-coords :pos (float-vector 0 0 200)) :transform *cube* :world) vs)
      (dolist (v vs)
        (send v :draw-on :flush nil))
      (send *irtviewer* :viewer :flush)
      (unix::usleep 100000)
      (x::window-main-one)
      )
    )

;;minjerk-interpolator-SO3
(setq *cube* (make-cube 200 100 400))
(objects (list *cube*))
(let ((vs nil))
  (setq l (instance minjerk-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))
        :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))
        :time-list (list 3.0 6.0))
  (send l :start-interpolation)
  (while (send l :interpolatingp)
    (send l :pass-time 0.1)
    (send *cube* :newcoords (copy-object (send l :position)) (float-vector 0 0 0))
    (send *irtviewer* :draw-objects :flush nil)
    (push (send (make-coords :pos (float-vector 0 0 200)) :transform *cube* :world) vs)
    (dolist (v vs)
      (send v :draw-on :flush nil))
    (send *irtviewer* :viewer :flush)
    (unix::usleep 100000)
    (x::window-main-one)
    )
  )

[Affonso-Gui]

SO3 はなんの略ですか。

[Naoki-Hiraoka]

3D rotation group のことです。
https://en.wikipedia.org/wiki/3D_rotation_group

[k-okada]

please consider inherit minjerk-interpolator, we do not want to re-rwrite

jskeus/irteus/irtutil.l

Lines 512 to 534 in 0543c2b

	(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

in same file

[Naoki-Hiraoka]

minjerk-interpolatorクラスは、positionとvelocityとaccelerationが同じ空間にあることを想定していますが、SO3の場合はそうではないため、そのまま継承することはできません。minjerk-interpolatorクラスの方も修正すれば可能です。どうすべきとお考えですか。

[k-okada]

minjerk-interpolator の変更も前提にしていました． -- ◉ Kei Okada 2021年11月16日(火) 18:33 Naoki Hiraoka ***@***.***>:

…

minjerk-interpolatorクラスは、positionとvelocityとaccelerationが同じ空間にあることを想定していますが、SO3の場合はそうではないため、そのまま継承することはできません。minjerk-interpolatorクラスの方も修正すれば可能です。どうすべきとお考えですか。 — You are receiving this because you commented. Reply to this email directly, view it on GitHub, or unsubscribe.