[AutoBalancer] goPosが返ってこない

[YoheiKakiuchi]

現状わかったことだけ報告します。

goPosを行うと、収束せずに無限ループにおちいり
（このとき、ステップを作り続けるのでメモリ使用量が増え続ける）

デバッグメッセージからこの部分で収束していないことが分かっている。
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/AutoBalancer/GaitGenerator.cpp#L609

原因としては、goPosで渡しているパラメータはx,y,thetaでz方向の変位は行わないが、
dpにz方向成分が入ってしまって、その値がしきい値を超えていると収束判定されない。

具体的には、ここで start_ref_coordsにx,yの回転成分を持ったcoordsが入る。
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/AutoBalancer/GaitGenerator.cpp#L612

その原因は、target_end_coordsにx,yの回転成分を持ったcoordsが入ることにある。
（これはそれほど問題ではない？？）
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/AutoBalancer/AutoBalancer.cpp#L1093-L1097
現状では、左右脚のtarget_coordsが対称でない値がはいっている。

それは、mid_rotでがeps=がしきい値以下のときは、１つめの回転と同じ回転を返すが、
しきい値（現状 0.001が）が大きく誤差が蓄積しているようだ。
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/ImpedanceController/RatsMatrix.cpp#L49

しきい値を 1e-8 程度にすると、左右脚のtarget_coordsは対称になった。
（x,yの回転成分は変わらずに入っている）
以下のバグが起こる条件は回避できているが、
x,yの回転成分は変わらずに入っていると思われる。

以下のgoPosの値で確実にバグが再現します。

 hcf.abc_svc.goPos(8.26252, -0.005596, -0.0012)

以下は同じ問題ですが、原因がAutoBalancerとはっきりしたのでこちらに移します。
start-jsk/rtmros_choreonoid#104

[snozawa]

これはどのjaxon+14.04で再現しますか？
手元のhrpsys-simulatorでjaxon_red+12.04, samplerobot+12.04では再現しませんでした。

[YoheiKakiuchi]

stを入れないと再現しないように思えてきました。
使っているのはchoreonoid+jaxon_jvrc ( https://github.com/start-jsk/rtmros_choreonoid ) です。

stを切ると、target_coordsのrotationが単位行列になって問題が出なくなります。
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/AutoBalancer/AutoBalancer.cpp#L1093-L1097

[snozawa]

stを切ると、target_coordsのrotationが単位行列になって問題が出なくなります。

これはprint文で確認されてますか？
平地であるいているぶんには、abcの中のこの部分はセンサフィードバックの値が入らないと思ってるのですが、、、

[YoheiKakiuchi]

これはprint文で確認されてますか？

はい．
stの入っているときと，入っていないときで変化します．
それほど大きくない(マイナス5乗 程度)ですが，
ここに 10mくらいの距離をかけると，0.1mmの閾値をこえます．

[snozawa]

hcf.startAutoBalancer()
hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), 
OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [0.999048, 0.0, 0.043619, 0.0], "lleg")])]);
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.goPos(1,0,0)

でも再現しました。

[snozawa]

上記の再現コードでは
#849
で解決しているので、お試しいただけますでしょうか。

[YoheiKakiuchi]

goPosは帰ってくるようになりました。

僕には判断がつきませんが、解決策はこれで問題ないですか？
真の原因がどこかにあったりしないでしょうか？

[snozawa]

#849
の解決法はどのみち必要なものでした。

真の原因がどこかにあったりしないでしょうか？

上記変更で（傾斜が一定な）傾斜のある面をgoposすることもできます。
が、そもそもstをいれていてもEusからangle-vectorを送る分には
target_end_coordsはほぼずれてかないはずだと思っているので、
そこに何かまだみきれてない問題があるかもしれません。

[YoheiKakiuchi]

現状，問題はなくなったようなので閉じます．