【robot-interface.l】fix return value of :wait-interpolation for real robot #351

Naoki-Hiraoka · 2018-06-08T07:17:16Z

#191
(null (some #'identity (send ri :wait-interpolation))) -> t if all interpolation has stopped
という使い方ができると期待し、

(defun touch ()
  ;;armが障害物にぶつかるか、一定距離動くまで動かす関数
  (send *ri* :angle-vector #f(...) 5000)
  (while (some #'identity (send *ri* :wait-interpolation nil 0.1))
    (when (障害物にぶつかった)
      (send *ri* :stop-angle-vector)
      (return-from touch t)))
  nil))

としたところ、ループを1周目の前に抜けてしまいました。
原因は、

jsk_pr2eus/pr2eus/robot-interface.l

Line 598 in 1d92322

(send-all controller-actions :interpolatingp))

で:wait-interpolationは(send-all controller-actions :interpolatingp)を返すことになっていますが、

jsk_pr2eus/pr2eus/robot-interface.l

Line 84 in 1d92322

(:interpolatingp () (not (null timer-sequence)))

で:interpolatingpはtimer-sequenceを返すことになっており、timer-sequenceはsimulationモード時しか更新されないためです。

(send ri :wait-interpolation)はactioncallの/resultが返って来たかどうかを返すのが適切ではないかと考え、コードを修正しました。

なお、(send ri :interpolatingp)は、actioncallがサーバー側で受理されてから完了するまでの間tを返す仕様になっているため、:angle-vectorを送った直後はサーバー側から受理されたとの信号がまだ届いておらず、nilを返すようです。

k-okada · 2018-06-12T11:00:30Z

なるほど．GoodCatchです．
#352
で直してみましたが，こちらでどうでしょうか？

また，これで副作用がないか @furushchev さんにも聞いてみてください．

furushchev

TL;DR

I think it will be better if both #351 and #352 are merged into one pull request

Long answer:

I think #352 is simpler solution for this problem, however the pull request will return nil on any other condition than active like when the controller is pending.

#351 uses comm-state::done instead, which is considered also for that case.

For more detail on behavior of actionlib:
http://wiki.ros.org/actionlib/DetailedDescription?action=AttachFile&do=get&target=simple_client_state_transitions.png

P.S. I think it is worth to discuss about this method should return t if the controller is only in active state or return t also in pending state...
@k-okada @mmurooka ?

Naoki-Hiraoka · 2018-06-15T06:58:51Z

ありがとうございます。
controller-action-clientの:interpolatingpで統一するという方法、勉強になります。

controller-action-clientの:interpolatingpが、
１、simple-goal-state-pendingまたはsimple-goal-state-activeのときにtを返す
２、simple-goal-state-activeのときにtを返す
のどちらがよいのか、自身がありません。

interpolation中かどうかという意味を考えると2が適切と思いますが、
2の方法ですと、先述した

(defun touch ()
  ;;armが障害物にぶつかるか、一定距離動くまで動かす関数
  (send *ri* :angle-vector #f(...) 5000)
  (while (some #'identity (send *ri* :wait-interpolation nil 0.1))
    (when (障害物にぶつかった)
      (send *ri* :stop-angle-vector)
      (return-from touch t)))
  nil))

のように、interpolationが完了したどうかを知りたい場合に使うことができません。

今気づいたのですが、
robot-interfaceの:wait-interpolationや:interpolatingpの返値の算出で利用されているcontroller-actionsですが、

jsk_pr2eus/pr2eus/robot-interface.l

Line 176 in 1d92322

(setq controller-actions

で:default-controllerに対応したcontroller-action-clientがリストに加えられたきり、他のcontroller-action-clientは加えられていないようです。
pr2の場合はdefault-controllerが各部位のcontroller-action-clientを内包しているため問題が現れませんが、hrp2の場合はdefault-controllerはfullbody-controllerのcontroller-action-clientに対応しているため、rarm-controllerやlleg-controllerといった他の部位に対応したcontroller-action-clientがcontroller-actionsに入っておらず、
robot-interfaceの:wait-interpolationや:interpolatingpの返値にこれらのcontroller-action-clientの情報が含まれないという状況になっています。

k-okada · 2018-06-20T09:11:46Z

controller-action-clientの:interpolatingpが、１、simple-goal-state-pendingまたはsimple-goal-state-activeのときにtを返す２、simple-goal-state-activeのときにtを返すのどちらがよいのか、自身がありません。 interpolation中かどうかという意味を考えると2が適切と思いますが、 2の方法ですと、先述した

一個一個のactionが動いているか，というよりは，ロボット自体が動いているかと，というのが:interpolationgpなので，１がそういうこといなるのかな？であれば１でいい気がします．今は https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/352/files#diff-15f6920b924513f6027d36b361f85bbdR89 だけど，これだとダメなのかな？

今気づいたのですが、 robot-interfaceの:wait-interpolationや:interpolatingpの返値の算出で利用されているcontroller-actionsですが、 https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/1d92322c744aecdf8051cfd30da02f1042f1c9e8/pr2eus/robot-interface.l#L176 で:default-controllerに対応したcontroller-action-clientがリストに加えられたきり、他のcontroller-action-clientは加えられていないようです。 pr2の場合はdefault-controllerが各部位のcontroller-action-clientを内包しているため問題が現れませんが、hrp2の場合はdefault-controllerはfullbody-controllerのcontroller-action-clientに対応しているため、rarm-controllerやlleg-controllerといった他の部位に対応したcontroller-action-clientがcontroller-actionsに入っておらず、 robot-interfaceの:wait-interpolationや:interpolatingpの返値にこれらのcontroller-action-clientの情報が含まれないという状況になっています。

なるほど，これはHRP2側の問題なので， @mmurooka にきいてみてください． hrp2でもdefault-conttoller１に追加できるといいんだけど．

Naoki-Hiraoka · 2018-06-20T13:29:35Z

今は https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/352/files#diff-15f6920b924513f6027d36b361f85bbdR89 だけど，これだとダメなのかな？

ありがとうございます。異存ありません。

なるほど，これはHRP2側の問題なので， @mmurooka にきいてみてください． hrp2でもdefault-conttoller１に追加できるといいんだけど．

mmurooka先生と話したところ、必ずしもhrp2固有の問題では無く:add-controller関数の問題であるけれども、解決しようとするとhrp2固有の問題によって解決できないということが分かりました。

robot-interfaceクラスのメンバ変数controller-actionsは、robot-intarfaceのもつcontroller-action-clientが重複なく1つずつ入っていることが期待されているものだと認識しています。例えば、:add-controller のオプションの:create-actionsをnilとすると、既に作ってあるcontroller-action-clientをcontroller-actionsから探して:~-controllerのインターフェイスに対応させるという処理をする、というような利用がされています。

jsk_pr2eus/pr2eus/robot-interface.l

Lines 245 to 246 in 1d92322

    
           (setq action (find-if #'(lambda (ac) (string= name (send ac :name))) 
        
                                 controller-actions))

ところが、:add-controller関数は新しく関数内で新しくcontroller-action-clientを作成した時に、controller-actionsにcontroller-action-clientを追加していません。
この結果、

jsk_pr2eus/pr2eus/robot-interface.l

Line 176 in 1d92322

(setq controller-actions

で:default-controllerに対応したcontroller-action-clientがリストに加えられたきり、他のcontroller-action-clientは加えられていないようです。 pr2の場合はdefault-controllerが各部位のcontroller-action-clientを内包しているため問題が現れませんが、hrp2の場合はdefault-controllerはfullbody-controllerのcontroller-action-clientに対応しているため、rarm-controllerやlleg-controllerといった他の部位に対応したcontroller-action-clientがcontroller-actionsに入っておらず、 robot-interfaceの:wait-interpolationや:interpolatingpの返値にこれらのcontroller-action-clientの情報が含まれないという状況になっています。

となっており、

send ri :wait-interpolation
(nil)

とfullbody-controllerの結果しか返らない状態になっています。

解決方を2通り考えました。
1.
master...Naoki-Hiraoka:add-controller
として:add-controller関数内でcontroller-actionsに新たに作成したcontrollerを追加します。

この方法ではhrp2固有の問題が浮上し、

hrp2のケース
send ri :wait-interpolation
[ERROR] [1529493826.030077077]: [rarm_controller/follow_joint_trajectory_action] :wait-for-result (return nil when no goal exists)
[ERROR] [1529493826.030191132]: [larm_controller/follow_joint_trajectory_action] :wait-for-result (return nil when no goal exists)
[ERROR] [1529493826.030245760]: [rleg_controller/follow_joint_trajectory_action] :wait-for-result (return nil when no goal exists)
[ERROR] [1529493826.030303977]: [lleg_controller/follow_joint_trajectory_action] :wait-for-result (return nil when no goal exists)
[ERROR] [1529493826.030354926]: [head_controller/follow_joint_trajectory_action] :wait-for-result (return nil when no goal exists)
[ERROR] [1529493826.030405122]: [torso_controller/follow_joint_trajectory_action] :wait-for-result (return nil when no goal exists)
(nil nil nil nil nil nil nil)

と、hrp2の場合Fullbody_controller以外のactionserverにgoalが設定されていることは稀なので、no goal existsのエラーメッセージが出てきてしまいます。

2.
start-jsk/rtmros_tutorials@master...Naoki-Hiraoka:default-controller
としてhrp2のeuslispの設定を書き換え、pr2のように
https://github.com/Naoki-Hiraoka/jsk_pr2eus/blob/1d92322c744aecdf8051cfd30da02f1042f1c9e8/pr2eus/pr2-interface.l#L153-L159
default-controllerが各部位のcontrollerを包含するようにします。default-controllerに指示を送ると、hrp2の各部位のcontrollerに指令が分割して送られるようになります。

この方法で生じる問題は、足の動作など左右でタイミングが一致しなければならない動作について、一致することが保障されないことです(hrpsys_ros_bridgeのHrpsysJointTrajectoryBridgeは、trajectoryのtime_from_startを、/goalのtopicが届いてからの時間として処理するため)。また、dashboardからのreset-poseやreset-manip-poseなどの指示はhrpsysのSequencePlayerのfullbody-controllerに相当する部分に送られるため、これらはhrpsysのSequencePlayerの各部位のcontrollerに相当する部分に上書きされる仕様上、dashboardからhrp2を動かせなくなってしまいます。

mmurooka · 2018-06-21T09:33:24Z

昨日話しているときは分かっているつもりだったんだけれど，一晩たったらどういう問題だったか思い出せなくなってしまったので，
まだ残っている問題について，どんな状況でどうなって欲しいのか，プログラムのサンプルを以下みたいに書いてもらってもいいかな．

(send *ri* :angle-vector hoge 1000 :head-controller)
(send *ri* :wait-interpolation) ;; PR2だと待ってくれる．HRP2だと待ってくれない．

という問題だっけ？

(send *ri* :angle-vector hoge 1000 :head-controller)
(send *ri* :wait-interpolation :head-controller) ;; これならHRP2でも待ってくれる？

とすればとりあえずは大丈夫だけれど，

一個一個のactionが動いているか，というよりは，ロボット自体が動いているかと，というのが:interpolationgpなので

という挙動になるようにするには，どう直していけばいいかという話であってる？

mmurooka先生と話したところ、必ずしもhrp2固有の問題では無く:add-controller関数の問題であるけれども、解決しようとするとhrp2固有の問題によって解決できないということが分かりました。

のhrp2固有の問題っていうのは何かな．:wait-for-result (return nil when no goal exists)がいっぱい出てしまうこと？

Naoki-Hiraoka · 2018-06-21T12:13:03Z

以下のようになります。

(send *ri* :angle-vector hoge 1000 :head-controller)
(send *ri* :wait-interpolation) ;; PR2だと待ってくれる．HRP2だと待ってくれない

(send *ri* :angle-vector hoge 1000 :head-controller)
(send *ri* :wait-interpolation nil 0.1) ;; PR2だと(nil nil t nil)が返りロボットが動いていることがわかるが、HRP2だと(nil)が返りわからない。

(send *ri* :angle-vector hoge 1000 :head-controller)
(send *ri* :wait-interpolation :head-controller 0.1) ;; 同上

(send *ri* :angle-vector hoge 1000 :head-controller)
(send *ri* :interpolatingp) ;; PR2だとtが返りロボットが動いていることがわかるが、HRP2だとnilが返る

(send *ri* :angle-vector hoge 1000 :head-controller)
(send *ri* :interpolatingp ：head-controller) ;;これは問題ない

(defmethod hrp2jsknts-interface
  (:excepthead-controller
     ()
     (append
      (send self :larm-controller)
      (send self :rarm-controller)
      (send self :torso-controller))))
(send *ri* :add-controller :excepthead-controller :joint-enable-check t :create-actions t)
;;larm,rarm,torsoに対応したcontroller-action-clientが、:larm-controller :rarm-controller  :torso-controllerと、:excepthed-controllerとで合わせて2つずつ重複して生成されしまう。PR2もHRP2も。

↑は
#64
で既に指摘されており、正しい挙動です。ところが、重複を避けてたければ:create-actions nilとすればよいとありますが、

(defmethod hrp2jsknts-interface
  (:excepthead-controller
     ()
     (append
      (send self :larm-controller)
      (send self :rarm-controller)
      (send self :torso-controller))))
(send *ri* :add-controller :excepthead-controller :joint-enable-check t :create-actions nil)
;;PR2では既に作成してあるcontroller-action-clientを検索しそれを参照するのをうまくやってくれる。HRP2では検索にhitせず、controller-action-clientを持たない何もしない:excepthead-controllerができる。

上記すべての問題は、先述の1または2の方法で解決します。

hrp2固有の問題というのは、
1の方法についてはおっしゃる通り、hrpsysでは各controllerに対応した部分に初期goalが入っていないために(入れるとfullbodycontrollerを上書きしてしまう)、:wait-for-result (return nil when no goal exists)がいっぱい出てしまうことを意図して、
2の方法についてはhrpsysでは各controllerがactionを実行開始するタイミングが必ずしも一致しないことを意図して書きました。

(editが多くてすみません)

furushchev · 2018-08-18T12:37:54Z

It seems #352 solved this issue.
@Naoki-Hiraoka Could you double check?

fix return value of :wait-interpolation for real robot

a172e9a

k-okada requested a review from furushchev June 12, 2018 03:04

k-okada mentioned this pull request Jun 12, 2018

pr2-ri-test.l : add test to check :wait-interpolation, with timeout #352

Merged

furushchev reviewed Jun 13, 2018

View reviewed changes

pazeshun mentioned this pull request Jul 14, 2018

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

Open

Naoki-Hiraoka closed this Aug 24, 2018

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

【robot-interface.l】fix return value of :wait-interpolation for real robot #351

【robot-interface.l】fix return value of :wait-interpolation for real robot #351

Naoki-Hiraoka commented Jun 8, 2018 •

edited

Loading

k-okada commented Jun 12, 2018

furushchev left a comment •

edited

Loading

Naoki-Hiraoka commented Jun 15, 2018

k-okada commented Jun 20, 2018 via email

Naoki-Hiraoka commented Jun 20, 2018

mmurooka commented Jun 21, 2018

Naoki-Hiraoka commented Jun 21, 2018 •

edited

Loading

furushchev commented Aug 18, 2018

【robot-interface.l】fix return value of :wait-interpolation for real robot #351

【robot-interface.l】fix return value of :wait-interpolation for real robot #351

Conversation

Naoki-Hiraoka commented Jun 8, 2018 • edited Loading

k-okada commented Jun 12, 2018

furushchev left a comment • edited Loading

Choose a reason for hiding this comment

Naoki-Hiraoka commented Jun 15, 2018

k-okada commented Jun 20, 2018 via email

Naoki-Hiraoka commented Jun 20, 2018

mmurooka commented Jun 21, 2018

Naoki-Hiraoka commented Jun 21, 2018 • edited Loading

furushchev commented Aug 18, 2018

Naoki-Hiraoka commented Jun 8, 2018 •

edited

Loading

furushchev left a comment •

edited

Loading

Naoki-Hiraoka commented Jun 21, 2018 •

edited

Loading