足力センサの値が振動してstを入れると転ぶ

[mmurooka]

HRP2などの力センサの値が振動していて，
おそらくstの中の接地状態に基づくON/OFF昨日が振動してしまうことが原因で，
start-stすると転んでしまいます．

STなし足力センサz成分

STあり足力センサz成分

Atlasだと起きませんでした．力の取得法が違うのでしょうか．

[mmurooka]

昔は以下の動画くらい歩いていましたが，今は@s-noda さんがパラチュンしてどうにか立てるようになった程度のようです．原因は何でしょう．．
https://drive.google.com/file/d/0Bzug7iC2FcWQcUh0dXJEbFlDazA/view?usp=sharing

[s-noda]

まったく転けそうじゃないですね。 > 動画

今のSTだと、かなりダンパ成分を大きくしないと振動してしまって立てないですが、
そうするとSTを切っている状態に近いことになってしまうのでイマイチという感じです。

おそらくstの中の接地状態に基づくON/OFF昨日が振動してしまうことが原因で，

このON/OFF機能は、今のSTで簡単に切って試すことができるでしょうか？

[YoheiKakiuchi]

ここのサンプルコードと同じようなことをしている（初期姿勢、start-st、go-pos）ということでいいでしょうか？
https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/test/hrpsys-samples/samplerobot-stabilizer.l

もともとどうだったか分からないのですが、手元の環境で、hrpsys-baseをmasterにすると、
samplerobotではstart-stの段階でこけてしまいます。

[mmurooka]

ここのサンプルコードと同じようなことをしている（初期姿勢、start-st、go-pos）ということでいいでしょうか？

はい，そうです．
歩いて数歩くらいで，突然飛び跳ねるようにして転んでしまう感じです．
stを入れないとそのような現象は起きません（が追従性が悪くて転びます．）

[snozawa]

このあたり、解決したいですね。
@s-nodaさん
最新のシミュレーションパラメータをもってたら教えてもらえるとたすかります。
また、力センサはLPFをかけたほうがいいんでしたっけ？
この辺も情報共有してもらえるとたすかります。

（力学シミュレーションありで）やりたいシミュレーションは、ST、impなどがはいったシミュレーションができたらうれしいので、
関節サーボ追従性が概ね実機に近くて、センサフィードバックいれても発散しないかんじになるとうれしいです。

[s-noda]

また、力センサはLPFをかけたほうがいいんでしたっけ？

s-noda@a099ab3

大昔に書いたコードを今さっき手元でマージしてみたものですが，
getRobotStates を getForceSensors, getImuSensors, getJointSensors の３つにわけて， getforcesensors で数ステップ分の平均を取るように変えています．

大昔の記憶だと， 力センサの値を 20~30msec ぐらいで平均して使うと，実機を静的に動かしているときにとった力センサのグラフと gazebo で同じ動作をさせたときのグラフがだいたい同じような見た目になった記憶があります．

まだ，動作確認してないのと，今の実装だと robotstates のトピックの周期も遅くなってしまう問題があるので，もうちょっと直してから pull request を出します．

（一ヶ月ぶりに update したら grub rescue があがりました．．．）

[k-okada]

Gazeboの中かプラグイン側で対応できないかな

◉ Kei Okada

2015/06/23 0:46、Noda, Shintaro notifications@github.com のメッセージ:

また、力センサはLPFをかけたほうがいいんでしたっけ？

s-noda/rtmros_gazebo@a099ab3

大昔に書いたコードを今さっき手元でマージしてみたものですが，
getRobotStates を getForceSensors, getImuSensors, getJointSensors の３つにわけて， getforcesensors で数ステップ分の平均を取るように変えています．

大昔の記憶だと， 力センサの値を 20~30msec ぐらいで平均して使うと，実機を静的に動かしているときにとった力センサのグラフと gazebo で同じ動作をさせたときのグラフがだいたい同じような見た目になった記憶があります．

まだ，動作確認してないのと，今の実装だと robotstates のトピックの周期も遅くなってしまう問題があるので，もうちょっと直してから pull request を出します．

（一ヶ月ぶりに update したら grub rescue があがりました．．．）

—
Reply to this email directly or view it on GitHub.

[s-noda]

IOBPlugin で書いてます

[k-okada]

Hrpsys依存しないプラグインでかいてgazebo側にコードを置きたいところです

◉ Kei Okada

2015/06/23 0:52、Noda, Shintaro notifications@github.com のメッセージ:

IOBPlugin で書いてます

—
Reply to this email directly or view it on GitHub.

[s-noda]

力センサの値のみをとって，平均して，投げるだけのプラグインに分けたがいいという意味ですか？

僕もそっちがいい気がしてきました．今後もっと複雑なモデルを使いたくなった時のためにも．

[k-okada]

はい

2015年6月23日火曜日、Noda, Shintaronotifications@github.comさんは書きました:

