Fix collision loop #990

YoheiKakiuchi · 2016-05-12T14:04:58Z

collision_loopを設定すると、挙動がおかしくテストが通らない問題を修正しました。

主には、recover時のcollision_check姿勢の更新タイミングがsafe時と違っているのを修正しました。

collision_loopを使うと、lastsafe_jointdataと出力されている関節角度が違ってしまう
(collision_loopの回数前のデータがlastsafe_jointdata)のをcollision判定されたら戻すようにしました。

k-okada · 2016-05-12T14:43:06Z

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/2620/
Test PASSed.

k-okada · 2016-05-12T15:21:47Z

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/2621/
Test PASSed.

snozawa · 2016-05-13T00:54:19Z

多分これと同様の問題が発生していて、今までcollision_loopが設定できませんでしたが、ありがとうございます。
具体的にどんな問題でしょうか。
こちらで確認したのは、
　collision状態から抜けてこない
だった気がします（前のことなので記憶がアレですが）

k-okada · 2016-05-13T01:53:06Z

11 のテストが落ちているのは期待通り？
https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_stabilizer.py#L30
みたいにバージョンチェックしましょう．

できれば，テストだけ入れた段階でpushしてtravisはしらせて全部Xになることを確認して，修正をpushして治ったか見たい所．

1. TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-base


[ROSTEST]-----------------------------------------------------------------------

[hrpsys.rosunit-samplerobot_co_loop/test_demo][FAILURE]-------------------------
File "/usr/lib/python2.7/unittest/case.py", line 327, in run
    testMethod()
  File "/home/travis/hrpsys_ws/install_isolated/share/hrpsys/test/test-samplerobot-collision.py", line 14, in test_demo
    samplerobot_collision_detector.demo()
  File "/home/travis/hrpsys_ws/install_isolated/share/hrpsys/samples/SampleRobot/samplerobot_collision_detector.py", line 127, in demo
    demoCollisionCheckFail()
  File "/home/travis/hrpsys_ws/install_isolated/share/hrpsys/samples/SampleRobot/samplerobot_collision_detector.py", line 42, in demoCollisionCheckFail
    assert((not cs.safe_posture) is True)
--------------------------------------------------------------------------------


