【10章】Gripperの開きが足りない

[MoriKen254]

#35 と #36 で言及。

なぜかPR2グリッパが開ききってくれない。

        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 = x.xx

の指令値によらず、ちゃんと開くようになってほしい。

現在値の確認は、下記コマンドで。

rostopic echo /l_gripper_controller/gripper_action/feedback

[MoriKen254]

まだよく分かりませんが、備忘録。

後述のPR2のグリッパにまつわる仕様に関して、リミッターに関わるクリティカルな変更があったようには見えず。。

また Gazebo のVer. が上がったことにより云々とかの話だと、面倒です汗。

action

https://github.com/PR2/pr2_controllers/blob/kinetic-devel/pr2_gripper_action/src/pr2_gripper_action.cpp

controller

https://github.com/PR2/pr2_controllers/blob/kinetic-devel/pr2_mechanism_controllers/src/pr2_gripper_controller.cpp

description

https://github.com/PR2/pr2_common/tree/melodic-devel/pr2_description/urdf/gripper_v0

その他

actionでは、goalを受け取ると下記メッセージを発信する。

pr2_controllers_msgs::Pr2GripperCommand

from https://github.com/PR2/pr2_controllers/blob/kinetic-devel/pr2_gripper_action/src/pr2_gripper_action.cpp

↓

controller がメッセージを受信し、effort指令値を計算しメッセージを発信する(pr2_controllers_msgs::JointControllerState なる、状態とか指令値とか色々入ったメッセージっぽい)。

https://github.com/PR2/pr2_controllers/blob/kinetic-devel/pr2_mechanism_controllers/src/pr2_gripper_controller.cpp

↓

Gazebo の場合、https://github.com/PR2/pr2_simulator/blob/kinetic-devel/pr2_gazebo_plugins/src/gazebo_ros_controller_manager.cpp が当該メッセージをハンドリングしている。（RobotHWSim は使っていないっぽい）

問題は、これらのコードについて、特に変更がなされていないのにグリッパが開かないということ。

あるいは、何らかの変更を陽に加えないといけないということなのだろうか…？
Gazebo側のSetForce メソッドに仕様が変わったとか…？SetPositionとかSetVelocityとかで物議を醸していた時代があったと思うので、ちょっとつぶやいてみる。

ここいらの材料を元に、何か見つけたいです。グリッパ開かないのは私達に限らず、影響が大きいかと汗。

必要であれば、本家Issueに投げることも検討しますが、ある程度こちらで原因を特定できないと、動いてもらうのは難しい思います。

ご協力お願いします。

[MoriKen254]

良道くん、本件調査の続きを任せたいのですが、協力頂けませんか？

弱音で申し訳ないのですが、そろそろ私一人では回らなくなってきましたので、助けて欲しいです笑。

その他頂いたコメント、エラッタ関係は、後ほど私がやりますので。取り急ぎ

[RyodoTanaka]

@MoriKen254
承知しました．
目標は

• ちゃんとグリッパを最後まで開くようにする

ということで取り組みます．
で，何かしらの進展をとりあえず来週頭（月曜）に報告するようにします．

[MoriKen254]

ありがとう。目標はそれでおねがいします。

忙しいところ悪いね、任せた！月曜日の報告、とても楽しみにしています。

エラッタやるぞぉ。

[RyodoTanaka]

@MoriKen254
原因わかりました！
設定しているeffortの値がでかすぎるのが原因です．
今，動画をあげるので少々お待ちください．

[RyodoTanaka]

@MoriKen254

原因

PR2のグリッパへの力司令ですが，今書かれている設定だと，2000 Nになっています．（下記コード参照）

        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'})

しかし，PR2のスペックでは，グリッパの最大把持力は ，80 Nです．（下記サイト参照）
http://www.willowgarage.com/pages/pr2/specs

というわけで，リミットオーバーどころではない司令値を入れているため，発散したんだと思われます．
ちなみに，Positionについては，スペック限界値の 0.09 m (90 mm)を与えているので，これは正しいです．

確認と対策

実験1

上記について実験した動画を下記リンクに貼ります．
https://goodroad.work/owncloud/index.php/s/uftaTmsUzkVwHYw
この動画では，右グリッパにこれまで通り，

• position : 0.09 m
• effort : 2000 N

を与え，左手に

• position : 0.09 m
• effort : 10 N

を与えています．
左グリッパはこれで正常に動きます．

実験2

