:wait-interpolation-smoothの待機時間

[k-kimura]

(send *ri* :wait-interpolation-smooth time-to-finish &optional (ctype))

の使い方ですが， http://euslisp-docs.readthedocs.io/en/latest/pr2eus/robot-interface/#wait-interpolation-smooth https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L514-521 などを参考にすると， 直前に送った指令(angle-vectorなど)が終了するtime-to-finish[msec]前まで， :wait-interpolationで待機する，という理解で合ってますでしょうか．

この理解を前提として，実機(シミュレータでない)のriに以下のコマンドを送って時間を測ってみると， 約4[s]くらいの値が返ってくる予想となりますが，ほとんど待機なしの早い値が返ってくる結果となりました． (ちなみに,今回の例ではhrp2jsk実機を用いてreset-pose -> reset-manip-poseのangle-vector遷移を参考としました．)

irteusgl$ (bench
(send *ri* :angle-vector (send *robot* :angle-vector) 5000) ;; 5[s]で遷移
(send *ri* :wait-interpolation-smooth 1000) ;; 遷移終了の1[s]前で:wait-interpolationから抜ける?
)
;; time -> 0.009716[s]
nil

[k-kimura]

また，参考までの情報ですが，

• :wait-interpolation-smooth
• :wait-interpolation
• unix:usleep

を使って，各実行時間を簡単に比較しました． (いずれもhrp2jsk実機でのreset-pose -> reset-manip-poseの変化です．)

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation-smooth 50))
;; time -> 0.018292[s]
nil

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation-smooth 1000))
;; time -> 0.020518[s]
nil

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation-smooth 5000))
;; time -> 0.023807[s]
nil

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation))
;; time -> 5.22598[s]

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (unix:usleep (* 5000 1000))) ;; unix:usleepを使って5[s]だけ待機
;; time -> 5.0199[s]

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (unix:usleep (round (* 0.8 (* 5000 1000))))) ;; unix:usleepを使って0.8*5[s]だけ待機
;; time -> 4.0195[s]

[k-okada]

それはrosbridgeがなにか間違えている可能性が高いです． :angle-vectorをおくるとjointtrajectoryactoinを送る，という部分はわかっているかな． であれば，そのactionのfeedbackを見て下さい．

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L514-L533 から https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L40-L52 あたりで経過時間をしらべています．

◉ Kei Okada

On Fri, Jun 17, 2016 at 11:00 PM, Kohei Kimura notifications@github.com wrote:

また，参考までの情報ですが，

• :wait-interpolation-smooth
• :wait-interpolation
• unix:usleep

を使って，各実行時間を簡単に比較しました． (いずれも_hrp2jsk_実機でのreset-pose -> reset-manip-poseの変化です．)

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 50)) ;; time -> 0.018292[s] nil

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 1000)) ;; time -> 0.020518[s] nil

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 5000)) ;; time -> 0.023807[s] nil

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation)) ;; time -> 5.22598[s]

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (unix:usleep (* 5000 1000))) ;; unix:usleepを使って5[s]だけ待機 ;; time -> 5.0199[s]

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (unix:usleep (round (* 0.8 (* 5000 1000))))) ;; unix:usleepを使って0.8*5[s]だけ待機 ;; time -> 4.0195[s]

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-226776228, or mute the thread https://github.com/notifications/unsubscribe/AAeG3Oeegow2l0J0O6Cm2oTfQEgb1WE-ks5qMqhigaJpZM4I4Yjc .

[k-okada]

feedback でていないですね．

◉ Kei Okada

On Sat, Jun 18, 2016 at 4:40 PM, Kei Okada k-okada@jsk.t.u-tokyo.ac.jp wrote:

それはrosbridgeがなにか間違えている可能性が高いです． :angle-vectorをおくるとjointtrajectoryactoinを送る，という部分はわかっているかな． であれば，そのactionのfeedbackを見て下さい．

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L514-L533 から

https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L40-L52 あたりで経過時間をしらべています．

◉ Kei Okada

On Fri, Jun 17, 2016 at 11:00 PM, Kohei Kimura notifications@github.com wrote:

