[CollisionDetector] Fix collision loop

rtc/CollisionDetector/CollisionDetector.cpp

@@ -373,21 +373,23 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
 #endif // USE_HRPSYSUTIL
 
         //set robot model's angle for collision check(two types)
-        //  1. current safe angle .. check based on qRef
-        //  2. recovery or collision angle .. check based on q'(m_recover_jointdata)
-        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
             if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing
                 for ( int i = 0; i < m_robot->numJoints(); i++ ){
                     m_robot->joint(i)->q = m_qRef.data[i];
                 }
             }
-        }else{   // recovery or collision angle
+        }else{  // 2. recovery
           for ( int i = 0; i < m_robot->numJoints(); i++ ){
+            if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing
               if ( m_curr_collision_mask[i] == 1) {// joint with 1 (do not move when collide :default), need to be updated using recover(safe) data
                   m_robot->joint(i)->q = m_recover_jointdata[i];
               }else{                               // joint with 0 (move even if collide), need to be updated using reference(dangerous) data
                   m_robot->joint(i)->q = m_qRef.data[i];
               }
+            }
           }
         }
         //        }
@@ -404,7 +406,7 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
             if ( sub_size*m_loop_for_check <= i && i < sub_size*(m_loop_for_check+1) ) {
                 CollisionLinkPair* c = it->second;
                 c->distance = c->pair->computeDistance(c->point0.data(), c->point1.data());
-                //std::cerr << i << ":" << (c->distance<=c->pair->getTolerance() ) << " ";
+                //std::cerr << i << ":" << (c->distance<=c->pair->getTolerance() ) << "/" << c->distance << " ";
             }
         }
         if ( m_loop_for_check == m_collision_loop-1 ) {
@@ -451,8 +453,12 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
             }
             if ( m_safe_posture ) {
                 m_have_safe_posture = true;
-                for ( int i = 0; i < m_q.data.length(); i++ ) {
-                    m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
+                if (m_recover_time != default_recover_time) {
+                    // sefe or recover
+                    // in collision, robot->q may differ from m_q.
+                    for ( int i = 0; i < m_q.data.length(); i++ ) {
+                        m_lastsafe_jointdata[i] = m_robot->joint(i)->q;
+                    }
                 }
             }else{
                 for ( int i = 0; i < m_q.data.length(); i++ ) {
@@ -485,13 +491,18 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
           // collision_mask used to select output                0: passthough reference data, 1 output safe data
           std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0); // false(0) clear output data
         } else {
+          static int collision_loop_recover = 0;
           if(m_safe_posture){  //recover
             //std::cerr << "recover-------------- " << std::endl;
             for ( int i = 0; i < m_q.data.length(); i++ ) {
               m_q.data[i] = m_recover_jointdata[i];
             }
             m_recover_time = m_recover_time - i_dt;
-          }else{ //collision
+          } else if (m_collision_loop > 1 && (m_recover_time != default_recover_time)) { // collision with collision_loop
+            //std::cerr << "collision_loop-------------- " << std::endl;
+            collision_loop_recover = m_collision_loop;
+            m_recover_time = default_recover_time;      // m_recover_time should be set based on difference between qRef and q
+          } else { //collision
             //std::cerr << "collision-------------- " << std::endl;
             //do nothing (stay previous m_q)
             m_recover_time = default_recover_time;      // m_recover_time should be set based on difference between qRef and q
@@ -510,9 +521,18 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id)
             m_recover_jointdata[i] = m_q.data[i] + (m_qRef.data[i] - m_q.data[i]) / m_recover_time;
           }
 #else
+          if (collision_loop_recover != 0) {
+            collision_loop_recover--;
+            for ( int i = 0; i < m_q.data.length(); i++ ) {
+              m_q.data[i] =
+                  m_lastsafe_jointdata[i] + collision_loop_recover * ((m_q.data[i] - m_lastsafe_jointdata[i])/m_collision_loop);
+            }
+          } else {
+          collision_loop_recover = 0;
           //minjerk interpolation
           m_interpolator->setGoal(m_qRef.data.get_buffer(), m_recover_time);
           m_interpolator->get(m_recover_jointdata);
+          }
 #endif
         }
         if ( DEBUGP ) {

sample/SampleRobot/CMakeLists.txt

@@ -1,6 +1,7 @@
 # generate .conf and .xml files for two controller period
 set(controller_time_list 0.002 0.005) # [s]
 set(controller_period_list 500 200) # [Hz]
+set(COLLISION_LOOP 1)
 foreach(_idx 0 1)
   list(GET controller_time_list ${_idx} _tmp_controller_time)
   list(GET controller_period_list ${_idx} _tmp_controller_period)
@@ -27,6 +28,8 @@ configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.v
 set(VirtualForceSensorSetting "#virtual_force_sensor") # comment out
 set(JointLimitTableSetting "joint_limit_table")
 configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.el.conf)
