[Stabilizer.cpp] fix com height of LIPM in Capture Point calculation

[eisoku9618]

@snozawa @YutaKojio

CPの計算式はこうではないでしょか？

• act_cog / act_cogvel / act_zmp は全部 foot_origin座標系相対
• foot_origin座標系のz方向は重力ベクトルの反対方向と一致
• foot_origin座標系の原点は足先リンク座標系の原点と一致
• CPの計算式に出てくるzは倒立振子の高さ

という理解で合っているとすると

(elt
 (v-
  (send (send (car (send *robot* :rleg :force-sensors)) :parent) :worldpos)
  (send (send *robot* :rleg :end-coords) :worldpos))
 2)

の分だけずれているように思いました．

[k-okada]

Refer to this link for build results (access rights to CI server needed):
http:https://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/2501/
Test PASSed.

[YutaKojio]

はい．こちらのほうが正確だと思います．
すみませんが，ref_cpも同じように変更お願いします．
https://github.com/eisoku9618/hrpsys-base/blob/patch-1/rtc/Stabilizer/Stabilizer.cpp#L1037

[k-okada]

Refer to this link for build results (access rights to CI server needed):
http:https://jenkins.jsk.imi.i.u-tokyo.ac.jp:8080/job/hrpsys-qnx/2504/
Test PASSed.

[eisoku9618]

ref_cpも同じように変更しました．

[snozawa]

LGTM。
travisも通ってるようですので、お手すきのときにご確認いただけますと幸いです。