では，今度は一体どこにeffortの閾値があるのか？
について調べた動画を下記リンクに貼ります．
https://goodroad.work/owncloud/index.php/s/LCYI3ueFgWJEW1v
上記動画からわかるように，13.5〜14 Nの間に発散する閾値があることがわかります．
80 N で開こうとしたらダメなんかい！ という気持ちはありますが，諸々の理由があり，これについては追求してないです．
多分，PR2のコントローラゲインをいじれば直せると思いますが，これを行うとなると，

• PR2のコントローラゲインファイルを自分で作って，それを参照するlaunchファイルを書かなきゃいけない
• つまり本への変更が大きくなる
• そもそもそんなことしなくても，司令値を変えれば必要要件は満たしている

という理由から，司令値を適正なものに変えれば良いと考えます．

また，2つの動画の中で右手を犠牲にしているのですが，
一度発散する値を入れてしまうと，その後正常に動かなくなる（2つ目の動画参照）
というのも確認できているので，早急にPRを作って投げます．

以上，よろしくお願いします．

[MoriKen254]

ありがとう！現象はよく分かりました。

2000Nという値自体は気にはなっていましたが笑、ネット上のサンプルはどれもこんなもんだったので、いいんだろうと高をくくっていました。助かりました。

以下ただの思考実験です。

追求すると藪蛇になりそうですが、おそらくリミッタがちゃんと働くようになった、という感じですかね。そもそも2000Nなんて値を入れていたのにのほほんと動いている過去の挙動のほうが異常だったんですかね。
PR2自体の記述に変更がないので、コントローラのリミッタ部分がアップデートされたという感じ？

で、80Nに対応していないのは、そうなるようにコントローラのパラメータが最適化されていない。現状のパラメータだと高々14Nくらいでリミットがかかってしまう。みたいな感じ？だから参照するコントローラ用のlaunchファイルを自作する、という改善が提案されている？

そして、一回リミットオーバがかかったら、未来永劫復旧しない（再起動するまでは）。これは所望の仕様なのかちょっと判断しかねますね。その割に、13.5N以下というとても微妙な値を突っ込まないといけないというのは、辛い笑。PR2のIssueにあげてもいいかもしれないですね。

なんにせよ、色々ありがとう。ひとまずちゃんと動くようで、一旦スッキリしました！

[MoriKen254]

PR2の件をIssueに上げるなら、 @RyodoTanaka くんの方が良いと思いますが、どうですか？

もし手が回らない等なら私が上げてもいいのですが、動画等含めせっかくの @RyodoTanaka くんの成果ですしね :)

[RyodoTanaka]

@MoriKen254
ありがとうございます．
下記，お答えします．

PR2自体の記述に変更がないので、コントローラのリミッタ部分がアップデートされたという感じ？

これについては，ちゃんと見てないのでわからないです...
というのも，制御入力値がどうなってるのかを観測してないから，なんとも言えないです．
憶測では，リミッタに引っかかったのかと思いますが，リミッタを作るなら，それ以上のちからが出ないようにするリミッタを普通は取り付けると思うので，今回のように，それ以降グリッパが動かなくなるような挙動になってるのはいまいち納得行かないです．
やはりリミッタがあると考えるのはおかしいと思います．単純にシミュレータ側（ODE側）で挙動がおかしくなる領域に入ったんだと思います．

現状のパラメータだと高々14Nくらいでリミットがかかってしまう。みたいな感じ？だから参照するコントローラ用のlaunchファイルを自作する、という改善が提案されている？

これについては，確かPR2のグリッパの制御はPID制御なんですけど，力の制御入力がオーバーシュートしてるのではないかと思っています．で，14Nあたりの制御入力を加えると，そのオーバーシュートが挙動がおかしくなる領域に達するのかなぁ...?
という憶測です．

その割に、13.5N以下というとても微妙な値を突っ込まないといけないというのは、辛い笑。PR2のIssueにあげてもいいかもしれないですね。

実はこのあたり，まだ検証中でして，というのも

• 開くときには14 N ぐらいに閾値があることはわかった．
• 閉じるときは14 N越えててもOKだった....

というよくわからない挙動が発生しています．
さらに，下記動画のように，一度把持した物体を離そうとすると，手にひっついたままになるという現象も確認しています．
https://goodroad.work/owncloud/index.php/s/IeVUgoIpEfMKVwT
以上より，

• 開閉の歳の指令値に関してまだ検証が必要
• 把持した後の挙動について検証が必要

という上記2点の問題があります．
これらについてもう少し状況を詳細に把握してからでないとIssueを立てづらい感は有ります．
が，そういうのはIssue立ててから順次upしていけばいいのかなという気もしています．

以上，長くなりましたが，よろしくお願いします．

[MoriKen254]

