:angle-vectorと:potentio-vectorに差があると:angle-vector-sequenceの動き出しがスローになる

[Kanazawanaoaki]

https://gist.github.com/Kanazawanaoaki/1a961d03da67a69eba64cef21cd3f374#file-ri-angle-vector-test-l
のようにして，
(send *ri* :angle-vector (send *pr2* :angle-vector))
をしても，(send *pr2* :angle-vector) と (send *ri* :state :potentio-vector) が一致しないことがあります．360度のズレなど．

その時に*pr2*で姿勢を作って:angle-vector-sequenceを送るとKinematics Simulatorでの動き出しがスローになることがあります．

2.irteusgl$ bag-test
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 10.9622 7.67527 -26.7165 -50.7115 -197.326 -50.1739 578.54 0.0 0.0)
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 10.9622 7.67527 -26.7165 -50.7115 -197.326 -50.1739 218.54 0.0 0.0)

のようにangle-vectorとpotentio-vectorに差がある時は手先を回転させるangle-vector-sequenceの動作の動き出しがゆっくりになって途中で加速します．

3.irteusgl$ bag-test
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 10.9622 7.67527 -26.7165 -50.7115 -197.326 -50.1739 578.54 0.0 0.0)
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 10.9622 7.67527 -26.7165 -50.7115 -197.326 -50.1739 578.54 0.0 0.0)

のようにangle-vectorとpotentio-vectorが一致している場合は動き出しがゆっくりになることなくスムーズに動作します．

4.irteusgl$ no-bag-test
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 10.9622 7.67527 -26.7165 -50.7115 -197.326 -50.1739 578.54 0.0 0.0)
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 10.9622 7.67527 -26.7165 -50.7115 -197.326 -50.1739 938.54 0.0 0.0)

angle-vector-sequenceを作る動作の前に(send *pr2* :angle-vector (send *ri* :state :potentio-vector))を入れることで回避することはできます．時間指定を正確にしたい場合は動作を作る前にangle-vectorとpotentio-vectorを一致させる必要があるのでしょうか？

[Naoki-Hiraoka]

(send *ri* :angle-vector (send *pr2* :angle-vector))をしても，(send *pr2* :angle-vector) と (send *ri* :state :potentio-vector) が一致しない理由についてはよくわかりませんが、
動き出しがゆっくりになる理由は、
:angle-vector-sequenceは、各区間について個別に関節の最大速度を超えないかチェックし、超える場合はその区間のtimeを最大速度を超えない値に自動的に延長する処理を行っていて、

jsk_pr2eus/pr2eus/robot-interface.l

Line 438 in a228cc7

if designated each tmn is faster than fastest speed, use fastest speed

jsk_pr2eus/pr2eus/robot-interface.l

Lines 537 to 547 in a228cc7

	(setq av (pop avs))
	(setq fastest-tm (* 1000 (send self :angle-vector-duration av-prev av scale min-time ctype)))
	(setq tm (pop tms))
	(cond
	((equal tm :fast)
	(setq tm fastest-tm))
	((null tm)
	(setq tm (* 5 fastest-tm)))
	((numberp tm)
	(if (< tm fastest-tm)
	(setq tm fastest-tm)))

最初の区間については現在のpotentio-vectorからの変位から計算される速度を用いて最大速度を超えないかチェックするため、

jsk_pr2eus/pr2eus/robot-interface.l

Line 454 in a228cc7

(av-prev (send self :state :potentio-vector)) av av-next

360度ずれていると、最初の区間の時間が (360度 / 関節最大速度) にまで自動的に延長されるためだと思います。

そのため、時間指定を正確にしたい場合は、

• 動作を作る前にangle-vectorとpotentio-vectorを一致させる

または、

• (send *ri* :angle-vector (send *pr2* :angle-vector))をしたら(send *pr2* :angle-vector) と (send *ri* :state :potentio-vector) が一致するようにPR2のコントローラ側を直す

または、

• jsk_pr2eus/pr2eus/robot-interface.l

  Line 454 in a228cc7

  (av-prev (send self :state :potentio-vector)) av av-next

  をpotentio-vectorではなくreference-vectorを使うようにする

のいずれかが必要になるのではないかと思います。

[Kanazawanaoaki]

ありがとうございます．
:angle-vector-sequenceに最大速度を超えないような処理があるなんて知りませんでした．遅くなっている原因はこの問題のような気がします．

一つ疑問なのは，:angle-vector-sequenceを行った後もangle-vectorとpotentio-vectorがズレているということです．

1.irteusgl$ bag-test
[ WARN] [1607691927.289927059]: continuous joint (r_wrist_roll_joint) moves 360.0 degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 10.9622 7.67527 -26.7165 -50.7115 -197.326 -50.1739 578.54 0.0 0.0)
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 10.9622 7.67527 -26.7165 -50.7115 -197.326 -50.1739 218.54 0.0 0.0)
(#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 13.6391 9.04789 -18.1237 -50.9708 -201.187 -48.688 617.97 0.0 0.0) #f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 15.9765 9.80238 -10.9912 -51.1208 -204.22 -47.2403 658.347 0.0 0.0) #f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 17.9582 10.1507 -5.09826 -51.1907 -206.624 -45.9109 699.527 0.0 0.0) #f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 19.6133 10.2457 -0.227961 -51.2075 -208.549 -44.7306 741.366 0.0 0.0) #f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 20.9852 10.1903 3.80556 -51.1927 -210.1 -43.7036 783.741 0.0 0.0) #f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 22.1181 10.0526 7.15377 -51.1623 -211.352 -42.8192 826.548 0.0 0.0) #f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 23.0558 9.87572 9.94929 -51.1267 -212.369 -42.0567 869.701 0.0 0.0) #f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 23.8352 9.68594 12.2957 -51.0914 -213.203 -41.3959 913.139 0.0 0.0))
(200 200 200 200 200 200 200 200)
(nil nil nil nil)
2.irteusgl$ (send *pr2* :angle-vector)
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 23.8352 9.68594 12.2957 -51.0914 -213.203 -41.3959 913.139 0.0 0.0)
3.irteusgl$ (send *ri* :state :potentio-vector)
#f(325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 23.8352 9.68594 12.2957 -51.0914 -213.203 -41.3959 553.139 0.0 0.0)

