[AutoBalancer/README.md]add documentation for FootGuidedController

[Naoki-Hiraoka]

FootGuidedControllerの導出を追いつつ自分用に数式をtexで書いたので、もしかしたら誰かの役に立つかもしれないと思いここに共有します。

こちらから読めます
https://github.com/Naoki-Hiraoka/hrpsys-base/blob/PR-tex/rtc/AutoBalancer/README.md

書いていて思ったのですが、dzを引く位置は正しくはここでは無いのではないかと思います。

hrpsys-base/rtc/AutoBalancer/FootGuidedController.cpp

Line 36 in 932498c

if (is_z) u -= dz;

また、数式上のj=nのインデックスが足されていないように思いました

hrpsys-base/rtc/AutoBalancer/FootGuidedController.cpp

Line 32 in 932498c

for (size_t j = 0; j < ref_zmp.size() - 1; j++) { // 数式上のindexとは1ずれてる

これはGaitGeneratorでref_zmpの末尾に余分に1つ追加していたので大丈夫でした。

hrpsys-base/rtc/AutoBalancer/GaitGenerator.cpp

Line 1357 in 932498c

ref_zmp_traj.push_back(LinearTrajectory<hrp::Vector3>(last_cp, last_cp, 1));