ありがとうございます。状況を理解しました。藪蛇してしまった感じですかね笑。

• 開閉の指令値
  • 闇ですねぇ。
  • ここまで検証したのですから、Issue投げても迷惑がられないんじゃないかなー笑。
• ひっつく
  • 暗闇ですねぇ。
  • ODEのシミュレーション演算部と描画状態の不一致が起きてしまってるんですかねぇ。分かりませんが。

前者と後者が全くの別問題なのか、いやはや実は同一の原因による問題で現象が異なるだけなのか、判断しかねますね。いずれにしても、現段階では独立したIssueとしてあげるのが妥当でしょう。

とりあえず前者についてはそれなりに調べたとは思うので、一旦Issueを投げて反応を見ながら、こっちからも必要に応じて情報出していく感じでいいんじゃないかなぁと思います。

そして、後者は確率的に起きるんですかね。

まったくの手探りで恐縮ですが、例えば、開閉時の力が強いとき、弱いとき、どっちのほうが発生する確率が高いとか、ありますかね（ODEの問題ならここに何らかの傾向が観察できないか？）。

あるいは、コップの代わりのコーラにしたら起きにくいとか（把持対象の形状がひっつきやすさに影響しないかとか？）。

もう少し温めた上で、以下の方針が取れるかと。

• 自己解決のパターン
  1. 前者Issueとは別に、自分で検証しているうちに解決できたらラッキー。
  2. 前者Issueの解決後に、後者も一緒に解決できたらラッキー。
• 他力本願のパターン
  1. 前者Issueをやりながら関係がありそうだと思ったときに、情報提供として同一Issueに上げる。
  2. 前者Issue解決後も後者が解決しないなら、別Issueを立てる。
  3. 前者Issueが解決しなくてボンヤリしてきたら、pingがてらに別Issueを立てる。
  4. そんなに気を遣わずに、両方Issue一度に上げてしまう。

私なら、一個ずつ尋ねると想います。あとは様子見ながら柔軟に。

よろしくおねがいします。

[RyodoTanaka]

@MoriKen254
ありがとうございます．
前者（effort値がオーバーシュートしてるっぽい問題）の方は，本家にIssue投げてみます！
後者（ひっつく問題）の方は，前者の対応をしながら，ぼちぼち検証したいと思います．
で，後者について今わかっている事なんですが，

まったくの手探りで恐縮ですが、例えば、開閉時の力が強いとき、弱いとき、どっちのほうが発生する確率が高いとか、ありますかね（ODEの問題ならここに何らかの傾向が観察できないか？）。

上記についてわかっていることは
どうもかける力ではなくて，目標位置によって挙動が変わります．
当たり前といえば当たり前なのですが，目標位置をコップの表面に触るギリギリに設定しておくと，一定の確立で引っつかなくなるということは確認しています．
動画を撮らなければ再現するのですが，動画を録画し始めると，ギリギリに設定していてもひっついちゃうので，やはり物理エンジンの話では？と思っています．
Gazeboとは別件で，先日出場したWRSで使用していたChoreonoidでも，似たような現象が起こることが有りました．（と言っても，ChoreonoidはGazebo(ODE)とは比べ物にならないくらい接触に関するシミュレーションが素晴らしく頑健に作られてました！ 上記の問題が起きたのは，AGXという，Choreonoidとは別の物理エンジンでシミュレートされている物体との絡みの時に起きてました．）
なので，やはり干渉や接触に関する物理エンジンの問題だと思っています．

いずれにせよ，一旦前者の問題をIssueに上げながらぼちぼち検証したいと思います．
よろしくお願いします．

[RyodoTanaka]

あと，下記動画を見ていると，机にコップが触れたのではなく，近づいただけで再びアームから離れてるようにも見える(1:57あたり）ので，もしかすると吸い付けプラグイン等の他の要因があったりするのかな？（未検証です）
という憶測もあったりします．
https://goodroad.work/owncloud/index.php/s/IeVUgoIpEfMKVwT

[MoriKen254]

ありがとうございます。Issue投稿、素晴らしいですね！よろしくおねがいします。

あと、1:57 辺りで、確かに不自然な挙動が見られますね。自然落下ではなく、テーブルからの引力、あるいはハンドからの斥力、が働いているような。経緯から考えると、見た目にそぐわない余計な斥力(Collision領域がおかしくなって、カップと重複してしまっている？)が働いているのではないか？というこれまた憶測ですが。

それにしても、産総研（というか中岡さん）の誇るChoreonoidはスゴいんですね！GazeboやV-REPと肩を並べる、日本発のスーパーシミュレータ！って感じで認知が広まるといいですね。