[hrpsys_gazebo_general]IOBplugin fix frame conversion of force-sensor

Naoki-Hiraoka commented 5 years ago

IOBPlugin.cppでgazeboからForceSensorの値を取得する際の変換が正しくないのではないかというpull requestです。

gazeboからForceSensorの値を取得するために、.yamlファイルでセンサの位置を指定します。指定する各パラメータは、hrpsys_gazebo_tutorialsにあるロボットでは、以下のような使われ方をしています。例： https://github.com/start-jsk/rtmros_tutorials/blob/a546adde4cf517082c47a68bbf58eba2dbd51fae/hrpsys_gazebo_tutorials/config/HRP2JSKNTS.yaml#L117-L120

force_torque_sensors/joint_name  (センサが付いているLinkのparent joint)
force_torque_sensors/translation (上記jointからセンサへのベクトルをセンサが付いているLinkの座標系で表したもの)
force_torque_sensors/rotation(センサが付いているLink座標系から見たセンサの傾き)

一方、hrpsys_gazebo_general/IOBPluginでは、受けとった各パラメータを以下のように異なる解釈をして力やモーメントを変換しています。

force_torque_sensors/joint_name  (センサが付いているLinkのparent joint)
force_torque_sensors/translation (センサから上記jointへのベクトルをセンサ座標系で表したもの)
force_torque_sensors/rotation (センサ座標系から見たセンサが付いているLinkの傾き)

IOBPlugin側をhrpsys_gazebo_tutorialsにあるロボットでの使われ方に合わせました。

YoheiKakiuchi commented 5 years ago

とりあえずデバッグ用に、力センサのあるリンクに力（トルク）をかけたときに、センサはどのような出力したらいいかの正しい値例を何例かつくってみてもらえるかな。

そのとき、修正前はどうなっていて、修正後はそれに同じくなるという感じで。

k-okada commented 4 years ago

@Naoki-Hiraoka これはもう必要ないのかな？

それとも，https://github.com/start-jsk/rtmros_tutorials/pull/568 に必要なのかな．

Naoki-Hiraoka commented 4 years ago

このPR無しでもhttps://github.com/start-jsk/rtmros_tutorials/pull/568 は動きますので，このPRは後回しにしていただきたいです．

start-jsk / rtmros_gazebo

[hrpsys_gazebo_general]IOBplugin fix frame conversion of force-sensor #236