From 59ecced0190d78dddf4e2c0eee7c853cc6ecd271 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sun, 14 Aug 2016 20:57:42 +0900 Subject: [PATCH 1/3] [hrpsys_config.py, AutoBalancer.*, GaitGenerator.h, Stabilizer.*] get leg_margin from st --- python/hrpsys_config.py | 1 + rtc/AutoBalancer/AutoBalancer.cpp | 8 ++++++++ rtc/AutoBalancer/AutoBalancer.h | 2 ++ rtc/AutoBalancer/GaitGenerator.h | 5 +++++ rtc/Stabilizer/Stabilizer.cpp | 9 +++++++++ rtc/Stabilizer/Stabilizer.h | 2 ++ 6 files changed, 27 insertions(+) diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py index ed9138c9ca8..691654f4acd 100755 --- a/python/hrpsys_config.py +++ b/python/hrpsys_config.py @@ -389,6 +389,7 @@ def connectComps(self): if self.es: connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal")) connectPorts(self.st.port("emergencySignal"), self.abc.port("emergencySignal")) + connectPorts(self.st.port("legMargin"), self.abc.port("legMargin")) # ref force moment connection for sen in self.getForceSensorNames(): diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index a655e26c25b..df7f8971210 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -62,6 +62,7 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager) m_AutoBalancerServicePort("AutoBalancerService"), m_walkingStatesOut("walkingStates", m_walkingStates), m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset), + m_legMarginIn("legMargin", m_legMargin), // gait_type(BIPED), move_base_gain(0.8), @@ -90,6 +91,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() addInPort("zmpIn", m_zmpIn); addInPort("optionalData", m_optionalDataIn); addInPort("emergencySignal", m_emergencySignalIn); + addInPort("legMargin", m_legMarginIn); // Set OutPort buffer addOutPort("q", m_qOut); @@ -442,6 +444,12 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) // gg->emergency_stop(); // } } + if (m_legMarginIn.isNew()) { + m_legMarginIn.read(); + for (size_t i = 0; i < 4; i++) { + gg->set_leg_margin(m_legMargin.data[i], i); + } + } Guard guard(m_mutex); hrp::Vector3 ref_basePos; diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index be21747854d..fc60ba25e4d 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -133,6 +133,8 @@ class AutoBalancer std::vector *> m_ref_forceIn; TimedLong m_emergencySignal; InPort m_emergencySignalIn; + TimedDoubleSeq m_legMargin; + InPort m_legMarginIn; // for debug TimedPoint3D m_cog; diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index e39e77ff191..52c89ad6e64 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -880,6 +880,7 @@ namespace rats std::map leg_type_map; coordinates initial_foot_mid_coords; bool solved; + double leg_margin[4]; /* preview controller parameters */ //preview_dynamics_filter* preview_controller_ptr; @@ -931,6 +932,7 @@ namespace rats swing_foot_zmp_offsets = boost::assign::list_of(hrp::Vector3::Zero()); prev_que_sfzos = boost::assign::list_of(hrp::Vector3::Zero()); leg_type_map = boost::assign::map_list_of(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm"); + for (size_t i = 0; i < 4; i++) leg_margin[i] = 0.1; }; ~gait_generator () { if ( preview_controller_ptr != NULL ) { @@ -1072,6 +1074,9 @@ namespace rats append_finalize_footstep(overwrite_footstep_nodes_list); print_footstep_nodes_list(overwrite_footstep_nodes_list); }; + void set_leg_margin (const double _leg_margin, const size_t idx) { + leg_margin[idx] = _leg_margin; + }; /* Get overwritable footstep index. For example, if overwritable_footstep_index_offset = 1, overwrite next footstep. If overwritable_footstep_index_offset = 0, overwrite current swinging footstep. */ size_t get_overwritable_index () const { diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index ab8817e31b9..19bf149a3a1 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -62,6 +62,7 @@ Stabilizer::Stabilizer(RTC::Manager* manager) m_actContactStatesOut("actContactStates", m_actContactStates), m_COPInfoOut("COPInfo", m_COPInfo), m_emergencySignalOut("emergencySignal", m_emergencySignal), + m_legMarginOut("legMargin", m_legMargin), // for debug output m_originRefZmpOut("originRefZmp", m_originRefZmp), m_originRefCogOut("originRefCog", m_originRefCog), @@ -123,6 +124,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize() addOutPort("actContactStates", m_actContactStatesOut); addOutPort("COPInfo", m_COPInfoOut); addOutPort("emergencySignal", m_emergencySignalOut); + addOutPort("legMargin", m_legMarginOut); // for debug output addOutPort("originRefZmp", m_originRefZmpOut); addOutPort("originRefCog", m_originRefCogOut); @@ -389,6 +391,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize() total_mass = m_robot->totalMass(); ref_zmp_aux = hrp::Vector3::Zero(); m_actContactStates.data.length(m_contactStates.data.length()); + m_legMargin.data.length(4); for (size_t i = 0; i < m_contactStates.data.length(); i++) { contact_states.push_back(true); prev_contact_states.push_back(true); @@ -597,6 +600,12 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id) m_actContactStatesOut.write(); m_COPInfo.tm = m_qRef.tm; m_COPInfoOut.write(); + m_legMargin.data[0] = szd->get_leg_front_margin(); + m_legMargin.data[1] = szd->get_leg_rear_margin(); + m_legMargin.data[2] = szd->get_leg_outside_margin(); + m_legMargin.data[3] = szd->get_leg_inside_margin(); + m_legMargin.tm = m_qRef.tm; + m_legMarginOut.write(); //m_tauOut.write(); // for debug output m_originRefZmp.data.x = ref_zmp(0); m_originRefZmp.data.y = ref_zmp(1); m_originRefZmp.data.z = ref_zmp(2); diff --git a/rtc/Stabilizer/Stabilizer.h b/rtc/Stabilizer/Stabilizer.h index 35c8746b9f1..d8b3a0e1f12 100644 --- a/rtc/Stabilizer/Stabilizer.h +++ b/rtc/Stabilizer/Stabilizer.h @@ -163,6 +163,7 @@ class Stabilizer RTC::TimedDoubleSeq m_qRefSeq; RTC::TimedBoolean m_walkingStates; RTC::TimedPoint3D m_sbpCogOffset; + RTC::TimedDoubleSeq m_legMargin; // for debug ouput RTC::TimedPoint3D m_originRefZmp, m_originRefCog, m_originRefCogVel, m_originNewZmp; RTC::TimedPoint3D m_originActZmp, m_originActCog, m_originActCogVel; @@ -205,6 +206,7 @@ class Stabilizer RTC::OutPort m_actContactStatesOut; RTC::OutPort m_COPInfoOut; RTC::OutPort m_emergencySignalOut; + RTC::OutPort m_legMarginOut; // for debug output RTC::OutPort m_originRefZmpOut, m_originRefCogOut, m_originRefCogVelOut, m_originNewZmpOut; RTC::OutPort m_originActZmpOut, m_originActCogOut, m_originActCogVelOut; From ec237b7c395da30c103b0029499367ab00d30de2 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sun, 14 Aug 2016 21:00:53 +0900 Subject: [PATCH 2/3] [AutoBalancerService.idl, AutoBalancer.cpp, GaitGenerator.*] add function to limit stride --- idl/AutoBalancerService.idl | 2 ++ rtc/AutoBalancer/AutoBalancer.cpp | 4 ++++ rtc/AutoBalancer/GaitGenerator.cpp | 31 ++++++++++++++++++++++++++++++ rtc/AutoBalancer/GaitGenerator.h | 10 +++++++++- 4 files changed, 46 insertions(+), 1 deletion(-) diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index ccbc974b5bc..57be560dcf3 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -221,6 +221,8 @@ module OpenHRP long optional_go_pos_finalize_footstep_num; /// Offset for overwritable footstep index. Offset from current footstep index. Used in emergency_stop and velocity_mode. long overwritable_footstep_index_offset; + /// Stride limitation when overwriting footsteps (front, rear, outside, inside) [m] + DblArray4 overwritable_stride_limitation; }; /** diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index df7f8971210..26bb17c81d7 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -1420,6 +1420,7 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai gg->set_zmp_weight_map(boost::assign::map_list_of(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3])); gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num); gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset); + gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation); // print gg->print_param(std::string(m_profile.instance_name)); @@ -1489,6 +1490,9 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM]; i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num(); i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset(); + for (size_t i=0; i<4; i++) { + i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i); + } return true; }; diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 1be6d4390a9..df2ded95f88 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -614,6 +614,37 @@ namespace rats return solved; }; + void gait_generator::limit_stride (step_node& cur_fs, const step_node& prev_fs) + { + leg_type cur_leg = cur_fs.l_r; + // prev_fs frame + cur_fs.worldcoords.pos = prev_fs.worldcoords.rot.transpose() * (cur_fs.worldcoords.pos - prev_fs.worldcoords.pos); + double stride_r = std::pow(cur_fs.worldcoords.pos(0), 2.0) + std::pow(cur_fs.worldcoords.pos(1) + (cur_leg == LLEG ? -1 : 1) * overwritable_stride_limitation[3], 2.0); + // front, rear, outside limitation + double stride_r_limit = std::pow(std::max(overwritable_stride_limitation[cur_fs.worldcoords.pos(0) >= 0 ? 0 : 1], overwritable_stride_limitation[2] - overwritable_stride_limitation[3]), 2.0); + if (stride_r > stride_r_limit) { + cur_fs.worldcoords.pos(0) *= sqrt(stride_r_limit / stride_r); + cur_fs.worldcoords.pos(1) = (cur_leg == LLEG ? 1 : -1) * overwritable_stride_limitation[3] + sqrt(stride_r_limit / stride_r) * (cur_fs.worldcoords.pos(1) + (cur_leg == LLEG ? -1 : 1) * overwritable_stride_limitation[3]); + } + if (cur_fs.worldcoords.pos(0) > overwritable_stride_limitation[0]) cur_fs.worldcoords.pos(0) = overwritable_stride_limitation[0]; + if (cur_fs.worldcoords.pos(0) < -1 * overwritable_stride_limitation[0]) cur_fs.worldcoords.pos(0) = -1 * overwritable_stride_limitation[1]; + if ((cur_leg == LLEG ? 1 : -1) * cur_fs.worldcoords.pos(1) > overwritable_stride_limitation[2]) cur_fs.worldcoords.pos(1) = (cur_leg == LLEG ? 1 : -1) * overwritable_stride_limitation[2]; + // inside limitation + std::vector cur_leg_vertices_y; + cur_leg_vertices_y.reserve(4); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1)); + if (cur_leg == LLEG) { + if (*std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) < overwritable_stride_limitation[3]) cur_fs.worldcoords.pos(1) += overwritable_stride_limitation[3] - *std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()); + } else { + if (*std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) > -1 * overwritable_stride_limitation[3]) cur_fs.worldcoords.pos(1) += -1 * overwritable_stride_limitation[3] - *std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()); + } + // world frame + cur_fs.worldcoords.pos = prev_fs.worldcoords.pos + prev_fs.worldcoords.rot * cur_fs.worldcoords.pos; + }; + /* generate vector of step_node from :go-pos params * x, y and theta are simply divided by using stride params * unit system -> x [mm], y [mm], theta [deg] diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 52c89ad6e64..7bf3ed88756 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -880,7 +880,7 @@ namespace rats std::map leg_type_map; coordinates initial_foot_mid_coords; bool solved; - double leg_margin[4]; + double leg_margin[4], overwritable_stride_limitation[4]; /* preview controller parameters */ //preview_dynamics_filter* preview_controller_ptr; @@ -933,6 +933,7 @@ namespace rats prev_que_sfzos = boost::assign::list_of(hrp::Vector3::Zero()); leg_type_map = boost::assign::map_list_of(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm"); for (size_t i = 0; i < 4; i++) leg_margin[i] = 0.1; + for (size_t i = 0; i < 4; i++) overwritable_stride_limitation[i] = 0.2; }; ~gait_generator () { if ( preview_controller_ptr != NULL ) { @@ -945,6 +946,7 @@ namespace rats const std::vector& initial_swing_leg_dst_steps, const double delay = 1.6); bool proc_one_tick (); + void limit_stride (step_node& cur_fs, const step_node& prev_fs); void append_footstep_nodes (const std::vector& _legs, const std::vector& _fss) { std::vector tmp_sns; @@ -1077,6 +1079,11 @@ namespace rats void set_leg_margin (const double _leg_margin, const size_t idx) { leg_margin[idx] = _leg_margin; }; + void set_overwritable_stride_limitation (const double _overwritable_stride_limitation[4]) { + for (size_t i = 0; i < 4; i++) { + overwritable_stride_limitation[i] = _overwritable_stride_limitation[i]; + } + }; /* Get overwritable footstep index. For example, if overwritable_footstep_index_offset = 1, overwrite next footstep. If overwritable_footstep_index_offset = 0, overwrite current swinging footstep. */ size_t get_overwritable_index () const { @@ -1228,6 +1235,7 @@ namespace rats size_t get_optional_go_pos_finalize_footstep_num () const { return optional_go_pos_finalize_footstep_num; }; bool is_finalizing (const double tm) const { return ((preview_controller_ptr->get_delay()*2 - default_step_time/dt)-finalize_count) <= (tm/dt)-1; }; size_t get_overwrite_check_timing () const { return static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1;}; // Almost middle of step time + double get_overwritable_stride_limitation (const size_t idx) const { return overwritable_stride_limitation[idx]; }; void print_param (const std::string& print_str = "") const { double stride_fwd_x, stride_y, stride_th, stride_bwd_x; From c1f5655a3bd0115a18c48c55963b92fab9c3f18a Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sun, 14 Aug 2016 21:01:48 +0900 Subject: [PATCH 3/3] [AutoBalancerService.idl, AutoBalancer.cpp, GaitGenerator.*] limit stride when use_stride_limitation is true --- idl/AutoBalancerService.idl | 2 ++ rtc/AutoBalancer/AutoBalancer.cpp | 2 ++ rtc/AutoBalancer/GaitGenerator.cpp | 16 ++++++++++++++++ rtc/AutoBalancer/GaitGenerator.h | 5 ++++- 4 files changed, 24 insertions(+), 1 deletion(-) diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index 57be560dcf3..fd2e416bd47 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -223,6 +223,8 @@ module OpenHRP long overwritable_footstep_index_offset; /// Stride limitation when overwriting footsteps (front, rear, outside, inside) [m] DblArray4 overwritable_stride_limitation; + /// Use stride limitation or not + boolean use_stride_limitation; }; /** diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 26bb17c81d7..54b429b22e1 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -1421,6 +1421,7 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num); gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset); gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation); + gg->set_use_stride_limitation(i_param.use_stride_limitation); // print gg->print_param(std::string(m_profile.instance_name)); @@ -1493,6 +1494,7 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener for (size_t i=0; i<4; i++) { i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i); } + i_param.use_stride_limitation = gg->get_use_stride_limitation(); return true; }; diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index df2ded95f88..8593138321a 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -578,6 +578,22 @@ namespace rats overwrite_footstep_nodes_list.clear(); } } + // limit stride + if (use_stride_limitation && lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-overwritable_footstep_index_offset-2 && + (overwritable_footstep_index_offset == 0 || lcg.get_lcg_count() == get_overwrite_check_timing())) { + if (lcg.get_footstep_index() == footstep_nodes_list.size()-overwritable_footstep_index_offset-3) { + hrp::Vector3 orig_footstep_pos = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos; + limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front()); + for (size_t i = get_overwritable_index() + 1; i < footstep_nodes_list.size(); i++) { + footstep_nodes_list[i].front().worldcoords.pos -= orig_footstep_pos - footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos; + } + } else { + limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front()); + } + overwrite_footstep_nodes_list.insert(overwrite_footstep_nodes_list.end(), footstep_nodes_list.begin()+get_overwritable_index(), footstep_nodes_list.end()); + overwrite_refzmp_queue(overwrite_footstep_nodes_list); + overwrite_footstep_nodes_list.clear(); + } if ( !solved ) { hrp::Vector3 rzmp; diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 7bf3ed88756..1f570ca42be 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -881,6 +881,7 @@ namespace rats coordinates initial_foot_mid_coords; bool solved; double leg_margin[4], overwritable_stride_limitation[4]; + bool use_stride_limitation; /* preview controller parameters */ //preview_dynamics_filter* preview_controller_ptr; @@ -927,7 +928,7 @@ namespace rats dt(_dt), default_step_time(1.0), default_double_support_ratio_before(0.1), default_double_support_ratio_after(0.1), default_double_support_static_ratio_before(0.0), default_double_support_static_ratio_after(0.0), default_double_support_ratio_swing_before(0.1), default_double_support_ratio_swing_after(0.1), gravitational_acceleration(DEFAULT_GRAVITATIONAL_ACCELERATION), finalize_count(0), optional_go_pos_finalize_footstep_num(0), overwrite_footstep_index(0), overwritable_footstep_index_offset(1), velocity_mode_flg(VEL_IDLING), emergency_flg(IDLING), - use_inside_step_limitation(true), + use_inside_step_limitation(true), use_stride_limitation(false), preview_controller_ptr(NULL) { swing_foot_zmp_offsets = boost::assign::list_of(hrp::Vector3::Zero()); prev_que_sfzos = boost::assign::list_of(hrp::Vector3::Zero()); @@ -1084,6 +1085,7 @@ namespace rats overwritable_stride_limitation[i] = _overwritable_stride_limitation[i]; } }; + void set_use_stride_limitation (const bool _use_stride_limitation) { use_stride_limitation = _use_stride_limitation; }; /* Get overwritable footstep index. For example, if overwritable_footstep_index_offset = 1, overwrite next footstep. If overwritable_footstep_index_offset = 0, overwrite current swinging footstep. */ size_t get_overwritable_index () const { @@ -1236,6 +1238,7 @@ namespace rats bool is_finalizing (const double tm) const { return ((preview_controller_ptr->get_delay()*2 - default_step_time/dt)-finalize_count) <= (tm/dt)-1; }; size_t get_overwrite_check_timing () const { return static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1;}; // Almost middle of step time double get_overwritable_stride_limitation (const size_t idx) const { return overwritable_stride_limitation[idx]; }; + bool get_use_stride_limitation () const { return use_stride_limitation; }; void print_param (const std::string& print_str = "") const { double stride_fwd_x, stride_y, stride_th, stride_bwd_x;