+set(COLLISION_LOOP 3)
+configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.co_loop.conf)
 
 set(robot_waist_translation "-0.8  0  0" "-4.15  -0.2  0" "-5.83  -0.2  -0.6096")
 set(file_suffix "SlopeUpDown" "StairUp" "StairDown")
@@ -55,6 +58,8 @@ install(FILES
   # for SoftErrorLimiter
   ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.el.conf
   SampleRobot.PDgain.dat
+  # for collision_loop
+  ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.co_loop.conf
   # TerrainFloor
   ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.SlopeUpDown.xml
   ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.StairUp.xml

sample/SampleRobot/SampleRobot.conf.in

@@ -5,6 +5,7 @@ abc_leg_offset: 0,0.09,0
 end_effectors: lleg,LLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, rleg,RLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, larm,LARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708, rarm,RARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708,
 
 # CollisionDetector Setting
+collision_loop: @COLLISION_LOOP@
 collision_pair: RARM_WRIST_P:WAIST LARM_WRIST_P:WAIST RARM_WRIST_P:RLEG_HIP_R LARM_WRIST_P:LLEG_HIP_R RARM_WRIST_R:RLEG_HIP_R LARM_WRIST_R:LLEG_HIP_R LLEG_ANKLE_R:RLEG_ANKLE_R
 #   Collision mask not to control legs joints
 #collision_mask: 0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1

sample/SampleRobot/samplerobot_collision_detector.py

@@ -14,13 +14,15 @@
     import time
 
 def init ():
-    global hcf, init_pose, col_safe_pose, col_fail_pose
+    global hcf, init_pose, col_safe_pose, col_fail_pose, hrpsys_version
     hcf = HrpsysConfigurator()
     hcf.getRTCList = hcf.getRTCListUnstable
     hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
     init_pose = [0]*29
     col_safe_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,0.15708,-0.113446,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0]
     col_fail_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.845363,0.03992,0.250074,-1.32816,0.167513,0.016204,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0]
+    hrpsys_version = hcf.co.ref.get_component_profile().version
+    print >> sys.stderr, "hrpsys_version = %s"%hrpsys_version
 
 # demo functions
 def demoCollisionCheckSafe ():
@@ -129,5 +131,13 @@ def demo():
     demoCollisionDisableEnable()
     #demoCollisionMask()
 
+def demo_co_loop():
+    init()
+    if hrpsys_version >= '315.10.0':
+        demoCollisionCheckSafe()
+        demoCollisionCheckFail()
+        demoCollisionCheckFailWithSetTolerance()
+        demoCollisionDisableEnable()
+
 if __name__ == '__main__':
     demo()

test/test-samplerobot-co_loop.test

@@ -0,0 +1,10 @@
+<launch>
+  <include file="$(find hrpsys)/launch/samplerobot.launch" >
+    <arg name="GUI" value="false" />
+    <arg name="corbaport" value="2809" />
+    <arg name="CONF_FILE" value="$(find hrpsys)/samples/SampleRobot/SampleRobot.500.co_loop.conf"/>
+  </include>
+
+  <test test-name="samplerobot_co_loop" pkg="hrpsys" type="test-samplerobot-collision.py"
+        args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" retry="2"/>
+</launch>

test/test-samplerobot-collision.py

@@ -9,9 +9,14 @@
 import samplerobot_collision_detector
 import unittest, rostest
 
-class TestSampleRobotCollisionDetector(unittest.TestCase):
-    def test_demo (self):
-        samplerobot_collision_detector.demo()
+if '__name:=samplerobot_co_loop' in sys.argv:
+    class TestSampleRobotCollisionDetector(unittest.TestCase):
+        def test_demo (self):
+            samplerobot_collision_detector.demo_co_loop()
+else:
+    class TestSampleRobotCollisionDetector(unittest.TestCase):
+        def test_demo (self):
+            samplerobot_collision_detector.demo()
 
 ## IGNORE ME: this code used for rostest
 if [s for s in sys.argv if "--gtest_output=xml:" in s] :