[jvrc-statenet.l] fix :pitch-roll

[Naoki-Hiraoka]

jvrc-statenet.lの:pitch-roll関数が、roll方向にロボットが傾いた時にうまく機能しないように思います。 pitch(マイナス), rollを正しく返すよう修正しました。

$ (defun pitch-roll-old (coords)    (let* ((z #f(0 0 1))
          (y #f(0 1 0))
          (lz (send coords :rotate-vector z))
          (lx (send coords :rotate-vector #f(1 0 0)))
          (ly (send coords :rotate-vector y))
          (nz (v* z lz))
          (ny (v* z ly))
          )
     (normalize-vector nz nz)
     (normalize-vector ny ny)
     ;; (print (list lz ly nz ny))
     (if (< (v. lx z) 0) (setq nz (v- nz)))
     (float-vector (vector-angle z lz nz)
                   ;;(vector-angle z ly ny)
                   (v. z ly)
                   )
     ))

$ (defun pitch-roll-new (coords)    (let ((rpyangles (car (rpy-angle (send coords :rot)))))
     (float-vector (- (elt rpyangles 1)) (elt rpyangles 2))
     ))

$ (pitch-roll-old (make-coords :rpy (list 0 pi/2 0)))
#f(-1.5708 0.0) ;;正しい
$ (pitch-roll-new (make-coords :rpy (list 0 pi/2 0)))
#f(-1.5708 0.0) ;;正しい

$ (pitch-roll-old (make-coords :rpy (list 0 0 0.3)))
#f(0.3 0.29552) ;;正しくない
$ (pitch-roll-new (make-coords :rpy (list 0 0 0.3)))
#f(0.0 0.3) ;;正しい

$ (pitch-roll-old (make-coords :rpy (list 0 0 pi/2))) 
#f(1.5708 1.0) ;;正しくない
$ (pitch-roll-new (make-coords :rpy (list 0 0 pi/2)))             
#f(0.0 1.5708) ;;正しい