ikを解くときに:rotation-axis #f(0 0 1)とかできると嬉しい

[Yasunori Toshimitsu]

cc: @k-okada

[Yasunori Toshimitsu]

ロボットアームに棒状のものをつかませるために使用しました。もっと手短に解決できると思いますが、このように実装しました。

get-orthogonalという関数は、入力したfloat-vectorに対して垂直なfloat-vectorを適当に返します

; returns a vector orthogonal to any random vector you throw at it.
; input&output are both float-vector
; http://www.cg.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/tech68.html
(defun get-orthogonal (vector)
  (let ((x (elt vector 0))
        (y (elt vector 1))
        (z (elt vector 2))
        (return-vector (float-vector 0 0 0)))
    (cond
      ((and (< (abs x) (abs y)) (< (abs x) (abs z)))
       (setq return-vector (float-vector 0 z (- 0 y))))
      ((and (< (abs y) (abs x)) (< (abs y) (abs z)))
       (setq return-vector (float-vector (- 0 z) 0 x)))
      (t
       (setq return-vector (float-vector y (- 0 x) 0)))
      )
    return-vector
    )
  )

get-matrixは、あるfloat-vectorを入力すると、それをz軸（ロボットアームの「握る軸」がz軸だったため）とした回転行列を返します。

; return a 3x3 matrix with the x axis being the vector you threw at it.
; http://euslisp.github.io/jskeus/jmanual-node111.html
; use :rot to set rotation matrix
(defun get-matrix (vector)
  (let ((x-vector (float-vector 0 0 0))
        (y-vector (float-vector 0 0 0))
        (z-vector (float-vector 0 0 0)))
    (setq x-vector (normalize-vector vector))
    (setq y-vector (normalize-vector (get-orthogonal x-vector)))
    (setq z-vector (normalize-vector (v* x-vector y-vector)))
    (setq return-matrix (make-matrix 3 3 (list
                                          y-vector
                                          z-vector
                                          x-vector)))
    ; this was the key!!!(or more like, my 過失 for not realizing it was caused by this)
    ; I'm not sure why it worked without the transposition in the first place, but I'm not complaining
    (setq return-matrix (transpose return-matrix))
    return-matrix))

最後に、get-matrixで得た行列を回転行列とする座標系(tmp-coords)を定義すれば、逆**学を解けます。

(send *dxl-armed-turtlebot* :inverse-kinematics tmp-coords :rotation-axis :z :revert-if-fail nil)

これで、例えばvectorを、パイプと平行なベクトルとした時、あとは握る位置を与えれば、パイプの軸に対して拘束をかけた状態で逆**学を解いて、握ることができます。

[Kei Okada]

ちょっとわかりづらいところに有りますが，
http://euslisp.github.io/jskeus/jmanual-node171.html

 (orient-coords-to-axis (make-coords :pos #f(100 100 100)) #f(1 1 1)))

とすると，第一引数の座標系のZ軸の向きを第二引数のベクトルに合わせることができます．
自分で作ったコードと比較して答え合わせをしてみてください．

[Yasunori Toshimitsu]

ありがとうございます。機能として、同じようなものを作ったしまったようです…