From 2b8bb47f265f9c3ddabc158e82e65b45801cfc11 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sat, 9 Jan 2016 10:12:12 +0900 Subject: [PATCH] [rtc/AutoBalancer/GaitGenerator.cpp, rtc/AutoBalancer/PreviewController.h] fix bug when overwriting footstep --- rtc/AutoBalancer/GaitGenerator.cpp | 2 +- rtc/AutoBalancer/PreviewController.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 117e99306db..6dca3440606 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -865,7 +865,7 @@ namespace rats while (not_solved) { std::vector 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 index ab3d888fa10..a90ca3ff438 100644 --- 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& qdata_ret, const hrp::Vector3& pr, const std::vector& qdata, const bool updatep) + bool update(hrp::Vector3& p_ret, hrp::Vector3& x_ret, std::vector& qdata_ret, const hrp::Vector3& pr, const std::vector& 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();