furushchev commented 8 years ago

:wait-interpolationについて、

robot-interface.lのdocstring https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L478 では values is a list of interpolatingp for all controllers, so (null (some #'identity (send *ri* :wait-interpolation))) -> t if all interpolation has stopped とあります。これはつまりtimeoutが0(無限大)で:wait-interpolationされたときに必ず(nil nil nil nil)になるという想定 testコードは https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/test/default-ri-test.l#L29
実機につないだ時のwait-interpolation https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L487 では:wait-interpolationの返り値は(send-all controller-actions :wait-for-result :timeout timeout) となっています。
- values is a list of interpolatingp for all controllersでは無いように思います。
- これは:wait-for-resultはresultが返ってくるとtを返す関数なので、actionがabortされない限りtを返すと思います。さらに1回でもactionを送ると、次からはtが返ります。(直前のgoalに対するwait-for-resultの結果)

pr2実機の場合

$ roscd pr2eus
$ roseus pr2-interface.l
$ pr2-init
$ send *ri* :wait-interpolation
[ INFO] [1448030250.007490532]: wait-interpolation debug: start
[ INFO] [1448030250.007857212]: wait-interpolation debug: end
(nil nil nil nil)
$ send *ri* :angle-vector (send *pr2* :reset-pose) 10000
#f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)
$ send *ri* :wait-interpolation
[ INFO] [1448030550.250459313]: wait-interpolation debug: start
[ INFO] [1448030552.680471962]: wait-interpolation debug: end
(t t t t)   ;; docstringと違う？
$ send *ri* :wait-interpolation
[ INFO] [1448030579.632002770]: wait-interpolation debug: start
[ INFO] [1448030579.632373273]: wait-interpolation debug: end
(t t t t)  ;; docstringと違う?

\3. 実機なしのwait-interpolation https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/master/pr2eus/robot-interface.l#L482 となっていて、docstringに従っている

simulationの場合

$ roscd pr2eus
$ roseus pr2-interface.l
$ pr2-init
$ send *ri* :wait-interpolation
(nil nil nil nil)
$ send *ri* :angle-vector (send *pr2* :reset-pose) 10000
#f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -60.0 74.0 -70.0 -120.0 -20.0 -30.0 180.0 0.0 0.0)
$ send *ri* :wait-interpolation
(nil nil nil nil)
$ send *ri* :wait-interpolation
(nil nil nil nil)

ちなみに https://github.com/jsk-ros-pkg/jsk_demos/blob/master/jsk_demo_common/euslisp/attention-observation.l#L29 などではwait-interpolationが終わると(t t t t)を返すような想定になっています。どちらを変えるのが正解でしょうか？

YoheiKakiuchi commented 8 years ago

docstringに合わせるのがAPIとしてスッキリしていると思いますが、実機でwait-interpolatinの結果を見て何かしているところに影響がありますね。 pr2/hrp2/urata系のすべてに影響が及びます。

それで、実機の場合のwait-interpolationの結果はwait-for-resultの結果ではなくて、 send *ri* :interpolatingpの結果を返すのがいいかと思います。 docstring通りですし、interpolatingpの結果は説明通りのように見えます。

k-okada commented 8 years ago

おそくなりました．

うーん，いまいちですね．ただ，:wait-interpolation でかえるのは，wait-for-result だとして，これがnil になるのは， followtrajectoryactionでエラーが大きくなった時か， https://github.com/start-jsk/rtmros_common/pull/765 がmergeされてて，canel させた時しか無いのではないでしょうか．

つまり，hrp2/urata系には影響なく，影響あるのはpr2だけかと．

ところで，pr2ではどの場面でこれがもんだいになりましたか．

◉ Kei Okada

2015-11-21 15:12 GMT+09:00 Yohei Kakiuchi notifications@github.com:

docstringに合わせるのがAPIとしてスッキリしていると思いますが、実機でwait-interpolatinの結果を見て何かしているところに影響がありますね。 pr2/hrp2/urata系のすべてに影響が及びます。

それで、実機の場合のwait-interpolationの結果はwait-for-resultの結果ではなくて、 send ri :interpolatingpの結果を返すのがいいかと思います。 docstring通りですし、interpolatingpの結果は説明通りのように見えます。

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/187#issuecomment-158593557 .

furushchev commented 8 years ago

ところで，pr2ではどの場面でこれがもんだいになりましたか．

例えば

https://github.com/jsk-ros-pkg/jsk_demos/blob/master/jsk_demo_common/euslisp/attention-observation.l#L29 などではwait-interpolationが終わると(t t t t)を返すような想定になっています。

ではsimulationの場合wait-interpolation中に必ず中断が入る挙動になってしまいます。 jsk_demo_commonのテストコードを書こうとしておかしいなと気づきました。

k-okada commented 8 years ago

なるほど．:wait-interplationの結果に依存するコードはないということでいいですか．

実機の場合のwait-interpolationの結果はwait-for-resultの結果ではなくて、 send ri :interpolatingpの結果を返すのがいいかと思います。

でやりましょう．

◉ Kei Okada

2015-11-23 10:37 GMT+09:00 Furushchev notifications@github.com:

ところで，pr2ではどの場面でこれがもんだいになりましたか．

例えば

https://github.com/jsk-ros-pkg/jsk_demos/blob/master/jsk_demo_common/euslisp/attention-observation.l#L29 などではwait-interpolationが終わると(t t t t)を返すような想定になっています。

ではsimulationの場合wait-interpolation中に必ず中断が入る挙動になってしまいます。 jsk_demo_commonのテストコードを書こうとしておかしいなと気づきました。

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/187#issuecomment-158826538 .

snozawa commented 8 years ago

こちらですが、pr2がやわらくてangle vectorをおおくってなにかにぶつかってとまり、そのときの間接エラーがおおきいとき、wait-interpolationがnilになったりするでしょうか。

k-okada commented 8 years ago

はい．そう期待しています．実際にどうなるか，明示的に確認したことはあまりない気がするので，一回試したらいいですね　＞ @furushchev @h-kamada @chiwunau @himlen1990

snozawa commented 8 years ago

初歩的な質問になりますが，

実際はエラーはどのがプログラムどのようにして判定してしてるでしょうか．実関節角度値はjoint_statesでしょうか
HrpsysROSBridge系であればどれがそれになるでしょうか．今も（正しく動いてたら）:wait-interpolationがHrpssyROSBridgeから返った時に判定結果がえられるのでしょうか．

furushchev commented 8 years ago

実際にどうなるか，明示的に確認したことはあまりない気がするので，一回試したらいいですね

37.irteusgl$ (send *pr2* :rarm :move-end-pos #f(0 0 500) *pr2*)
#f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -12.4833 16.5145 -51.6378 -76.3784 58.4912 -23.4754 122.546 -24.5457\
 59.33)
38.irteusgl$ (progn (send *ri* :angle-vector (send *pr2* :angle-vector)) (send *ri* :wait-interpolation))
[ INFO] [1448786993.901075475]: wait-interpolation debug: start
[ INFO] [1448786997.010105078]: wait-interpolation debug: end
(t t t t)

などとして(アームの上方向300mmくらいに机がある環境で500mm上にend-coordsを動かしてみる。)試してみたところ、アームに関しては机と衝突して目標位置に行けなくてもtが帰ってきているように思います。返り値自体は各コントローラがresultを返したかどうかという意味でのtでしょうか。アームに司令を送り、他のノードからcancelを送るとnilが帰ってくるのは確認しています。

k-okada commented 8 years ago

pr2/ros 系の実装は https://github.com/ros-controls/ros_controllers/blob/4508d8797b245004c6aa3fa5722eb8e23a27208d/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h#L200 のあたりだともいます． hrpsys ros bridgeではやっていないと思いますので，やるべきという声は有りますか？という話でした． https://github.com/start-jsk/rtmros_common/pull/765 も，含めて，必要，ということがあれば，issueを抱いておいて下さい．

◉ Kei Okada

2015-11-29 17:53 GMT+09:00 Furushchev notifications@github.com:

実際にどうなるか，明示的に確認したことはあまりない気がするので，一回試したらいいですね

37.irteusgl$ (send pr2 :rarm :move-end-pos #f(0 0 500) pr2)

f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -12.4833 16.5145 -51.6378 -76.3784 58.4912 -23.4754 122.546 -24.5457\

59.33) 38.irteusgl$ (progn (send ri :angle-vector (send pr2 :angle-vector)) (send ri :wait-interpolation)) [ INFO] [1448786993.901075475]: wait-interpolation debug: start [ INFO] [1448786997.010105078]: wait-interpolation debug: end (t t t t)

などとして(アームの上方向300mmくらいに机がある環境で500mm上にend-coordsを動かしてみる。)試してみたところ、アームに関しては机と衝突して目標位置に行けなくても tが帰ってきているように思います。返り値自体は各コントローラがresultを返したかどうかという意味でのtでしょうか。アームに司令を送り、他のノードからcancelを送るとnilが帰ってくるのは確認しています。

— Reply to this email directly or view it on GitHub https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/187#issuecomment-160391977 .

furushchev commented 8 years ago

https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/191 のように関数をdocumentに従うように修正して見ました。おかしかったらご指摘お願いします。

k-okada commented 7 years ago

ちなみに、:wait-interpolation-smoothの挙動はどうでしょうか？docstringはReturn value is a list of interpolatingp for all controllersですが、そうなっていないような気がします．

k-okada commented 7 years ago

:interpolating-smoothpの挙動もdocstringと異なり常にnilでしょうか．

furushchev commented 7 years ago

:wait-interpolation-smoothはPR2では使われていないのでわからないですが、:wait-interpolationと同じ場所で使われるなら揃えたほうがいいのではないでしょうか？ (CCでmentionしたらいい人もわからない。。。 @wkentaro ?)

furushchev commented 7 years ago

（これテストしたいところですが、gazeboをテストに入れると途端にテストの速度低下＆＆エラー率が上がるので、何とかしたいところですね。。。）

knorth55 commented 7 years ago

@pazeshunがsmoothを使っています

pazeshun commented 7 years ago

smoothの戻り値をチェックしたことがないので、わからないです。

pazeshun commented 6 years ago

ちなみに、:wait-interpolation-smoothの挙動はどうでしょうか？docstringはReturn value is a list of interpolatingp for all controllersですが、そうなっていないような気がします．

:interpolating-smoothpの挙動もdocstringと異なり常にnilでしょうか．

大変遅くなりましたが、 #356 を作っていて、:wait-interpolation-smoothについて色々確認しました。 @k-okada のご指摘通り、:wait-interpolation-smoothが終了した際には常にnilを返すようになっていましたので、 #356 で:wait-interpolationと同じくdocstringに従うようにしました。

なお、angle-vectorを送った後で(send *ri* :wait-interpolation-smooth 500)と送った場合は、補間終了の500ms前に関数から抜けることになるので、返り値はtを含んだlistになると予想されますが、まだ実機で確認していません。この変化に伴い、testも修正しました。

pazeshun commented 6 years ago

351 #352 がどうmergeされるかによって、`:wait-interpolation`の仕様が変わりそうですが、変わった場合は`:wait-interpolation-smooth`もそれに合わせていこうと思います。

furushchev commented 6 years ago

放置していましたが，まとめとして，

docstringに書いてある

return values is a list of interpolatingp for all controllers, so (null (some #'identity (send ri :wait-interpolation))) -> t if all interpolation has stopped

を原則として，

:wait-interpolationはactionlibを想定して，:angle-vectorなどを使って，action serverに投げたgoalに対して，resultが返ってくるまで待つ．ということになる. (i.e. :wait-interpolationはすべてのresultを待ってcontrollerの数分nilの入ったlistを返す)
ただし:wait-interpolationに対してtimeoutが実際にresultが返ってくるまでの時間より短く与えられた時にはresultが帰ってきていないcontrollerに対してtが返る．

のような挙動にすることになったという理解で良いでしょうか．

また， :wait-interpolation-smoothなどの関連メソッドも:wait-interpolationに合わせるので問題ないでしょうか．

@YoheiKakiuchi @pazeshun @Naoki-Hiraoka @knorth55 ?

pazeshun commented 3 years ago

@k-okada @YoheiKakiuchi こちらのissueで議論されていた:wait-interpolationの返り値ですが、controller-typeが指定されていた場合にはどうあるべきか、についてご意見いただけないでしょうか。現在は、実装・docstringともに、controller-typeが指定されていても全てのcontrollerのinterpolatingpが返ってくるようになっていて、:wait-interpolationの関連メソッドもそのようにしようということになっているのですが、controller-typeが指定されていた場合はそのinterpolatingpのみが返ってくるようにしたいです。

事情としては、以下の通りです。

444 で、`:wait-interpolation`の関連メソッドを作ろうとして、その返り値のリストに`t`が含まれているかどうかで補間中なのか補間終了したのかを判断しようとしていたのですが、`controller-type`が指定されていた場合にも全てのcontrollerの`interpolatingp`が返ってくるので、興味のあるcontroller以外に補間中のcontrollerが一つでもあると、返り値のリストには必ず`t`が含まれてしまい、興味のあるcontrollerだけを判断できない、ということに気づきました。

メインのrobot-interfaceノード以外に、頭だけ別ノードからも動かしている、みたいな時に困ることが予想されます（レアケースかもしれないですが・・・）。

PR2実機では以下のようにチェックできます。

$ roscd pr2eus
$ roseus pr2-interface.l
$ pr2-init
$ progn (send *ri* :angle-vector (send *pr2* :reset-pose) 3000) (send *ri* :wait-interpolation)  ;; 初期化
$ send *ri* :wait-interpolation
[ INFO] [1628079905.249275561]: wait-interpolation debug: start
[ INFO] [1628079905.297132332]: wait-interpolation debug: end
(nil nil nil nil)  ;; 全てのcontroller
$ send *ri* :wait-interpolation :rarm-controller
[ INFO] [1628079975.218505638]: wait-interpolation debug: start
[ INFO] [1628079975.256166219]: wait-interpolation debug: end
(nil nil nil nil)  ;; この場合も全てのcontroller
$ progn (send *ri* :angle-vector (send *pr2* :init-pose) 10000 :larm-controller) (send *ri* :angle-vector (send *pr2* :init-pose) 3000 :rarm-controller) (some #'identity (send *ri* :wait-interpolation :rarm-controller))
...
t  ;; :rarm-controllerの補間は終わっているが、:larm-controllerの補間が終わっていないため、返り値のリストにtが入っている
$ progn (send *ri* :angle-vector (send *pr2* :reset-pose) 3000) (send *ri* :wait-interpolation)  ;; 初期化
$ progn (send *ri* :angle-vector (send *pr2* :init-pose) 10000 :larm-controller) (send *ri* :angle-vector (send *pr2* :init-pose) 3000 :rarm-controller) (some #'identity (send *ri* :wait-interpolation :rarm-controller 1.0))
...
t  ;; :wait-interpolationがtimeoutした場合でも区別できない

jsk-ros-pkg / jsk_pr2eus

:wait-interpolationの正しい挙動 #187

f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -12.4833 16.5145 -51.6378 -76.3784 58.4912 -23.4754 122.546 -24.5457\

351 #352 がどうmergeされるかによって、`:wait-interpolation`の仕様が変わりそうですが、変わった場合は`:wait-interpolation-smooth`もそれに合わせていこうと思います。

jsk-ros-pkg / jsk_pr2eus

:wait-interpolationの正しい挙動 #187

f(50.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -12.4833 16.5145 -51.6378 -76.3784 58.4912 -23.4754 122.546 -24.5457\

351 #352 がどうmergeされるかによって、:wait-interpolationの仕様が変わりそうですが、変わった場合は:wait-interpolation-smoothもそれに合わせていこうと思います。

351 #352 がどうmergeされるかによって、`:wait-interpolation`の仕様が変わりそうですが、変わった場合は`:wait-interpolation-smooth`もそれに合わせていこうと思います。