hrpsys-simulatorのセンサ値の座標系

[snozawa]

センサの親リンク相対のオフセット（の姿勢）

                                                    DEF gsensor AccelerationSensor {
                                                       sensorId 0
                                                       rotation 0 1 0 3.14159
                                                    }

のように単位行列以外になるようにすると、AccelSensor等の読みがおかしい気がします。
この状態でhrpsys-simulatorを起動して、

rtm.readDataPort(rh.port("gsensor")).data

のようにシミュレータのgsensorポートをよむと、rotation 0 1 0 3.14159でなくてrotaion 0 1 0 0としたときと同じ値がかえってきます。
例えば
https://github.com/fkanehiro/hrpsys-base/blob/master/sample/Sample6dofRobot/Sample6dofRobot.wrl#L574
のsample6dofrobotの場合、rotation 0 1 0 0のとき初期姿勢でgsensorは0 0 9.8とでますが、
rotation 0 1 0 3.14159としても初期姿勢でgsensorは0 0 9.8とでます。

rotation 0 1 0 3.14159とした場合は、初期姿勢でgsensorは0 0 -9.8とでるのが望ましい挙動のように思います。
（センサが上下反転してくっついているので）

https://github.com/fkanehiro/hrpsys-base/blob/master/lib/util/PortHandler.cpp#L219
の部分のaccelerationのdvなどは、どこ座標系の値になってますでしょうか。
この部分で値の表現されている座標系を直す必要がありそうですが、あっておりますでしょうか。

よろしくお願いいたします

[fkanehiro]

確かにそうですね。
現状ではdvはセンサーがくっついているリンクの座標系で出力してしまっているようです。
直すとしたら、dvがセンサ座標系になっている方が正しいと思いますので、
https://github.com/fkanehiro/openhrp3/blob/master/hrplib/hrpModel/ForwardDynamics.cpp#L145
を直すのが良さそうな気がします。

[snozawa]

ForceSensor, RateGyroSensorも同様に変換が必要そうですが、

• RateGyrosensorは
  https://github.com/fkanehiro/openhrp3/blob/master/hrplib/hrpModel/ForwardDynamics.cpp#L111
  でよろしいでしょうか。
• ForceSensorはどちらになりますでしょうか。

よろしくお願いいたします。

[fkanehiro]

ForceSensor, RateGyroSensorについては対応がされてそうですね。

https://github.com/fkanehiro/openhrp3/blob/master/hrplib/hrpModel/ForwardDynamicsABM.cpp#L492
https://github.com/fkanehiro/openhrp3/blob/master/hrplib/hrpModel/ForwardDynamics.cpp#L111

[snozawa]

fkanehiro/openhrp3#41
にて作成しましたので、こちら一旦closeさせていただきます。

ありがとうございました。