smach のサンプルで動作不良

[MoriKen254]

書籍 p217のサンプルで動作不良です。一番最初のOpen_gripperで止まってしまうようです。

昨年、本プログラムを作成した段階では、正常動作を確認したものですので、ROS側の仕様の変化等も含めて考慮したうえで、対策をしましょう。

再現方法

roslaunch pr2_gazebo pr2_empty_world.launch

rosrun smach_sample smach_pr2.py

bashメッセージ

[INFO] [1540312867.519997, 398.986000]: State machine starting in initial state 'MOVE_FORWARD' with userdata: 
	[]
[INFO] [1540312867.520861, 398.986000]: State machine starting in initial state 'Open_gripper' with userdata: 
	[]

smach viewer 画面

問題箇所

どうやら、Pr2GripperCommandActionがうまく動いてないように思われます。

        with init_sub:
            gripper_open  = Pr2GripperCommandGoal()
            gripper_open.command.position = 0.09
            gripper_open.command.max_effort = 2000
            smach.StateMachine.add('Open_gripper',
                                   SimpleActionState('/l_gripper_controller/gripper_action',
                                                     Pr2GripperCommandAction,
                                                     goal=gripper_open),
                                   transitions={'succeeded': 'Init_pose',
                                                'preempted': 'Init_pose',
                                                'aborted': 'Open_gripper'})

gripper_open.command.position = 0.09 (9cm) の目標値を 0 に書き換えたら、状態は遷移しました。

ただし、これは初期値=目標値のため、即座にactionが終了して、先に進んだというだけの話で、所望の動きを達成できていません。

問題は、gripper_open.command.position = 0.09という目標値を与えているにもかかわらず、そのように動いてくれないことです。

解決

Issue書いている途中で解決しました笑。

色々やったのですが、結論を示すと、目標値を0.03以下にすればOKです。多分、途中でPR2の仕様が変わったのでしょう。そうでないと説明がつきません。

経緯

当初の目標値でactionのfeedbackを覗いてみたら、こんな感じでした。

rostopic echo /l_gripper_controller/gripper_action/feedback

feedback: 
  position: 0.0292726664819
  effort: 862.604106492
  stalled: False
  reached_goal: False

positionが、0.029辺をふらふらしてそれ以上広がりませんでした。何らかのリミッターがかかっていると考えるのが自然です。

というわけで、PR出しておきます。

[AtsukiYokota]

確認及び修正ありがとうございます。smach_pr2.pyがステートマシンの途中で止まらずに最後まで動作することを確認しました。
しかし、テーブルと対象物のspawnにまだ問題が残っているようです。別でissueを建てようかとも思いましたか取り急ぎこちらで情報共有させていただきます。

問題点

テーブルと対象物をspawnさせるとテーブルの下に対象物がspawnしてしまう

詳細

本に書いてあるspawn_object.launch（masterブランチには無かったので本の通りに自作）ではテーブルの高さが高すぎて（テーブルモデルの足が長い？）テーブルの上に対象物が乗らない問題がありました。
mk/fix-smachブランチでは上記launchファイルとモデルのsdfファイルが追加されておりテーブルの高さは見た目では直ったように見えますが依然としてテーブルの上に対象物が乗らずpr2が把持することができません。

試したことと結果

spawn_object.launchの中で各対象物のz方向の高さを決め打ちで変更したが、高すぎると物体が倒れたりテーブル上で滑ってしまう。
これは試してみて思ったことなのですが各objectがspawnするタイミングがlaunchするたびにバラバラになっているように感じます。これのせいでうまくテーブルの上に対象物が乗らないのかもしれません。

現状試せているのはここまでです。取り急ぎご報告でした。

[MoriKen254]

各論を #37 #38 に展開しましたので、閉じます。

ご協力ありがとうございました ^^

引き続きよろしくおねがいします m(_ _)m