search

start-jsk / rtmros_tutorials

Tutorials for rtmros packages
6 stars 61 forks source link

HIRONXJSKのend_coordsの傾きがhrpsysの内外で15度ずれている? #603

Open Naoki-Hiraoka opened 2 years ago

Naoki-Hiraoka commented 2 years ago

https://github.com/start-jsk/rtmros_common/pull/1114 を書いていて気づいたのですが、

HIRONXJSKのend_coordsは、URDFやeusモデルでは初期姿勢のときに親リンクから親リンク座標系で90度回転したものとして定義されていますが、 https://github.com/start-jsk/rtmros_tutorials/blob/2edfaf5ef6c35ffdd5bbfb6799dd5c2090f31c8c/hrpsys_ros_bridge_tutorials/models/hironxjsk.yaml#L31 https://github.com/start-jsk/rtmros_tutorials/blob/2edfaf5ef6c35ffdd5bbfb6799dd5c2090f31c8c/hrpsys_ros_bridge_tutorials/CMakeLists.txt#L291

https://github.com/start-jsk/rtmros_common/pull/925 https://github.com/start-jsk/rtmros_common/pull/1114 で書かれているOpenHRP3の仕様の影響で、hrpsysの中では初期姿勢のときにworld座標系からworld座標系で90度回転したものとして扱われてしまうので、

肩の関節軸が15度ずれているHIRONXJSKは、hrpsysの内外でend_coordsの傾きが15度ずれてしまっているかもしれません。

Naoki-Hiraoka commented 2 years ago

直すためには、 https://github.com/fkanehiro/hrpsys-base/blob/bdbc4c8809b8d044e9bcd6583bda6b5bffb7c8b7/rtc/ImpedanceController/ImpedanceController.cpp#L188 のような箇所全てに、左から親リンクのRsをかける必要がありそう

pazeshun commented 2 years ago

@Naoki-Hiraoka ごめん、全くわかってなくて教えてほしいんだけど、end_coordsの傾きがhrpsysの内外で異なると、具体的な問題としては何が起こるんだろう。 hrpsysの中では矛盾なく成立するだろうからインピーダンス制御とかは動くけど、hrpsysで計測した力をROSの世界に持ってくると、向きがおかしくなる、みたいなことなのかな。

Naoki-Hiraoka commented 2 years ago

hrpsysの中では矛盾なく成立するので，HIRONXの用途だと，具体的な困る点は

だけだと思います．

hrpsysで計測した力はセンサ座標系でROSに持ってきているので，end_coordsのズレの影響は受けません．

pazeshun commented 2 years ago

なるほど、ありがとう。 *_gainをROSから変えるインターフェースって、確かidlからの自動生成だったと思うから、絶妙にパッチ当てにくいですね。 さらにその上の、PythonとかEusLispのインターフェースにパッチを当てることならできそうだけど、Hiroで*_gainを変えること自体を、ここ3年くらいやってなくて、挙動を確認できるデモがないですね。 一旦そのままにしておいて、gain変える必要が出てきたら修正してみようかなと思いました。

pazeshun commented 2 years ago

(操作ミスで一瞬closeしました、すみません)

k-okada commented 2 years ago

ちゃんと理解できていないかもしれないけど， https://github.com/start-jsk/rtmros_tutorials/issues/603#issuecomment-985536653 をmerge したら，hironx のimpedanceで_gain をセットしたときに，現状と15度違う（ただしworldcoordsとそろっている） という事でいいかな？ で，@pazeshun のD論中にgain を設定している場面がなければ，https://github.com/start-jsk/rtmros_common/pull/1114 をマージしてもいいのかな．

pazeshun commented 2 years ago

@k-okada 僕の理解が正しければ、https://github.com/start-jsk/rtmros_common/pull/1114 をマージしても、HIRONXには何も影響なくて、https://github.com/start-jsk/rtmros_common/pull/1114 に関わらず、とにかく*_gainの軸が想定と15度ずれているよということを@Naoki-Hiraoka は教えてくれたんだと思います。 なぜかというと、HIRONX（に限らず全てのhrpsys_ros_bridgeを使うロボット）が力センサ値をROSに渡すコードには既に、https://github.com/start-jsk/rtmros_common/pull/1114 に相当する変更がhttps://github.com/start-jsk/rtmros_common/pull/925 として入っているからです。 https://github.com/start-jsk/rtmros_common/pull/1114 は、hrpsysで視覚センサを扱うロボットをROSに繋ぐ際に必要な変更で、最近の実機では珍しくて、主にhrpsys-simulatorとchoreonoidをROSに繋ぐための修正なんじゃないかと思っています。 HIRONXは、僕の知る限り、hrpsys-simulatorをROSに繋いで視覚処理、みたいなことはしたことがなくて、choreonoidに至っては全く使ってないので、影響ないはずです。

なので、https://github.com/start-jsk/rtmros_common/pull/1114 に関しては、詳しい人（どなたかわかりませんが・・・@k-okada になるのでしょうか）の同意が得られれば、mergeして問題ないはずです。 で、それとは全く別の話として、僕はこれまでは*_gainの設定をやってきていませんが、これからする可能性があります。 必要になったら、軸が15度ずれてることを念頭において設定するか、何らかの方法でパッチ当てるか、考えたいと思っています。

上記の理解で合ってますでしょうか。->@Naoki-Hiraoka

Naoki-Hiraoka commented 2 years ago

@pazeshun おっしゃる通りです。

現在、視覚センサの座標のズレと、end_coordsの座標のズレという2つの異なるバグがあって、https://github.com/start-jsk/rtmros_common/pull/1114 は前者を直すもので、このissueは後者を指摘するものです。

で、それとは全く別の話として、僕はこれまでは*_gainの設定をやってきていませんが、これからする可能性があります。 必要になったら、軸が15度ずれてることを念頭において設定するか、何らかの方法でパッチ当てるか、考えたいと思っています。

end_coordsの座標のズレに対する根本的な治療は、https://github.com/start-jsk/rtmros_tutorials/issues/603#issuecomment-985536653 のようにhrpsys内でend_coordsのlocalpとlocalRにRsを左からかけることで、そうするとend_coordsの軸が15度ずれなくなります。