move 360 rotatoin problem from pr2-interface.l to robot-interface.l

[k-okada]

related to jsk-ros-pkg/jsk_robot#844 (comment)

#319, #322 are related to 360 rotation problem on fetch

===

何を正解とすべきかはむづかしいんだけど、多分だけど、

5.irteusgl$ send *fetch* :angle-vector
#f(385.822 13.4851 29.5611 -158.632 113.043 -182.46 80.8974 -18.0974 0.0 0.0)
6.irteusgl$ send *ri* :state :potentio-vector
#f(380.0 14.4741 30.3673 -156.29 112.813 174.715 81.1236 -17.8094 0.0 0.0)

となっていて送るときは-182を希望するんだけど、*ri*のなかで
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L72
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L890
あたりで、近い方の178を目標に補間されている
https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/pr2-interface.l#L368
のようにして、-182側に廻るようにすると、期待通りになることはなる．

===

[708yamaguchi]

改めて方針を確認します。

• fetchを含む腕の無限回転問題をrobot-interface.l内で修正する。
• moveitを使わない場合(euslispのangle-vectorをそのまま実機に送る場合)
  • 基本的に関節の回転量が小さくなるような回転方向に補間する。
  • 補間は-180 ~ 180度の範囲内で行うようにして、180(-180)度をまたぐときのみ0度側に遠回りをする。(例えば、無限回転関節のjoint-angleを-178から178にする時に、-178 -> -180(180) -> 178とするのではなく、-178 -> 0 -> 178とする)
• moveitを使う場合
  • moveitの補間に完全に従う。