Add checking for discontinuity of COG trajectory during footstep overwriting by checking COG too large acc.

[snozawa]

#940
に関連して、ABCがoverwrite footstepなときに重心軌道が不連続になるのを
チェックするものを追加しました。
goVelocity時にoverwrite がおこったときに、COGが不連続になっってるかを
過大な重心加速度をチェックしてます。

[snozawa]

@YutaKojio
goVelocity(0,0,0)したときの重心加速度は通常で1.25[m/s^2]くらいで、
不連続になってるときには10[m/s^2]くらいだったので、5[m/s^2]をしきい値にしました。

のグラフがそのときのものですが、縦軸が[mm/s^2]になってるので1000分の１してください。

このPRは、ABCをチェックしてるtravisのJOBの9番がエラるかどうかをチェックするためのものです。
エラってるのが確認できればcloseします。

[k-okada]

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

[snozawa]

https://travis-ci.org/fkanehiro/hrpsys-base/builds/101223639
の９番が無事エラーになりました。
https://api.travis-ci.org/jobs/101223648/log.txt?deansi=true
をみると

]]></system-out>
  <system-err><![CDATA[1. AutoBalancer mode by fixing feet
  ret =  True , actual base rpy = ( RTC.Orientation3D(r=1.2143955797444039e-08, p=-0.0008643847322727575, y=0.0001644610797460533) ),  reference base rpy = ( RTC.Orientation3D(r=0.0, p=-0.0, y=0.0) )
  Start and Stop AutoBalancer by fixing feet=>OK
2. AutoBalancer mode by fixing hands and feet
  ret =  True , actual base rpy = ( RTC.Orientation3D(r=-1.000293746778374e-08, p=-0.000913829302577853, y=0.00016449515961479777) ),  reference base rpy = ( RTC.Orientation3D(r=0.0, p=-0.0, y=0.0) )
  Start and Stop AutoBalancer by fixing hands and feet=>OK
3. getAutoBalancerParam
  getAutoBalancerParam() => OK
4. setAutoBalancerParam
  default_zmp_offsets setting check in start and stop
  setAutoBalancerParam() => OK
5. change base height, base rot x, base rot y, and upper body while AutoBalancer mode
  ret =  True , actual base rpy = ( RTC.Orientation3D(r=1.568184775134931e-07, p=-0.001318276325797973, y=0.0001782685201600477) ),  reference base rpy = ( RTC.Orientation3D(r=0.0, p=-0.0, y=0.0) )
6. start stop check
  ret =  True , actual base rpy = ( RTC.Orientation3D(r=-1.1347043089517451e-07, p=-0.0009211752693307355, y=-0.0071556220414199755) ),  reference base rpy = ( RTC.Orientation3D(r=0.0, p=-0.0, y=0.0) )
7. balance against hand force
  ret =  True , actual base rpy = ( RTC.Orientation3D(r=1.0220011665805021e-07, p=-0.000923046059979365, y=-0.007155697276026834) ),  reference base rpy = ( RTC.Orientation3D(r=0.0, p=-0.0, y=0.0) )
8. balance with arms
  startAutoBalancer with arms
  stopAutoBalancer
  ret =  True , actual base rpy = ( RTC.Orientation3D(r=-3.660998887802859e-08, p=-6.223505788467614e-05, y=-0.007338032974502039) ),  reference base rpy = ( RTC.Orientation3D(r=0.0, p=-0.0, y=0.0) )
1. goPos
  Check goPosParam (diff =  8.77505583942e-06 [m],  -7.1379475941e-10 [m],  -1.65431772103e-06 [deg])
  =>  True
  Check goPosParam (diff =  8.32667268469e-17 [m],  -5.55111512313e-17 [m],  8.34887714518e-14 [deg])
  =>  True
  ret =  True , actual base rpy = ( RTC.Orientation3D(r=-0.00020571636719443091, p=-0.0008868793406020385, y=0.1666059933101285) ),  reference base rpy = ( RTC.Orientation3D(r=-0.0002447053129119351, p=-0.0008609446828861586, y=0.1747280729624605) )
  goPos()=>OK
2. goVelocity and goStop
  goVelocity few steps
  ret =  True , actual base rpy = ( RTC.Orientation3D(r=-0.000283376818397455, p=-0.0008928039953687736, y=-0.008245072248487808) ),  reference base rpy = ( RTC.Orientation3D(r=-0.0002447053129119348, p=-0.0008609446828861592, y=0.0001951477630276062) )
  goVelocity few steps=>OK
  Check discontinuity of COG by checking too large COG acc.
  Check acc x =  0.0 , y =  11.4999999999 , thre =  5.0 [m/s^2], ret =  False
]]></system-err>
</testsuite>
<?xml version="1.0" encoding="utf-8"?>
<testsuite errors="0" failures="1" name="unittest.suite.TestSuite" tests="1" time="109.194">
  <testcase classname="__main__.TestSampleRobotAutoBalancer" name="test_demo" time="109.1942">
    <failure type="AssertionError">
  File "/usr/lib/python2.7/unittest/case.py", line 327, in run
    testMethod()
  File "/home/travis/catkin_ws/install_isolated/share/hrpsys/test/test-samplerobot-abc.py", line 14, in test_demo
    samplerobot_auto_balancer.demo()
  File "/home/travis/catkin_ws/install_isolated/share/hrpsys/samples/SampleRobot/samplerobot_auto_balancer.py", line 686, in demo
    demoGaitGeneratorGoVelocity()
  File "/home/travis/catkin_ws/install_isolated/share/hrpsys/samples/SampleRobot/samplerobot_auto_balancer.py", line 267, in demoGaitGeneratorGoVelocity
    checkTooLargeABCCogAcc()
  File "/home/travis/catkin_ws/install_isolated/share/hrpsys/samples/SampleRobot/samplerobot_auto_balancer.py", line 127, in checkTooLargeABCCogAcc
    assert(ret)
    </failure>
  </testcase>

のエラーしたrostsetの出力でcheckTooLargeABCCogAcc()が失敗しているのがわかります。