Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

:wait-interpolation-smoothの待機時間 #233

Open
k-kimura opened this issue Jun 17, 2016 · 23 comments
Open

:wait-interpolation-smoothの待機時間 #233

k-kimura opened this issue Jun 17, 2016 · 23 comments

Comments

@k-kimura
Copy link

(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
Copy link
Author

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

  • :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
Copy link
Member

k-okada commented Jun 18, 2016

それは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 [email protected]
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
#233 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/AAeG3Oeegow2l0J0O6Cm2oTfQEgb1WE-ks5qMqhigaJpZM4I4Yjc
.

@k-okada
Copy link
Member

k-okada commented Jun 20, 2016

feedback でていないですね.

◉ Kei Okada

On Sat, Jun 18, 2016 at 4:40 PM, Kei Okada [email protected]
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 [email protected]
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
#233 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/AAeG3Oeegow2l0J0O6Cm2oTfQEgb1WE-ks5qMqhigaJpZM4I4Yjc
.

@k-kimura
Copy link
Author

デバッグ中ですが,
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
Copy link
Author

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

/fullbody_controller/joint_trajectory_action/feedback

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

@k-okada
Copy link
Member

k-okada commented Jun 20, 2016

なるほど.これは案外難しかったですね.
start-jsk/rtmros_common#957
して
#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 の問題は
#236
で解決するかな.

◉ Kei Okada

On Mon, Jun 20, 2016 at 6:09 PM, Kohei Kimura [email protected]
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
#233 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/AAeG3FAay0P-tcy0_VTEH8NeKjdCifCXks5qNljPgaJpZM4I4Yjc
.

@pazeshun
Copy link
Collaborator

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

@k-kimura
Copy link
Author

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
Copy link
Member

k-okada commented Jun 20, 2016

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

◉ Kei Okada

2016-06-20 21:48 GMT+09:00 Kohei Kimura [email protected]:

start-jsk/rtmros_common#957
start-jsk/rtmros_common#957
#235 #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
#236 の変更も加えて試すと,
#231 #231
のissueと同じく永久に終わらない症状が出てしまいました.


You are receiving this because you commented.
Reply to this email directly, view it on GitHub
#233 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/AAeG3NYkAOtOJe6s9FzhsW1mfRlqlyalks5qNowTgaJpZM4I4Yjc
.

@k-kimura
Copy link
Author

はい,#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
Copy link
Member

k-okada commented Jun 20, 2016

ループしているのは
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 [email protected]
wrote:

はい,#235 #235#236
#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
#233 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/AAeG3N0MpXqbb0zPrDiM_gzyps2hx9Dwks5qNpDkgaJpZM4I4Yjc
.

@k-kimura
Copy link
Author

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

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
Copy link
Member

k-okada commented Jun 20, 2016

あ,
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 [email protected]
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
#233 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/AAeG3NkfHd6_l0ABqmZIi29Q97ww9q1lks5qNpWDgaJpZM4I4Yjc
.

@k-okada
Copy link
Member

k-okada commented Jun 20, 2016

ちがうや,
(: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 [email protected]
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 [email protected]
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
#233 (comment),
or mute the thread
https://github.com/notifications/unsubscribe/AAeG3NkfHd6_l0ABqmZIi29Q97ww9q1lks5qNpWDgaJpZM4I4Yjc
.

@k-kimura
Copy link
Author

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
Copy link
Member

k-okada commented Jun 20, 2016

#236
さらに変更しました.バグをいれていないか確認してもらえると嬉しいです @k-kimura

@k-kimura
Copy link
Author

a8923e5 の変更を加えても大丈夫そうでした.

@pazeshun
Copy link
Collaborator

pazeshun commented Jul 9, 2018

こちらに関係した問題ですが、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
Copy link
Collaborator

pazeshun commented Jul 9, 2018

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

@k-kimura
Copy link
Author

k-kimura commented Jul 9, 2018

最近更新した最新版の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として採用していた頃で
#237
の議論を経て,
#250
の変更が加わってからはfollow_joint_trajectory_actionを使うようになったので
:wait-interpolation-smoothが動かなくなってしまったんだとおもいます.

@pazeshun
Copy link
Collaborator

pazeshun commented Jul 9, 2018

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

@k-kimura
Copy link
Author

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

@pazeshun
Copy link
Collaborator

pazeshun commented Jul 13, 2018

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants