fix bug when overwriting footstep

rtc/AutoBalancer/GaitGenerator.cpp

@@ -540,18 +540,7 @@ namespace rats
 
   bool gait_generator::proc_one_tick ()
   {
-    hrp::Vector3 rzmp;
-    std::vector<hrp::Vector3> sfzos;
-    bool refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after);
-    if (!refzmp_exist_p) {
-      finalize_count++;
-      rzmp = prev_que_rzmp;
-      sfzos = prev_que_sfzos;
-    } else {
-      prev_que_rzmp = rzmp;
-      prev_que_sfzos = sfzos;
-    }
-    bool solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-default_step_time/dt));
+    solved = false;
     /* update refzmp */
     if (emergency_flg == EMERGENCY_STOP && lcg.get_footstep_index() > 0) {
         leg_type cur_leg = footstep_nodes_list[lcg.get_footstep_index()].front().l_r;
@@ -589,6 +578,22 @@ namespace rats
         overwrite_footstep_nodes_list.clear();
       }
     }
+
+    if ( !solved ) {
+      hrp::Vector3 rzmp;
+      std::vector<hrp::Vector3> sfzos;
+      bool refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after);
+      if (!refzmp_exist_p) {
+        finalize_count++;
+        rzmp = prev_que_rzmp;
+        sfzos = prev_que_sfzos;
+      } else {
+        prev_que_rzmp = rzmp;
+        prev_que_sfzos = sfzos;
+      }
+      solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-default_step_time/dt));
+    }
+
     rg.update_refzmp(footstep_nodes_list);
     // { // debug
     //   double cart_zmp[3];
@@ -877,11 +882,10 @@ namespace rats
     }
     /* fill preview controller queue by new refzmp */
     hrp::Vector3 rzmp;
-    bool not_solved = true;
-    while (not_solved) {
+    while ( !solved ) {
       std::vector<hrp::Vector3> sfzos;
       bool refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after);
-      not_solved = !preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, refzmp_exist_p);
+      solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, refzmp_exist_p);
       rg.update_refzmp(footstep_nodes_list);
     }
   };

rtc/AutoBalancer/GaitGenerator.h

@@ -879,6 +879,7 @@ namespace rats
     bool use_inside_step_limitation;
     std::map<leg_type, std::string> leg_type_map;
     coordinates initial_foot_mid_coords;
+    bool solved;
 
     /* preview controller parameters */
     //preview_dynamics_filter<preview_control>* preview_controller_ptr;

sample/SampleRobot/samplerobot_auto_balancer.py

@@ -93,6 +93,39 @@ def checkGoPosParam (goalx, goaly, goalth, prev_dst_foot_midcoords):
     assert(ret)
     return ret
 
+def calcVelListFromPosList(pos_list, dt):
+    '''Calculate velocity list from position list.
+    Element of pos_list and vel_list should be list like [0,0,0].
+    '''
+    vel_list=[]
+    ppos=pos_list[0]
+    for pos in pos_list:
+        vel_list.append(map(lambda x, y: (x-y)/dt, pos, ppos));
+        ppos=pos
+    return vel_list
+
+def checkTooLargeABCCogAcc (acc_thre = 5.0): # [m/s^2]
+    '''Check ABC too large cog acceleration.
+    This is used discontinuous cog trajectory.
+    '''
+    # Parse COG [m] and tm [s]
+    cog_list=[]
+    tm_list=[]
+    for line in open("/tmp/test-abc-log.abc_cogOut", "r"):
+        tm_list.append(float(line.split(" ")[0]));
+        cog_list.append(map(float, line.split(" ")[1:-1]));
+    cog_list=cog_list[:-1000] # ?? Neglect latter elements
+    dt = tm_list[1]-tm_list[0] # [s]
+    # Calculate velocity and acceleration
+    dcog_list=calcVelListFromPosList(cog_list, dt)
+    ddcog_list=calcVelListFromPosList(dcog_list, dt)
+    # Check max
+    max_cogx_acc = max(map(lambda x : abs(x[0]), ddcog_list))
+    max_cogy_acc = max(map(lambda x : abs(x[1]), ddcog_list))
+    ret = (max_cogx_acc < acc_thre) and (max_cogy_acc < acc_thre)
+    print >> sys.stderr, "  Check acc x = ", max_cogx_acc, ", y = ", max_cogy_acc, ", thre = ", acc_thre, "[m/s^2], ret = ", ret
+    assert(ret)
+
 def demoAutoBalancerFixFeet ():
     print >> sys.stderr, "1. AutoBalancer mode by fixing feet"
     hcf.startAutoBalancer(["rleg", "lleg"]);
@@ -219,11 +252,19 @@ def demoGaitGeneratorGoPos():
 
 def demoGaitGeneratorGoVelocity():
     print >> sys.stderr, "2. goVelocity and goStop"
+    print >> sys.stderr, "  goVelocity few steps"
     hcf.abc_svc.goVelocity(-0.1, -0.05, -20)
     time.sleep(1)
     hcf.abc_svc.goStop()
     checkActualBaseAttitude()
-    print >> sys.stderr, "  goVelocity()=>OK"
+    print >> sys.stderr, "  goVelocity few steps=>OK"
+    print >> sys.stderr, "  Check discontinuity of COG by checking too large COG acc."
+    hcf.setMaxLogLength(10000)
+    hcf.clearLog()
+    hcf.abc_svc.goVelocity(0,0,0) # One step overwrite
+    hcf.abc_svc.goStop()
+    hcf.saveLog("/tmp/test-abc-log");
+    checkTooLargeABCCogAcc()
 
 def demoGaitGeneratorSetFootSteps():
     print >> sys.stderr, "3. setFootSteps"