add interpolation in :angle-vector-raw

[k-okada]

Fixes #839

rewrited versoin of #841

fetch interface have problems on trajectory interpolation after cancel , (maybe) when the original trajectory goes over 180 degree ( ZebraDevs/robot_controllers#33 )

This PR fixes by sending interpolated angle-vector-sequence within :angle-vector-raw

[708yamaguchi]

diff-avが定義されていないと言われます。
diff-avを(v- av (send robot :angle-vector))
のようにするのが正しいでしょうか。

[k-okada]

ありがとう．直してみました．

[708yamaguchi]

ここのdiff-avも(v- av prev-av)だと思います。さっきかくの忘れてました。

[k-okada]

そうですね．こんどこそ．

[708yamaguchi]

角度が180度を超えたら云々というのは、test-fetcheus.lの中でdiff-avを計算するときにdiff-avが大きくなりすぎるという問題から来るものだったような気がします。

[k-okada]

diff-avが大きくなる理由が #841 (comment) だという理解です．

[708yamaguchi]

ありがとうございます

[k-okada]

@708yamaguchi これでマージして大丈夫かな？確認してみてください．

[708yamaguchi]

実機で試したのですが、
(send *ri* :angle-vector (send *fetch* :angle-vector #f(0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) 10000)
として最初の姿勢を作ったあとに、
(send *ri* :angle-vector-raw (send *fetch* :angle-vector #f(0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 0.0 0.0)) 10000)
とすると、
#f(1.421085e-14 1.387779e-17 0.0 640.0 2.220446e-16 0.0 -1.110223e-16 0.0 0.0 4.440892e-16)
のような値がかえってきて、実際、fetchの4番目の関節が640度回転したのを目で確認しました。

(send *ri* :sub-angle-vector #f(0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 0.0 0.0) #f(0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0))
の返り値が
#f(0.0 0.0 0.0 640.0 0.0 0.0 0.0 0.0 0.0 0.0)
となるのが問題だと思うのですが、どうでしょうか。

[k-okada]

なるほど．
jsk-ros-pkg/jsk_pr2eus#323
が必要でしょうか．

[708yamaguchi]

jsk-ros-pkg/jsk_pr2eus#323
を入れて再び実機で

(send *ri* :angle-vector (send *fetch* :angle-vector #f(0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0)) 10000)
(send *ri* :angle-vector-raw (send *fetch* :angle-vector #f(0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 0.0 0.0)) 10000)

としたところ、
#f(0.0 -5.551115e-17 -5.551115e-17 -80.0 0.0 4.163336e-17 2.220446e-16 -1.110223e-16 \ 0.0 1.776357e-15)
のような値がかえってきて、実際にfetchの腕が-80度だけ回転するのを確認しました。
また、
(send *ri* :sub-angle-vector #f(0.0 0.0 0.0 1000.0 0.0 0.0 0.0 0.0 0.0 0.0) #f(0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0))
の返り値も
#f(0.0 0.0 0.0 -80.0 0.0 0.0 0.0 0.0 0.0 0.0)
だったので、正しい挙動をしていると思います。