gazebo+kinematics_modeでHrpsysSeqStateROSBridgeの出すodomの速度が振動する

[orikuma]

gazebo上においてkinematics_modeを有効にした状態でauto-balancerのみを使ってgo-posすると速度が大きく振動します.
hrpsys-simulatorではこのような速度の振動は再現しなかったためgazeboとhrpsysを組み合わせた時に発生する問題のように思います.
kinematics_mode無効だとまだ歩けない(転ぶ)のでkinematics_mode:=falseは試していません.
gazeboとの時間合わせあたりに問題があるのでしょうか?

roslaunchのコマンド

roslaunch hrpsys_gazebo_tutorials gazebo_jaxon_no_controllers.launch
rtmlaunch hrpsys_gazebo_tutorials jaxon_hrpsys_bringup.launch SYNCHRONIZED:=true KINEMATICS_MODE:=true

eusのコマンド

roseus jaxon-interface.l
jaxon-init
send *ri* :angle-vector (send *jaxon* :reset-pose)
send *ri* :start-auto-balancer
send *ri* :go-pos 5 0 0
...

/odom/twist/twist/linear/xのログ

[garaemon]

kinematics_mode:=falseと比較してみてください。転んでいても/odomはでるはずです

[orikuma]

kinematics_mode:=falseで試しました.
gazebo起動直後に転んだため空中足踏みです.
同じような振動を起こしています.

[orikuma]

なおhrpsysはかなりの頻度でtimeoverしていました.