また，参考までの情報ですが，

• :wait-interpolation-smooth
• :wait-interpolation
• unix:usleep

を使って，各実行時間を簡単に比較しました． (いずれも_hrp2jsk_実機でのreset-pose -> reset-manip-poseの変化です．)

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 50)) ;; time -> 0.018292[s] nil

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 1000)) ;; time -> 0.020518[s] nil

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 5000)) ;; time -> 0.023807[s] nil

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation)) ;; time -> 5.22598[s]

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (unix:usleep (* 5000 1000))) ;; unix:usleepを使って5[s]だけ待機 ;; time -> 5.0199[s]

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (unix:usleep (round (* 0.8 (* 5000 1000))))) ;; unix:usleepを使って0.8*5[s]だけ待機 ;; time -> 4.0195[s]

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-226776228, or mute the thread https://github.com/notifications/unsubscribe/AAeG3Oeegow2l0J0O6Cm2oTfQEgb1WE-ks5qMqhigaJpZM4I4Yjc .

[k-kimura]

デバッグ中ですが， https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L533 まできたときに

(0)

のリストが返ってきています．

ここでのcontroller-actionsは

(#<controller-action-client #X876a2f0 fullbody_controller/joint_trajectory_action>)

のリストで https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L40-L42 においてtime-to-finishをはき出していますが， 前の

(setq time-to-finish 0)

がそのまま返ってきている様子です．

ros_bridgeを立ち上げた後に

rostopic echo /fullbody_controller/joint_trajectory_action/feedback

を確認するのが正しいか怪しいですが， 手元のhrpsys simulator + hrp2実機(hrp20xxv)共に確かにこのトピックは出ていないですね．

[k-kimura]

@pazeshun :wait-interpolation-smoothで待つことができるというBaxter系の環境では

/fullbody_controller/joint_trajectory_action/feedback

のトピックははき出されていますでしょうか？

[k-okada]

なるほど．これは案外難しかったですね． https://github.com/start-jsk/rtmros_common/pull/957 して https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/235 したらいけるとおもいます．ところで，いまつかっているのはfullbody？fullbodyだけpr2_controllers_msgsになって，larm_controllerとかつかうとcontrollers_msgs になるのかな？ https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L773 を直してもいいのかな？

@pazeshun の問題は https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236 で解決するかな．

◉ Kei Okada

On Mon, Jun 20, 2016 at 6:09 PM, Kohei Kimura notifications@github.com wrote:

@pazeshun https://github.com/pazeshun :wait-interpolation-smoothで待つことができるというBaxter系の環境では

/fullbody_controller/joint_trajectory_action/feedback

のトピックははき出されていますでしょうか？

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-227088987, or mute the thread https://github.com/notifications/unsubscribe/AAeG3FAay0P-tcy0_VTEH8NeKjdCifCXks5qNljPgaJpZM4I4Yjc .

[pazeshun]

@k-kimura Baxter環境では、/robot/limb/right/follow_joint_trajectory/feedbackとかなのですが、これはちゃんと出ていることを確認しています。一時期グリッパーの方のjtaのfeedbackがまちがっていたのですが、その時は永遠に終わらなかったです。( #231 )

[k-kimura]

start-jsk/rtmros_common#957

235

の変更を加えたところ，angle-vectorを送っている間だけ

rostopic echo /fullbody_controller/joint_trajectory_action/feedback

のfeedbackが返ってきました． (hrpsys simulatorで確認)

一方で:wait-interpolation-smoothの待ち時間はまだ早いままなので， time-to-finishあたりを確認してみます．

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation-smooth 2000))
;; time -> 0.149312[s]
nil

ところで，いまつかっているのはfullbody？

はい，現状は

(send *ri* :wait-interpolation-smooth 2000)

としてctype(controller-type)を特に指定せずに fullbodyでチェックしています．

