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

furushchev · 2015-11-20T14:46:57Z

: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)を返すような想定になっています。
どちらを変えるのが正解でしょうか？

The text was updated successfully, but these errors were encountered:

YoheiKakiuchi · 2015-11-21T06:12:14Z

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

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

k-okada · 2015-11-23T01:34:10Z

おそくなりました．

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

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

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

◉ Kei Okada

2015-11-21 15:12 GMT+09:00 Yohei Kakiuchi [email protected]:

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
#187 (comment)
.

furushchev · 2015-11-23T01:37:56Z

ところで，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 · 2015-11-24T03:43:45Z

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

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

でやりましょう．

◉ Kei Okada

2015-11-23 10:37 GMT+09:00 Furushchev [email protected]:

ところで，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
#187 (comment)
.

snozawa · 2015-11-28T03:12:26Z

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

k-okada · 2015-11-28T03:33:46Z

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

snozawa · 2015-11-28T06:33:19Z

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

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

furushchev · 2015-11-29T08:53:41Z

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

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 · 2015-11-29T09:36:04Z

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ではやっていないと思いますので，やるべきという声は有りますか？という話でした．
start-jsk/rtmros_common#765
も，含めて，必要，ということがあれば，issueを抱いておいて下さい．

◉ Kei Okada

2015-11-29 17:53 GMT+09:00 Furushchev [email protected]:

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

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
#187 (comment)
.

furushchev · 2015-11-30T03:11:40Z

#191 のように関数をdocumentに従うように修正して見ました。
おかしかったらご指摘お願いします。

k-okada · 2017-05-22T11:31:43Z

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

k-okada · 2017-05-23T01:12:07Z

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

furushchev · 2017-05-23T01:33:07Z

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

furushchev · 2017-05-23T01:35:44Z

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

knorth55 · 2017-05-23T01:51:40Z

@pazeshunがsmoothを使っています

pazeshun · 2017-05-23T02:19:43Z

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

pazeshun · 2018-07-14T04:02:45Z

ちなみに、: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 · 2018-07-14T04:36:26Z

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

furushchev · 2018-07-31T13:41:42Z

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

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 · 2021-08-04T12:35:40Z

@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した場合でも区別できない

furushchev mentioned this issue Nov 30, 2015

[pr2eus/robot-interface.l, pr2eus/pr2-interface.l] fix: :wait-interpolation returns :interpolatingp on real robot #191

Merged

pazeshun mentioned this issue Oct 6, 2020

[WIP][pr2eus/robot-interface.l] Add method waiting until last sent motion is finished or given function returns t #444

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

furushchev commented Nov 20, 2015

YoheiKakiuchi commented Nov 21, 2015

k-okada commented Nov 23, 2015

furushchev commented Nov 23, 2015

k-okada commented Nov 24, 2015

snozawa commented Nov 28, 2015

k-okada commented Nov 28, 2015

snozawa commented Nov 28, 2015

furushchev commented Nov 29, 2015

k-okada commented Nov 29, 2015

furushchev commented Nov 30, 2015

k-okada commented May 22, 2017

k-okada commented May 23, 2017

furushchev commented May 23, 2017

furushchev commented May 23, 2017

knorth55 commented May 23, 2017

pazeshun commented May 23, 2017

pazeshun commented Jul 14, 2018 •

edited

Loading

pazeshun commented Jul 14, 2018

furushchev commented Jul 31, 2018 •

edited

Loading

pazeshun commented Aug 4, 2021

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

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

Comments

furushchev commented Nov 20, 2015

YoheiKakiuchi commented Nov 21, 2015

k-okada commented Nov 23, 2015

furushchev commented Nov 23, 2015

k-okada commented Nov 24, 2015

snozawa commented Nov 28, 2015

k-okada commented Nov 28, 2015

snozawa commented Nov 28, 2015

furushchev commented Nov 29, 2015

k-okada commented Nov 29, 2015

furushchev commented Nov 30, 2015

k-okada commented May 22, 2017

k-okada commented May 23, 2017

furushchev commented May 23, 2017

furushchev commented May 23, 2017

knorth55 commented May 23, 2017

pazeshun commented May 23, 2017

pazeshun commented Jul 14, 2018 • edited Loading

pazeshun commented Jul 14, 2018

furushchev commented Jul 31, 2018 • edited Loading

pazeshun commented Aug 4, 2021

pazeshun commented Jul 14, 2018 •

edited

Loading

furushchev commented Jul 31, 2018 •

edited

Loading