[rtc/AutoBalancer/GaitGenerator.cpp, rtc/AutoBalancer/PreviewControll…

…er.h] fix bug when overwriting footstep
fkanehiro · Jan 9, 2016 · 2b8bb47 · 2b8bb47
1 parent 3df67aa
commit 2b8bb47
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 4 deletions.
diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp
@@ -865,7 +865,7 @@ namespace rats
     while (not_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);
+      not_solved = !preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, refzmp_exist_p, true);
       rg.update_refzmp(footstep_nodes_list);
     }
   };

diff --git a/rtc/AutoBalancer/PreviewController.h b/rtc/AutoBalancer/PreviewController.h
@@ -148,7 +148,7 @@ namespace rats
     {
         _qdata = qdata.front();
     };
-    bool is_doing () { return p.size() >= 1 + delay; };
+    bool is_doing (const size_t count_offset = 1) { return p.size() >= count_offset + delay; };
     bool is_end () { return ending_count <= 0 ; };
     void remove_preview_queue(const size_t remain_length)
     {
@@ -236,12 +236,12 @@ namespace rats
     preview_dynamics_filter(const double dt, const double zc, const hrp::Vector3& init_xk, const double _gravitational_acceleration = DEFAULT_GRAVITATIONAL_ACCELERATION, const double q = 1.0, const double r = 1.0e-6, const double d = 1.6)
         : preview_controller(dt, zc, init_xk, _gravitational_acceleration, q, r, d), finishedp(false) {};
     ~preview_dynamics_filter() {};
-    bool update(hrp::Vector3& p_ret, hrp::Vector3& x_ret, std::vector<hrp::Vector3>& qdata_ret, const hrp::Vector3& pr, const std::vector<hrp::Vector3>& qdata, const bool updatep)
+    bool update(hrp::Vector3& p_ret, hrp::Vector3& x_ret, std::vector<hrp::Vector3>& qdata_ret, const hrp::Vector3& pr, const std::vector<hrp::Vector3>& qdata, const bool updatep, const bool is_overwrite = false)
     {
       bool flg;
       if (updatep) {
         preview_controller.update_x_k(pr, qdata);
-        flg = preview_controller.is_doing();
+        flg = preview_controller.is_doing(is_overwrite?0:1);
       } else {
         if ( !preview_controller.is_end() )
           preview_controller.update_x_k();