irteusgl$ (send *ri* :get-val 'controller-actions)
(#<controller-action-client #X8250c40 fullbody_controller/joint_trajectory_action>)

また，@pazeshun への#236 の変更も加えて試すと，

231 のissueと同じく永久に終わらない症状が出てしまいました．

[k-okada]

235 と #236 と両方含まれているコードをつかっている？

◉ Kei Okada

2016-06-20 21:48 GMT+09:00 Kohei Kimura notifications@github.com:

start-jsk/rtmros_common#957 https://github.com/start-jsk/rtmros_common/pull/957

235 https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/235

の変更を加えたところ，angle-vectorを送っている間だけ

rostopic echo /fullbody_controller/joint_trajectory_action/feedback

のfeedbackが返ってきました． (hrpsys simulatorで確認)

一方で:wait-interpolation-smoothの待ち時間はまだ早いままなので， time-to-finishあたりを確認してみます．

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 2000)) ;; time -> 0.149312[s] nil

ところで，いまつかっているのはfullbody？

はい，現状は

(send ri :wait-interpolation-smooth 2000)

としてctype(controller-type)を特に指定せずに fullbodyでチェックしています．

irteusgl$ (send ri :get-val 'controller-actions) (#<controller-action-client #X8250c40 fullbody_controller/joint_trajectory_action>)

また，@pazeshun https://github.com/pazeshun への#236 https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236 の変更も加えて試すと，

231 https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/231

のissueと同じく永久に終わらない症状が出てしまいました．

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-227132500, or mute the thread https://github.com/notifications/unsubscribe/AAeG3NYkAOtOJe6s9FzhsW1mfRlqlyalks5qNowTgaJpZM4I4Yjc .

[k-kimura]

はい，#235 と #236 の両方の変更を加えたrobot-interface.lをloadして

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation-smooth 1000))

を試すと，returnせずに抜け出せない状況です． (/fullbody_controller/joint_trajecty_action/feedbackは5秒間だけ流れてきています．)

[k-okada]

ループしているのは https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236/files#diff-15f6920b924513f6027d36b361f85bbdR527 だろうか．

(while (some #'(lambda (x) (print (list tm (send x :last-feedback-msg-stamp)) (<= (send (ros::time- (send x :last-feedback-msg-stamp) tm) :to-sec) 0)) cacts) にしたらどうなるかな＞

◉ Kei Okada

On Mon, Jun 20, 2016 at 10:08 PM, Kohei Kimura notifications@github.com wrote:

はい，#235 https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/235 と #236 https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236 の両方の変更を加えたrobot-interface.lをloadして

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 1000))

を試すと，returnせずに抜け出せない状況です． (/fullbody_controller/joint_trajecty_action/feedbackは5秒間だけ流れてきています．)

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-227136983, or mute the thread https://github.com/notifications/unsubscribe/AAeG3N0MpXqbb0zPrDiM_gzyps2hx9Dwks5qNpDkgaJpZM4I4Yjc .

[k-kimura]

ループ箇所を変更した後の結果です．

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation-smooth 1000))
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>)
(以下ループ)

[k-okada]

あ， https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236/files#diff-15f6920b924513f6027d36b361f85bbdR47 の後ろに (setq last-feedback-msg-stamp (ros::time-now)) がないとダメじゃない．

◉ Kei Okada

On Mon, Jun 20, 2016 at 10:28 PM, Kohei Kimura notifications@github.com wrote:

ループ箇所を変更した後の結果です．

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 1000)) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (以下ループ)

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-227141757, or mute the thread https://github.com/notifications/unsubscribe/AAeG3NkfHd6_l0ABqmZIi29Q97ww9q1lks5qNpWDgaJpZM4I4Yjc .

[k-okada]

