[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

[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.