search

Nishida-Lab / rosbook_pkgs

「実用ロボット開発のためのROSプログラミング」サンプルコード
http://amzn.asia/d/ct0zwBh
44 stars 18 forks source link

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

Open MoriKen254 opened 5 years ago

MoriKen254 commented 5 years ago

https://github.com/Nishida-Lab/rosbook_pkgs/issues/35 と https://github.com/Nishida-Lab/rosbook_pkgs/pull/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 commented 5 years ago

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

後述の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 commented 5 years ago

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

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

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

RyodoTanaka commented 5 years ago

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

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

MoriKen254 commented 5 years ago

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

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

エラッタやるぞぉ。

RyodoTanaka commented 5 years ago

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

RyodoTanaka commented 5 years ago

@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 この動画では，右グリッパにこれまで通り，

を与え，左手に

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

実験2

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

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

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

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

MoriKen254 commented 5 years ago

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

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

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

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

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

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

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

MoriKen254 commented 5 years ago

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

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

RyodoTanaka commented 5 years ago

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

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

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

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

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

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

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

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

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

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

MoriKen254 commented 5 years ago

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

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

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

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

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

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

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

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

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

RyodoTanaka commented 5 years ago

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

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

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

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

RyodoTanaka commented 5 years ago

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

MoriKen254 commented 5 years ago

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

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

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

MoriKen254 commented 5 years ago

ping @RyodoTanaka

issue 投稿した？笑。あまり優先度高くないから、無理しなくても大丈夫ですが。

RyodoTanaka commented 5 years ago

@MoriKen254 完全に忘れてました... すみません...orz