SUMMARY
�[1;31m * RESULT: FAIL�[0m
 * TESTS: 1
 * ERRORS: 0
�[1;31m * FAILURES: 1�[0m

14 : TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-ros-bridge

[ROSTEST]-----------------------------------------------------------------------

[hrpsys_ros_bridge.rosunit-hztest_tf/test_hz][FAILURE]--------------------------
average rate (236.527Hz) exceeded minimum (300.000Hz)
  File "/usr/lib/python2.7/unittest/case.py", line 327, in run
    testMethod()
  File "/opt/ros/hydro/share/rostest/nodes/hztest/hztest", line 115, in test_hz
    self._test_hz(hz, hzerror, topic, test_duration, wait_time)
  File "/opt/ros/hydro/share/rostest/nodes/hztest/hztest", line 185, in _test_hz
    (rate, self.min_rate))
  File "/usr/lib/python2.7/unittest/case.py", line 420, in assertTrue
    raise self.failureException(msg)
--------------------------------------------------------------------------------

[hrpsys_ros_bridge.rosunit-samplerobot/test_force_sensor][passed]
[hrpsys_ros_bridge.rosunit-samplerobot/test_joint_angles][passed]
[hrpsys_ros_bridge.rosunit-samplerobot/test_load_pattern][passed]
[hrpsys_ros_bridge.rosunit-samplerobot/test_odom][passed]
[hrpsys_ros_bridge.rosunit-samplerobot/test_odom_tf][passed]

SUMMARY
�[1;31m * RESULT: FAIL�[0m
 * TESTS: 6
 * ERRORS: 0
�[1;31m * FAILURES: 1�[0m

15: TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-ros-bridge


[hrpsys_ros_bridge.rosunit-hztest_tf/test_hz][FAILURE]--------------------------
average rate (205.674Hz) exceeded minimum (300.000Hz)
  File "/usr/lib/python2.7/unittest/case.py", line 327, in run
    testMethod()
  File "/opt/ros/hydro/share/rostest/nodes/hztest/hztest", line 115, in test_hz
    self._test_hz(hz, hzerror, topic, test_duration, wait_time)
  File "/opt/ros/hydro/share/rostest/nodes/hztest/hztest", line 185, in _test_hz
    (rate, self.min_rate))
  File "/usr/lib/python2.7/unittest/case.py", line 420, in assertTrue
    raise self.failureException(msg)
--------------------------------------------------------------------------------

k-okada · 2016-05-13T01:54:32Z

rtc/CollisionDetector/CollisionDetector.cpp

- if (m_safe_posture && m_recover_time == 0) { // 1. current safe angle
+ // 1. current safe angle or collision angle .. check based on qRef
+ // 2. recovery   .. check based on q'(m_recover_jointdata)
+ if (m_recover_time == 0 || m_recover_time == default_recover_time ) { // 1. current safe angle or collision angle


m_safe_posuture の条件がが消えているけど大丈夫？

以前の条件が冗長でして、
(!m_safe_posture && m_recover_time == 0)
という条件が成立しないので、ここはこれで大丈夫です。

k-okada · 2016-05-13T09:25:19Z

たしかに，一回のPRでテスト中のバージョンチェックはできないですね．いっかい機能を入れてリリースしてバージョンをあげないと，ダメか．どうしましょうかね．

テストなしで入れておく
バージョンチェックをいれておいて，バージョンが上がった時に壊れないことを願う

YoheiKakiuchi · 2016-05-16T09:06:06Z

こちらで確認したのは、
　collision状態から抜けてこない

collision_loopを設定した時は、

collision_loop毎にrobot->qに関節角度を設定
collision_loop回に分けてcollision detectionする (この間常にsafe or recoverモード)
collision_loop毎にモード判定が行われて、モードが変わる

となっています。

safe -> collisionの判定は問題なくて、recoverモードの時だけ、毎ループrobot->q (collision checkされる関節角度)を更新していて、
collision_loopの回数だけ、collision判定されるまでに遅れることがあって、そうするとrobot->qがcollisionしている姿勢にもかかわらず、safeであると判定され、それがlastsafe_angleになってしまうことがある状況だった。

そうなると、そこから復帰するための補間がなされると、hoffarbib補間では最初の1ステップはごく小さいので、lastsafe_angleがsafeでないと、復帰のための1ステップ目がcollisionしているため、recoverの動作に移行できないということだった。

YoheiKakiuchi · 2016-05-16T09:13:51Z

11 のテストが落ちているのは期待通り？

これは、期待とはちょっと違うのですが、これまでのCollisionDetectorのテストは確率的に落ちていたのではないかと思っています。
というのは、collisionしたあとに、referenceがcollisionしていても、referenceとlastsafe_angleの補間がsafeならば、recoverモードになっていて、collisionする限界までrecoverを試みるという動作になっていました。
これだと、interpolationが終わったあとであっても、recover->collision->recoverと動いており、recover時はcollisionしていないため、
collision姿勢にして、collisionしているか調べると、recoverモードの時はcollisionしていないという結果になり、collisionモードの時はcollisionしているという結果になり、wait_interpolationのあとにどれぐらいループが回ったかで変化してしまいます。

これについても、collisionしたあとのcollision判定をreferenceを見るように変更しています。
recover時はこれまで通り、recoverの出力angleのcollision判定をします。

YoheiKakiuchi · 2016-05-20T11:54:18Z

#993
にてマージされたので閉じます。

YoheiKakiuchi added 2 commits May 12, 2016 23:13

add test for collision_loop on CollisionDetector

740f2eb

[CollisionDetector] fix behavior when using collision_loop

2a07885

YoheiKakiuchi force-pushed the fix_collision_loop branch from 1b370e0 to 2a07885 Compare May 12, 2016 14:13

k-okada reviewed May 13, 2016
View reviewed changes

YoheiKakiuchi mentioned this pull request May 13, 2016

check version in CMakeLists.txt against package.xml #991

Merged

YoheiKakiuchi mentioned this pull request May 16, 2016

Add collision loop test #992

Merged

YoheiKakiuchi mentioned this pull request May 17, 2016

[CollisionDetector] Fix collision loop #993

Merged

YoheiKakiuchi closed this May 20, 2016

YoheiKakiuchi deleted the fix_collision_loop branch May 24, 2016 04:13

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix collision loop #990

Fix collision loop #990

YoheiKakiuchi commented May 12, 2016

k-okada commented May 12, 2016

k-okada commented May 12, 2016

snozawa commented May 13, 2016

k-okada commented May 13, 2016

k-okada May 13, 2016

YoheiKakiuchi May 16, 2016

k-okada commented May 13, 2016

YoheiKakiuchi commented May 16, 2016

YoheiKakiuchi commented May 16, 2016

YoheiKakiuchi commented May 20, 2016

Fix collision loop #990

Fix collision loop #990

Conversation

YoheiKakiuchi commented May 12, 2016

k-okada commented May 12, 2016

k-okada commented May 12, 2016

snozawa commented May 13, 2016

k-okada commented May 13, 2016

k-okada May 13, 2016

Choose a reason for hiding this comment

YoheiKakiuchi May 16, 2016

Choose a reason for hiding this comment

k-okada commented May 13, 2016

YoheiKakiuchi commented May 16, 2016

YoheiKakiuchi commented May 16, 2016

YoheiKakiuchi commented May 20, 2016