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

[Naoki-Hiraoka]

start-jsk/rtmros_common#1114 を書いていて気づいたのですが、

HIRONXJSKのend_coordsは、URDFやeusモデルでは初期姿勢のときに親リンクから親リンク座標系で90度回転したものとして定義されていますが、

rtmros_tutorials/hrpsys_ros_bridge_tutorials/models/hironxjsk.yaml

Line 31 in 2edfaf5

rotate : [0, 1, 0, 90]

rtmros_tutorials/hrpsys_ros_bridge_tutorials/CMakeLists.txt

Line 291 in 2edfaf5

--conf-file-option "end_effectors: rarm,RARM_JOINT5,CHEST_JOINT0,-0.05,0,0,0,1,0,1.5708, larm,LARM_JOINT5,CHEST_JOINT0,-0.05,0,0,0,1,0,1.5708,"

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

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

[Naoki-Hiraoka]

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

[pazeshun]

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

[Naoki-Hiraoka]

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

• ImpedanceControllerのパラメータforce_gain moment_gainをX,Y,Z軸ごとに変えたいときに，ROSのend_coordsの軸とは15度ずれた軸が変わってしまう

だけだと思います．

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

[pazeshun]

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

[pazeshun]

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

[k-okada]

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

[pazeshun]

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

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

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

[Naoki-Hiraoka]

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

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

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

end_coordsの座標のズレに対する根本的な治療は、#603 (comment) のようにhrpsys内でend_coordsのlocalpとlocalRにRsを左からかけることで、そうするとend_coordsの軸が15度ずれなくなります。