というように，angle-vector-sequenceを行った直後に確認してもズレています．
360度ズレている場合にはその分のズレを修正するために大きく動く必要があり遅くなっている，ならばangle-vector-sequenceを行った時には指令値と全く同じpotentio-vectorになっていると納得できますが，実際には360度のズレの修正は行われずにそのままズレているように思います．Kinematics Simulatorの問題なのかもしれませんが．

解決策としてはやはり，

• 動作を作る前にangle-vectorとpotentio-vectorを一致させる
  が現実的なのかなと思いました．

[Naoki-Hiraoka]

関連するissue
#1
jsk-ros-pkg/jsk_common#87

:angle-vectorや、:angle-vector-sequenceを行った後もangle-vectorとpotentio-vectorがズレている理由は、「無限回転関節」かつ「kinematics simulator」の場合固有の問題だと思います。多分実機では生じていないのではないでしょうか。

PR2実機のコントローラは、実機のコントローラは無限回転の場合，指令した角度にするのではなく、最小パスを通るように回転方向を決めるようになっている.
kinematics simulatorは、それと同じ挙動をするように作られている。

jsk_pr2eus/pr2eus/robot-interface.l

Line 937 in a228cc7

(:sub-angle-vector (v0 v1)

指令した角度にならないのは不便なので、:angle-vectorや:angle-vector-sequenceで180度を超える変位を実機に指令するときは経由点を追加するようにして、指令した角度になるようにしている。

jsk_pr2eus/pr2eus/pr2-interface.l

Line 419 in a228cc7

(when (send self :check-continuous-joint-move-over-180 diff-av)

jsk_pr2eus/pr2eus/pr2-interface.l

Line 442 in a228cc7

(when (send self :check-continuous-joint-move-over-180 diff-av)

ところが、kinematics simulatorでは経由点を追加しないままなので、指令した角度にならない。

jsk_pr2eus/pr2eus/pr2-interface.l

Line 426 in a228cc7

(unless (or (send self :simulation-modep) (cadr (memq :end-coords-interpolation args)))

kinematics simulatorでも経由点を追加するように、
#452 で直してみたので、このバージョンのpr2-interface.lをloadして試してみて欲しいです。

[Naoki-Hiraoka]

@708yamaguchi ご意見をお聞かせください。
#319
#388

[Kanazawanaoaki]

ありがとうございます．
#452 のpr2-interface.lを使うとangle-vectorとpotentio-vectorが一致するようになりました．
当然ですが，今回のプログラムの場合angle-vector-sequenceを送る前の時点でangle-vectorとpotentio-vectorが一致しているのでスローになる現象が起きませんでした．

[708yamaguchi]

@Naoki-Hiraoka

事情を深く理解できていなくて申し訳ないのですが、もともとKinematics Simulatorでは以下の2つの異なる問題があったという理解で良いでしょうか。

• (send *pr2* :angle-vector) と (send *ri* :state :potentio-vector)の値が異なる。
• 180度を超える変位を指令を送っても、経由点を追加しない。

#452 はこれら両方の問題を解消しているけど、このissueで話されている動き出しスロー現象は1つ目の問題を解決したことで解消された、ということでしょうか。

[Naoki-Hiraoka]

こちらのコメントにまとめました。

もともとKinematics Simulatorにあった問題は、実機はコメントのイなのに、kinematics simulatorはコメントのアであったという問題です。(180度を超える変位を指令を送っても、経由点を追加しない。)

B+アの組み合わせは相性が悪いので、(send *pr2* :angle-vector) と (send *ri* :state :potentio-vector)の値が異なりやすいです。(send *pr2* :angle-vector) と (send *ri* :state :potentio-vector)の値が360度異なると、動き出しスロー現象が生じます。

#452 は、kinematics simulatorを実機に合わせてイにすることで、これらを解消しました。

#452 後は(send *pr2* :angle-vector) と (send *ri* :state :potentio-vector)の値は同じになりますが、360度異なっていたら動き出しスロー現象が生じてしまうことには変わりは無いので、実機でもkinematics simulatorでもangle-vector-sequenceを作る動作の前に(send pr2 :angle-vector (send ri :state :potentio-vector))を入れた方が安心ということには変わりが無いです。360度異なっていても動き出しスロー現象が生じないようにするには、実機もkinematics simulatorもウにすることが必要だと思いました。