add :wait-interpolation in test

[708yamaguchi]

(send *ri* :wait-interpolation)がないと、

(setq diff-av (v- (send *ri* :state :potentio-vector) (send *fetch* :angle-vector)))

の(send *ri* :state :potentio-vector)が正しく更新されない気がします。

[708yamaguchi]

:wait-interpolationを入れると、
#844 (comment)
のようになり、無限回転問題が発生するようになりました。

[knorth55]

Good!

[708yamaguchi]

Test passed on my local PC.
Thank you for your advice!

[k-okada]

adding (send *ri* :wait-interpolation) is ok

Please check
(setq diff-av (concatenate float-vector (subseq diff-av 0 1) (map float-vector #'(lambda (x) (cond ((> x 180) (- x 360)) ((< x -180) (+ x 360)) (t x))) (subseq diff-av 1)))) will not be required if we have
please check please check jsk-ros-pkg/jsk_pr2eus#319

[k-okada]

いくつか勘違いしていたきがします。

• robot-interface.l (シミュレーション) 無限回転軸のロボットは、:angle-vectorだと、近い方の回転方向を選んで動くようになっている。
  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#L895

• :angle-vector-sequenceだと、近い方で速度が計算されている？https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L530-L537

  • シミュレーションは上記のルールが適用される？
  • 実機はどう動くかはcontroller次第?。PR2だと単純に補完せずに近い方から動くようになっている？

• pr2-interface.l (シミュレーション) 無限回転軸のロボットは:angle-vectorで送っても:angle-vector-sequenceになる。https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/pr2-interface.l#L368 そのルールはrobot-interface.lと同じ。

• pr2-interface.l （実機）だと、-200, +200 と与えたら、ちゃんと＋方向に動くようになっている ?
  　https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/pr2-interface.l#L368
  https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/pr2-interface.l#L384-L400

関連：
jsk-ros-pkg/jsk_common#87
jsk-ros-pkg/jsk_pr2eus#1

結局現在の仕様は

• eusレベルでは関節は無限回転。IKを解くと180以上のものが出てくる。
• 実機に送るときは、-200, +200 としたら+方向に動いてほしい。

なので、8751169 がなくてもOKにするのが正解？

[knorth55]

無限回転して腕にコードが絡まるのが解決できるならそれに越したことはないと思います。

[k-okada]

fetch 実機の現状
:angle-vector-raw : -150度 > 150度 とするときは，-60度で動く．つまり思った角度とは違う状況になる．（姿勢は同じ）　これはこれでeusでIK解いて，違う角度だけど，同じ（手先）姿勢の場合に，:angle-vector-rawで，手先がグルンと回らないようにするという効果がある．
:angle-vector : moveitを経由するので，-150度 -> 150度は+300度で動く．こうしないと最終姿勢は同じでも，経路が違う状況になるので，これは回避できない．回避するにはmoveitのconstraintでできるだけ-360 - 360 で動かすような設定側などがあるか？

[k-okada]

#849 でいいかな．よければclose してください． @708yamaguchi

[knorth55]

状況がよくわかっていないのですが、

回避するにはmoveitのconstraintでできるだけ-360 - 360 で動かすような設定側などがあるか？

の返答としては
MotionPlanRequestのConstraintsにJointConstraintsという制約が掛けられます。
このmsgは以下の通りでposition=0でtolerance_above=360, tolerance_below=-360にすれば出来ると思います。

# Constrain the position of a joint to be within a certain bound
string joint_name

# the bound to be achieved is [position - tolerance_below, position + tolerance_above]
float64 position
float64 tolerance_above
float64 tolerance_below

# A weighting factor for this constraint (denotes relative importance to other constraints. Closer to zero means less important)
float64 weight

ただ実機で何が問題になっているのかを理解していないです。
moveitのpath planningに渡す前にeus側でangle-vectorをangle-vector-raw同様に書き換えるのがよいのかなと思いますがどうでしょう？

http://docs.ros.org/jade/api/moveit_msgs/html/msg/JointConstraint.html

[k-okada]

このmsgは以下の通りでposition=0でtolerance_above=360, tolerance_below=-360にすれば出来ると思います。

実際にどうなるか，やってみましょう．

＝＝

• eusを使っているとき + IK を解いている時．関節角度が何度かは気にしない．手先の姿勢が重要．なので，IKを解いてる途中で関節が３６０度以上回転しても，これを実機におくったら，３６０度以上まわってほしくない．実機も３６０度以上送ると，近い方の回転をする．
• eusを使っているとき + :angle-vector を直接送っているとき． ３６０度以上の回転を:angle-vectorで送ると実機もそのように送って欲しい気もするが，上のルールと条件が同じなので，ここは我慢してもらうしかない．
• moveitを使っている時，指令値は関節角度が何度かは気にしない．手先の姿勢だけが重要．ただし，moveitの出力で関節が３６０度以上まわるのであれば，それはそのまま３６０度以上まわらないと，障害物を避けないから，ここは尊重する．

問題はmoveitを使っている時

• rotation-axis :z で途中の起動を動いてほしい．ものが入ったコップを持っているときなど．
• 上ほどの成約ではないけど，手首が回転するとコードが絡むからやめてほしい

の２つの条件が満たされるプランニングができると嬉しい．

[708yamaguchi]

fetchの腕の無限回転問題ですが、
riが作られるときにスロット変数を定義して、fetchの各joint-angleが回転した累積の角度を保存しておいて、無限回転する関節については累積回転角度がプラスマイナス360度以上になったら0度に近くなるように戻す
という処理をすればコードが絡まることは少なくなるのかなぁと思ったのですがどうでしょうか。