[jskeus/irteus/irtmodel.l] fix target joint table in :angle-vector

[mmurooka]

No description provided.

[mmurooka]

もっと良いテストコードが作れそうですが，

(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l")

(setq *robot* (instance hrp2jsknts-robot :init))
(objects (list *robot*))

(send *robot* :init-pose)
(send *robot* :lleg :crotch-r :joint-angle 1.2058)
(send *robot* :lleg :crotch-p :joint-angle -111.255)

(format t "crotch-r: ~a  crotch-p: ~a~%"
        (send *robot* :lleg :crotch-r :joint-angle)
        (send *robot* :lleg :crotch-p :joint-angle))

(send *robot* :angle-vector (send *robot* :angle-vector))

(format t "crotch-r: ~a  crotch-p: ~a~%"
        (send *robot* :lleg :crotch-r :joint-angle)
        (send *robot* :lleg :crotch-p :joint-angle))

をやっていて，発見しました．
PR前だと，以下のようにcrotch-pの角度が元から大きく変わってしまいおかしいです．

crotch-r: 1.2058  crotch-p: -111.255
crotch-r: 1.18522  crotch-p: -19.0

PR後だと，以下のように正しく修正されます．

crotch-r: 1.2058  crotch-p: -111.255
crotch-r: 1.0  crotch-p: -111.255

となります．

[k-okada]

なかなか凄いバグなきがしますが，いままで出てこなかったんでしょうか？それともなんか変だなぁ，で，スルーしてきたのでしょうか．

[snozawa]

euslibの時代からあった問題で、euslibの時代には治ってて、
irteusに移植するときにバグが入ったように思います。
testコードがirteusになかったのが良くなかったように思います。
（irteus/test/joint.lにmin-max-tableのテストがあるので、そこに追加できると良いかも）

[mmurooka]

#478 で再現テストと本PRの修正を合わせたPRにしたので，こちらはcloseします．

それともなんか変だなぁ，で，スルーしてきたのでしょうか．

だと思います．
・ min-max-tableの行列が２つの関節で同じような値を持っていると，大きなズレは生じない．
・ rotational-jointの:joint-angleメソッドの中でmin-max-tableを確認しているところは正しくて，:angle-vectorの中では間違えていたので，IKを解いてirtviewerで眺めるだけでは大丈夫で，*ri*に:angle-vectorを送るときにはじめて問題が起きる．
あたりがバグを見つけづらくしていたと思います．