add test program for kf precision

[eisoku9618]

hrpsys-baseの姿勢推定コンポーネントの精度を評価するテストプログラムを書きました．

roscd hrpsys_ros_bridge_tutorials/test/test-kf/
roseus

(load "check-kf-precision.l")
(main :deflection-ypr-angle (float-vector 0 (/ pi 6) 0) :wrt :world :init-ypr-angle (float-vector 0 0 0))

で下記のプロットが出ます．

このロボットはIMUがend-coordsについているようなので，

画像の左の列：kf_rpyが出しているRPYとルートリンクを地面に固定してForwardKinematicsで求めたIMUのRPY
画像の右の上・真ん中：IMUが出力している加速度・ジャイロ
画像の右下：end-coordsの位置

をプロットしています．

(load "check-kf-precision.l")
(main :deflection-ypr-angle (float-vector 0 (/ pi 6) 0) :wrt :world :init-ypr-angle (float-vector 0 0 0))

では，初期姿勢を init-ypr-angle で与え，
そこから目標姿勢 deflection-ypr-angle までIMUの位置はずらさずに振り子のような形で動くようなIKを解いています．

[k-okada]

マージしました．こういう需要なプログラムは， start-jsk/rtmros_common -> start-jsk/hrpsys ->fkanehiro/hrpsys-base とよりupstreamの部分でテストされるよう，持って行くとよいとおもいます．先ずはeuslisp依存を外せばstart-jsk/hrpsysにいれられて，https://github.com/fkanehiro/hrpsys-base/blob/master/.travis.sh#L163 のあたりでrostestが動きます．

もちろん，fkanehiro/hrpsys-base/.travis.yml でrtmros_tutorialsのテストを含んでもいいですが，なにか動かなかった時に面倒を見なければいけなくなるので，あまりおすすめしません．

[garaemon]

これは素晴らしいですね！

2014年8月12日火曜日、Kei [email protected]さんは書きました:

マージしました．こういう需要なプログラムは， start-jsk/rtmros_common -> start-jsk/hrpsys
->fkanehiro/hrpsys-base
とよりupstreamの部分でテストされるよう，持って行くとよいとおもいます．先ずはeuslisp依存を外せばstart-jsk/hrpsysにいれられて，
https://github.com/fkanehiro/hrpsys-base/blob/master/.travis.sh#L163
のあたりでrostestが動きます．

もちろん，fkanehiro/hrpsys-base/.travis.yml
でrtmros_tutorialsのテストを含んでもいいですが，なにか動かなかった時に面倒を見なければいけなくなるので，あまりおすすめしません．

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

from iPhone

[snozawa]

こういう需要なプログラムは， start-jsk/rtmros_common -> start-jsk/hrpsys ->fkanehiro/hrpsys-base とよりupstreamの部分でテストされるよう，持って行くとよいとおもいます

hrpsys-baseだけで動かせる（はず）のサンプルとロボットを
[1] fkanehiro/hrpsys-base#281
[2] start-jsk/hrpsys#104
に追加しています。

[1]のsample6dofrobot_kalman_filter.pyがそれですが、ROSユーザであれば
downstreamパッケージであるhrpsysにある[2]も使えて、

[terminal1]
$ rtmlaunch hrpsys sample6dofrobot.launch

[terminal2]
$ rosrun hrpsys sample6dofrobot-kalman-filter.py

でサンプル実行できます。
6dofロボットは、

いや、モデルはopenhrp3に入っているので新しくbody一個imu一個のモデルを作るというのはどうでしょう？

にかなりちかいものになってるとおもいます。
直動関節x3、回転関節x3で６自由度がsetJointAnglesでいじれて、
その一番先にIMUがついてます。

直動を動かさなければ、

そこから目標姿勢 deflection-ypr-angle までIMUの位置はずらさずに振り子のような形で動くようなIKを解いています．

がいらずに、（IKもいらないので、）ほぼEuslispなしでサンプルがかけるようになってると思います。

なので、プログラムがsample6dofrobot_kalman_filter.py (hrpsys-baseの方)に追加しPRだす形でかけないか検討してみてください。