...
[1441955914.329046] Timeover: processing time = 2.36[ms]
0.36, 0.02, 0.16, 0.03, 0.04, 0.04, 0.02, 0.08, 0.02, 0.03, 0.15, 0.18, 0.89, 0.05, 0.04, 0.02, 0.02, 0.05, 0.14, 
[1441955914.332176] Timeover: processing time = 2.15[ms]
0.34, 0.03, 0.23, 0.04, 0.07, 0.06, 0.03, 0.14, 0.02, 0.04, 0.20, 0.18, 0.49, 0.04, 0.03, 0.02, 0.02, 0.05, 0.12, 
[1441955914.337674] Timeover: processing time = 2.87[ms]
0.51, 0.02, 0.20, 0.04, 0.07, 0.56, 0.03, 0.10, 0.02, 0.04, 0.18, 0.19, 0.61, 0.04, 0.04, 0.02, 0.01, 0.04, 0.14, 
[1441955914.341746] Timeover: processing time = 2.17[ms]
0.33, 0.03, 0.18, 0.04, 0.06, 0.08, 0.03, 0.12, 0.03, 0.04, 0.29, 0.21, 0.43, 0.04, 0.03, 0.02, 0.02, 0.04, 0.14, 
[ INFO] [1441955914.341863690, 182.868000000]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (rleg
[1441955914.347155] Timeover: processing time = 2.57[ms]
0.27, 0.03, 0.35, 0.06, 0.07, 0.05, 0.03, 0.11, 0.02, 0.03, 0.17, 0.37, 0.79, 0.03, 0.03, 0.01, 0.01, 0.04, 0.11, 
[1441955914.352407] Timeover: processing time = 2.23[ms]
0.31, 0.03, 0.24, 0.04, 0.12, 0.05, 0.04, 0.10, 0.03, 0.04, 0.22, 0.35, 0.42, 0.04, 0.03, 0.01, 0.01, 0.04, 0.12, 
[1441955914.357233] Timeover: processing time = 2.20[ms]
0.39, 0.03, 0.26, 0.04, 0.06, 0.08, 0.03, 0.14, 0.03, 0.04, 0.21, 0.20, 0.47, 0.03, 0.03, 0.01, 0.01, 0.04, 0.10, 
[1441955914.363007] Timeover: processing time = 4.01[ms]
0.59, 0.02, 0.57, 0.04, 0.05, 0.04, 0.03, 0.12, 0.02, 0.04, 0.19, 1.57, 0.45, 0.03, 0.03, 0.02, 0.02, 0.04, 0.14, 
[1441955914.365601] Timeover: processing time = 2.45[ms]
0.34, 0.02, 0.71, 0.04, 0.05, 0.04, 0.03, 0.13, 0.02, 0.03, 0.17, 0.16, 0.46, 0.04, 0.03, 0.02, 0.02, 0.04, 0.11, 
[1441955914.369553] Timeover: processing time = 3.26[ms]
[ INFO] [1441955914.369894840, 182.879000000]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (torso
0.79, 0.02, 0.15, 0.03, 0.05, 0.05, 0.03, 0.10, 0.02, 0.03, 0.20, 0.21, 1.27, 0.04, 0.04, 0.02, 0.02, 0.05, 0.14, 
[HrpsysSeqStateROSBridge-4] killing on exit
[1441955914.379782] Timeover: processing time = 2.18[ms]
0.30, 0.03, 0.17, 0.04, 0.06, 0.08, 0.03, 0.10, 0.02, 0.03, 0.20, 0.18, 0.60, 0.04, 0.04, 0.02, 0.02, 0.05, 0.17, 
[ INFO] [1441955914.380521390, 182.883000000]: [HrpsysSeqStateROSBridge] @onFinalize : HrpsysSeqStateROSBridge0
^C[1441955914.388363] Timeover: processing time = 6.98[ms]
3.50, 0.03, 0.16, 0.04, 0.06, 0.05, 0.03, 0.10, 0.02, 0.04, 0.22, 1.89, 0.53, 0.04, 0.04, 0.03, 0.02, 0.04, 0.14, 
[1441955914.393037] Timeover: processing time = 2.21[ms]
0.51, 0.02, 0.15, 0.04, 0.05, 0.06, 0.03, 0.08, 0.02, 0.03, 0.20, 0.16, 0.57, 0.04, 0.04, 0.02, 0.02, 0.04, 0.14, 
[ INFO] [1441955914.398256433, 182.889000000]: [HrpsysJointTrajectoryBridge0] @~jointTrajectoryActionObj (head
[1441955914.400344] Timeover: processing time = 2.30[ms]
0.29, 0.02, 0.16, 0.04, 0.06, 0.05, 0.03, 0.09, 0.02, 0.04, 0.20, 0.15, 0.50, 0.04, 0.03, 0.02, 0.02, 0.41, 0.13, 
[1441955914.406378] Timeover: processing time = 2.15[ms]
0.47, 0.02, 0.13, 0.04, 0.05, 0.05, 0.03, 0.08, 0.02, 0.03, 0.19, 0.25, 0.49, 0.04, 0.03, 0.02, 0.02, 0.06, 0.13, 
...

[garaemon]

HrpsysSeqStateROSBridgeの速度計さんの中の割り算が怪しい気がする

[YoheiKakiuchi]

https://github.com/start-jsk/rtmros_common/blob/master/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp#L63-L83
ここらへんで、クロックのありなしで使うスタンプを切り替えていて、現状は、hrpsys-simulator
以外はhrpsysのスタンプを使っていない。
これ、実機もあまりうまく動かないかもしれないけどなぜそうなってるんだろう。

HrpsysSeqStateROSBridge m_baseTform.tm にどんな値が入っているか確かめて、
つかって良さそうな値が入っているなら、stampをそれにしてみるんじゃないかな。
とかいたけど、gazeboはhrpsysに時間を渡せていなくて実際時間が入っているはずだ。

[YoheiKakiuchi]

gazeboでの良い方法はわかっていないのですが、実機やhrpsysでは正しいstampが入るように以下の修正でなると思います。
fkanehiro/hrpsys-base#807

[snozawa]

autoblanacerの出すbaseTformは指令値情報なので、バグってない限りなめらか（計測値より）

速度計算は

• HrpsysROSBridgeでAutoBalancerから位置情報を取得して、差分/dtで速度にする
• そもそもAutoBalancerから速度情報のdat a Portもだす

だと、ほんとは後者がおすすめです。