力センサの値のみをとって，平均して，投げるだけのプラグインに分けたがいいという意味ですか？

僕もそっちがいい気がしてきました．今後もっと複雑なモデルを使いたくなった時のためにも．

—
Reply to this email directly or view it on GitHub
#119 (comment)
.

◉ Kei Okada

[snozawa]

力センサの値を 20~30msec ぐらいで平均して使うと，

20-30msecの平滑化はだいぶフィルタしすぎじゃないかな？
実機でも力センサ側にLPFを強めにいれたら、センサの応答が悪くて挙動が悪くなりました。
@eisoku9618 さんの方法でsimulationのstepを細かくしてるので、例えば
1msecでsimulation => (hrp2なら) 4msecで制御周期
となってたら1msecの力センサを4msec分（4サンプリング）平滑化するのだとどうでしょうか。

[s-noda]

となってたら1msecの力センサを4msec分（4サンプリング）平滑化するのだとどうでしょうか。

はい、平均とる幅は外から変えられるようにしていくつか試してみるのがいいと思います。

力センサの値を平滑化して publish するプラグインを作りました。
あとはこれを robot_state のトピックに入れなおせば、hrpsys の中でも平滑化が効くと思います。
https://github.com/s-noda/rtmros_gazebo/tree/force_sensor_filter2

ところでいまさらなのですが、 gazebo の plugin 間の通信は ROS を使うのが正解でしょうか？　今

ForceSensorPlugin (新しくつくったやつ） --[平滑化された力センサの出力]--> IOBPlugin --[robot_state]--> iob.cpp 

なものをつくろうと思っています。
通信方法の候補は ROS か gazebo topic(?) かぐらいが簡単そうなのですが、同じ gazebo の中で動く plugin 間の通信なので、もっといい方法があるような気もします。

また、ROSの通信を使う場合、IOBPlugin を見ると ROS の通信は 100Hz で行われている気がしますが、これは 1000Hz ぐらいにしても問題ないものでしょうか？

[garaemon]

ROSで1000Hzは結構重いですね。それだけでCPUをけっこう喰います

[YoheiKakiuchi]

また、ROSの通信を使う場合、IOBPlugin を見ると ROS の通信は 100Hz で行われている気がしますが

そうだっけ？ 全力で送っているような気がするけど。

[s-noda]

そうだっけ？ 全力で送っているような気がするけど。

すいません。当てずっぽうで言ってますが。

https://github.com/start-jsk/rtmros_gazebo/blob/master/hrpsys_gazebo_general/src/IOBPlugin.cpp#L940

で

ros::spinOnce のしたで
this->rosQueue.callAvailable(ros::WallDuration(timeout));
してて、 timeout=0.01
となっているのは、 sleep してるわけではなかったですか？

[s-noda]

あ、IOBPlugin が publish する話でなく、 subscribe する話です。

[garaemon]

ROSで1000Hzは結構重いですね。それだけでCPUをけっこう喰います

C++同士なら通信をUDP-ROSにするといいかも. imuや力センサくらいなら動きますね

[YoheiKakiuchi]

ところでいまさらなのですが、 gazebo の plugin 間の通信は ROS を使うのが正解でしょうか？　今

ForceSensorPlugin (新しくつくったやつ） --[平滑化された力センサの出力]--> IOBPlugin --[robot_state]--> > iob.cpp 

なものをつくろうと思っています。

いろいろ見たのですが、gazebo側に入れるならプラグインではなくてセンサーモデル（のソース）を変更するんじゃないかと思い始めました。

Hrpsys依存しないプラグインでかいてgazebo側にコードを置きたいところです

これは考えておくとして、ひとまず、IOBPluginに入れて効果の程を見てみないかな？

[YoheiKakiuchi]

ros::spinOnce のしたで
this->rosQueue.callAvailable(ros::WallDuration(timeout));
してて、 timeout=0.01
となっているのは、 sleep してるわけではなかったですか？

キューにメッセージがなかったら最大0.01秒待つはずで、あったら即実行されているはずです。
http://docs.ros.org/diamondback/api/roscpp/html/classros_1_1CallbackQueue.html

C++同士なら通信をUDP-ROSにするといいかも. imuや力センサくらいなら動きますね

こちらは、robot_state全体を送るときにMTUが大きくなるのでTCPになっています。

[garaemon]

こちらは、robot_state全体を送るときにMTUが大きくなるのでTCPになっています。

RobotState型みたいなのがあるのでしょうか?

[YoheiKakiuchi]

これです
https://github.com/start-jsk/rtmros_gazebo/blob/master/hrpsys_gazebo_msgs/msg/RobotState.msg

[garaemon]

おお、なるほど。MTU=1500をオーバーするのですね。

[s-noda]

IOBPlugin ないでフィルタするものを PR しました #198

いろいろ見たのですが、gazebo側に入れるならプラグインではなくてセンサーモデル（のソース）を変更するんじゃないかと思い始めました。

これいいですね。