ちがうや， (:action-feedback-cb (msg) (let ((finish-time) (current-time)) (setq last-feedback-msg-stamp (send msg :header :stamp)) (ros::ros-debug "[~A] feedback-cb ~A" ros::name-space msg) が正解

◉ Kei Okada

On Mon, Jun 20, 2016 at 10:34 PM, Kei Okada k-okada@jsk.t.u-tokyo.ac.jp wrote:

あ，

https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236/files#diff-15f6920b924513f6027d36b361f85bbdR47 の後ろに (setq last-feedback-msg-stamp (ros::time-now)) がないとダメじゃない．

◉ Kei Okada

On Mon, Jun 20, 2016 at 10:28 PM, Kohei Kimura notifications@github.com wrote:

ループ箇所を変更した後の結果です．

irteusgl$ (bench (send ri :angle-vector (send robot :angle-vector) 5000) (send ri :wait-interpolation-smooth 1000)) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (#<ros::time #X934aeb8 5732.027> #<ros::time #X8323340 5706.815>) (以下ループ)

— You are receiving this because you commented. Reply to this email directly, view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-227141757, or mute the thread https://github.com/notifications/unsubscribe/AAeG3NkfHd6_l0ABqmZIi29Q97ww9q1lks5qNpWDgaJpZM4I4Yjc .

[k-kimura]

https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236/files#diff-15f6920b924513f6027d36b361f85bbdR48 を加えることで，:wait-interpolation-smoothで待機できるようになりました． ありがとうございます．

irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation-smooth 1000))
;; time -> 5.48133[s]
nil
irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation-smooth 4000))
;; time -> 1.54616[s]
nil
irteusgl$ (bench (send *ri* :angle-vector (send *robot* :angle-vector) 5000) (send *ri* :wait-interpolation))
;; time -> 5.91715[s]
(nil)

[k-okada]

https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236 さらに変更しました．バグをいれていないか確認してもらえると嬉しいです @k-kimura

[k-kimura]

https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/236/commits/a8923e549954f62147ceb1ca51ea38009d841e0c の変更を加えても大丈夫そうでした．

[pazeshun]

こちらに関係した問題ですが、HIRONXで:wait-interpolation-smoothを使おうとしたら永久に終わりませんでした。 fullbodyを使ってやっていますが、以前の @k-kimura さんの状況とは異なりFollowJointTrajectoryActionなので、 https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.13/pr2eus/robot-interface.l#L48-L49 のようにfeedbackのerrorのtime_from_startを読み込んで、今どこまで実行されたかを取得しています。 ところが、この値がどうなっているか調べて見ると、

$ rostopic echo /fullbody_controller/follow_joint_trajectory_action/feedback/feedback/error/time_from_start

secs: 0
nsecs:         0
---

常に0が入っており、これによりいつまで経っても:wait-interpolation-smoothから抜けられないものと思われます。

@k-kimura さんの状況では、https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/0.3.13/pr2eus/robot-interface.l#L53 のように、(ros::time-now)から今どこまで実行されたかを計算するので、time_from_startが0でも問題が起こらなかったのだと思います。

[pazeshun]

@k-kimura 現在使っているHRP2ではFullbodyでもFollowJointTrajectoryActionを使っていますでしょうか。もしそうなら、:wait-interpolation-smoothが動かなくなっているのではないかと思うのですが、どうでしょうか。

[k-kimura]

最近更新した最新版のHRP2環境では， angle-vector送ったときに /fullbody_controller/joint_trajectory_action/feedback ではなく /fullbody_controller/follow_joint_trajectory_action/feedback のactionlibが応答していて，確かにfollow_joint_trajectory_actionの場合は :wait-interpolation-smoothから永遠に抜けられないようです．

以前に自分が:wait-interpolation-smoothが使えるのを確認したのは まだjoint_trajectory_actionをdefault-controllerとして採用していた頃で https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/237 の議論を経て， https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/250 の変更が加わってからはfollow_joint_trajectory_actionを使うようになったので :wait-interpolation-smoothが動かなくなってしまったんだとおもいます．

[pazeshun]

@k-kimura https://github.com/start-jsk/rtmros_common/pull/1049 で多分治ったと思うので、試していただけると嬉しいです。

[k-kimura]

治ったようです． benchで計測した待機時間も正しそうです．

[pazeshun]

いろいろ考えた末、 #356 を作りました。 これで、https://github.com/start-jsk/rtmros_common/pull/1049 なしでも:wait-interpolation-smoothが動くと思います。 ただ、https://github.com/start-jsk/rtmros_common/pull/1052 なしだと、:angle-vectorにstart-timeを渡した場合、動作では無視されるが:wait-interpolation-smoothでは無視されず、start-time分待機が長くなる挙動になります。