fix bug when overwriting footstep

[YutaKojio]

footstepを上書くためにoverwrite_refzmp_queue を呼んだときに，
Autobalancerが出すcogの値が飛ぶバグがあったので，修正しました．

・いままで

・このPR

(progn
  (load "package:https://hrpsys_ros_bridge_tutorials/euslisp/samplerobot-interface.l")
  (samplerobot-init)
  (setq *robot* *sr*)
  (send *ri* :angle-vector (send *robot* :angle-vector) 2000)
  (send *ri* :wait-interpolation)
  (send *ri* :start-auto-balancer)
  (send *ri* :start-st)
  (send *ri* :go-velocity 0.1 0 0)
  (unix:sleep 5)
  (send *ri* :go-stop))

[snozawa]

なるほど
確認してみます。

[k-okada]

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

[snozawa]

これって、overwriteじか、普段の時に、一回updateの回数が多いのかな？

[YutaKojio]

overwriteの時にupdateが一回多いです．
overwrite時は同じ制御周期内で，
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/AutoBalancer/GaitGenerator.cpp#L554
と
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/AutoBalancer/GaitGenerator.cpp#L884
で二度updateするために，
overwriteしたときに，1制御周期分飛んだcogが出ていました．
なので，解決策はこのPRか，
overwrite時は
https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/AutoBalancer/GaitGenerator.cpp#L554
を呼ばないようにするのでもいいかもしれないです．

[snozawa]

• コメント１

  Overwrite時は
  https://github.com/fkanehiro/hrpsys-base/blob/master/rtc/AutoBalancer/GaitGenerator.cpp#L554
  をよばないようにするので、問題なく動きそうかな？であればそうしたほうがよさそうですね。
  もともとのこのPRの変更の、PreviewController側をなおす、というのではなく、「updateは常にこのproc_one関数で１度のみよばれる」というふうにした方が混乱ないかもしれないので。
  必要であればproc_one関数でoverwriteしてる箇所を通常のupdate関数の近くにしたり、配置しなおしてもよいかも。

• コメント２

  Add checking for discontinuity of COG trajectory during footstep overwriting by checking COG too large acc. #941
  にテスト関数を作りました。このPRなしではエラるはずのものです。
  Add checking for discontinuity of COG trajectory during footstep overwriting by checking COG too large acc. #941 の内容をこのPRにmergeして、テストもいれて通るか（手元＋travis）確認してもらえるとたすかます。
  Add checking for discontinuity of COG trajectory during footstep overwriting by checking COG too large acc. #941 はこのPRの修正がないので、えらって終わるはず。

[YutaKojio]

はい．コメント１コメント２のとおり変更，確認します．

[YutaKojio]

proc_one関数内で２回以上updateしないように変更しました．
また，手元でtravisが通ることを確認しました．

@snozawa さん
ご確認よろしくお願い致します．

[k-okada]

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

[snozawa]

LGTM.

travisも通ってるようですのでmergeをお願いできますと助かります。