From 6b6ffe94b0b22961605c82ded22b56c7d367d96d Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 11 Nov 2016 14:34:41 +0900 Subject: [PATCH 1/5] [AutoBalancer.cpp, GaitGenerator.*] calculate future cog based on LIPM dynamics --- rtc/AutoBalancer/AutoBalancer.cpp | 4 ++++ rtc/AutoBalancer/GaitGenerator.cpp | 35 ++++++++++++++++++++++++++++++ rtc/AutoBalancer/GaitGenerator.h | 7 ++++-- 3 files changed, 44 insertions(+), 2 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index c4b8a72ef3c..b24b986026c 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -923,6 +923,10 @@ void AutoBalancer::getTargetParameters() } multi_mid_coords(fix_leg_coords, tmp_end_coords_list); } + + { + std::vector future_d_ee_pos = gg->get_future_d_ee_pos(); + } }; hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2) diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index e0d07eeea84..56240ee898b 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -638,6 +638,41 @@ namespace rats // std::cerr << ")" << std::endl; // } + { + double omega = std::sqrt(gravitational_acceleration / cog(2) - refzmp(2)); + future_d_ee_pos.resize(all_limbs.size(), hrp::Vector3::Zero()); + std::vector swg_leg_stps = lcg.get_swing_leg_steps(); + if (lcg.get_lcg_count() <= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1 + && lcg.get_lcg_count() > static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * default_double_support_ratio_after) - 1) { // single support phase + double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * default_double_support_ratio_after; + hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + (cog - prev_cog) / dt / omega * std::sinh(omega * remain_time) + refzmp; + for (size_t i = 0; i < all_limbs.size(); i++) { + bool is_swing = false; + for (size_t j = 0; j < swg_leg_stps.size(); j++) { + if (all_limbs[i] == leg_type_map[swg_leg_stps[j].l_r]) { + future_d_ee_pos[i] = (prev_cog - swg_leg_stps[j].worldcoords.pos) - (future_cog - footstep_nodes_list[lcg.get_footstep_index()][j].worldcoords.pos); + is_swing = true; + } + } + if (!is_swing) future_d_ee_pos[i] = prev_cog - future_cog; + } + } else if (lcg.get_lcg_count() > static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1) { // double support (before) phase + double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * (1.0 - default_double_support_ratio_before); + hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * remain_time) + refzmp + (refzmp - prev_refzmp)/dt * remain_time; + for (size_t i = 0; i < all_limbs.size(); i++) { + future_d_ee_pos[i] = prev_cog - future_cog; + } + } else { // double support (after) phase + double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt; + hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * remain_time) + refzmp + (refzmp - prev_refzmp)/dt * remain_time; + for (size_t i = 0; i < all_limbs.size(); i++) { + future_d_ee_pos[i] = prev_cog - future_cog; + } + } + prev_cog = cog; + prev_refzmp = refzmp; + } + /* update swing_leg_coords, support_leg_coords */ if ( solved ) { lcg.update_leg_steps(footstep_nodes_list, default_double_support_ratio_swing_before, default_double_support_ratio_swing_after, thtc); diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 3c36016703d..1c3f6b1302c 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -1029,7 +1029,7 @@ namespace rats footstep_parameter footstep_param; velocity_mode_parameter vel_param, offset_vel_param; toe_heel_type_checker thtc; - hrp::Vector3 cog, refzmp, prev_que_rzmp; /* cog by calculating proc_one_tick */ + hrp::Vector3 cog, prev_cog, refzmp, prev_refzmp, prev_que_rzmp; /* cog by calculating proc_one_tick */ std::vector swing_foot_zmp_offsets, prev_que_sfzos; double dt; /* control loop [s] */ std::vector all_limbs; @@ -1054,6 +1054,7 @@ namespace rats double leg_margin[4], overwritable_stride_limitation[4]; bool use_stride_limitation; stride_limitation_type default_stride_limitation_type; + std::vector future_d_ee_pos; /* preview controller parameters */ //preview_dynamics_filter* preview_controller_ptr; @@ -1096,7 +1097,7 @@ namespace rats const double _stride_fwd_x, const double _stride_y, const double _stride_theta, const double _stride_bwd_x) : footstep_nodes_list(), overwrite_footstep_nodes_list(), rg(_dt), lcg(_dt), footstep_param(_leg_pos, _stride_fwd_x, _stride_y, _stride_theta, _stride_bwd_x), - vel_param(), offset_vel_param(), thtc(), cog(hrp::Vector3::Zero()), refzmp(hrp::Vector3::Zero()), prev_que_rzmp(hrp::Vector3::Zero()), + vel_param(), offset_vel_param(), thtc(), cog(hrp::Vector3::Zero()), prev_cog(hrp::Vector3::Zero()), refzmp(hrp::Vector3::Zero()), prev_refzmp(hrp::Vector3::Zero()), prev_que_rzmp(hrp::Vector3::Zero()), dt(_dt), all_limbs(_all_limbs), 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), @@ -1107,6 +1108,7 @@ namespace rats 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; + future_d_ee_pos.resize(_all_limbs.size(), hrp::Vector3::Zero()); }; ~gait_generator () { if ( preview_controller_ptr != NULL ) { @@ -1430,6 +1432,7 @@ namespace rats stride_limitation_type get_stride_limitation_type () const { return default_stride_limitation_type; }; double get_toe_check_thre () const { return thtc.get_toe_check_thre(); }; double get_heel_check_thre () const { return thtc.get_heel_check_thre(); }; + std::vector get_future_d_ee_pos () const { return future_d_ee_pos; }; void print_param (const std::string& print_str = "") const { double stride_fwd_x, stride_y, stride_th, stride_bwd_x; From 2ff25f80d2cc794aeddf9b50c269302312b6977f Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 11 Nov 2016 20:53:54 +0900 Subject: [PATCH 2/5] [AutoBalancerService.idl, AutoBalancer.*, GaitGenerator.*] calculate root link height to avoid limb stretching --- idl/AutoBalancerService.idl | 2 ++ rtc/AutoBalancer/AutoBalancer.cpp | 29 +++++++++++++++++++++++++---- rtc/AutoBalancer/AutoBalancer.h | 3 ++- rtc/AutoBalancer/GaitGenerator.cpp | 11 +++++++---- rtc/AutoBalancer/GaitGenerator.h | 1 + 5 files changed, 37 insertions(+), 9 deletions(-) diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index b0c6fb27da5..d0c3bdeffd8 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -301,6 +301,8 @@ module OpenHRP GaitType default_gait_type; /// Sequence for all end-effectors' ik limb parameters sequence ik_limb_parameters; + /// Sequence of limb length margin from max limb length [m] + sequence limb_length_margin; }; /** diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index b24b986026c..b53aa7a42e2 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -168,6 +168,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str()); coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str()); ABCIKparam tp; + hrp::Link* root = m_robot->link(ee_target); for (size_t j = 0; j < 3; j++) { coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); } @@ -192,6 +193,13 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() tp.avoid_gain = 0.001; tp.reference_gain = 0.01; tp.pos_ik_error_count = tp.rot_ik_error_count = 0; + tp.max_limb_length = 0.0; + while (!root->isRoot()) { + tp.max_limb_length += root->b.norm(); + tp.parent_name = root->name; + root = root->parent; + } + tp.limb_length_margin = 0.13; ikp.insert(std::pair(ee_name , tp)); ikp[ee_name].target_link = m_robot->link(ee_target); ee_vec.push_back(ee_name); @@ -477,6 +485,23 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) } else { rel_ref_zmp = input_zmp; } + + { + std::vector future_d_ee_pos = gg->get_future_d_ee_pos(); + double tmp_d_root_z_pos = 0.0; + for (size_t i = 0; i < leg_names.size(); i++) { + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(ikp[leg_names[i]].parent_name); + hrp::Vector3 future_targetp = ikp[leg_names[i]].target_p0 + future_d_ee_pos[i] - parent_link->p; // position from parent to target link in future (world frame) + double limb_length_limitation = ikp[leg_names[i]].max_limb_length - ikp[leg_names[i]].limb_length_margin; + double tmp = limb_length_limitation * limb_length_limitation - future_targetp(0) * future_targetp(0) - future_targetp(1) * future_targetp(1); + if (future_targetp.norm() > limb_length_limitation && tmp >= 0) { + tmp_d_root_z_pos = std::min(tmp_d_root_z_pos, future_targetp(2) + std::sqrt(tmp)); + } + } + gg->clear_future_d_ee_pos(); + } + // transition if (!is_transition_interpolator_empty) { // transition_interpolator_ratio 0=>1 : IDLE => ABC @@ -923,10 +948,6 @@ void AutoBalancer::getTargetParameters() } multi_mid_coords(fix_leg_coords, tmp_end_coords_list); } - - { - std::vector future_d_ee_pos = gg->get_future_d_ee_pos(); - } }; hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2) diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index 0b1723e6f5f..c0d62f18b0a 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -192,10 +192,11 @@ class AutoBalancer struct ABCIKparam { hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0; hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0; + std::string parent_name; // Name of parent ling in the limb rats::coordinates target_end_coords; hrp::Link* target_link; hrp::JointPathExPtr manip; - double avoid_gain, reference_gain; + double avoid_gain, reference_gain, max_limb_length, limb_length_margin; size_t pos_ik_error_count, rot_ik_error_count; bool is_active, has_toe_joint; }; diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 56240ee898b..be04e1f4543 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -646,27 +646,30 @@ namespace rats && lcg.get_lcg_count() > static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * default_double_support_ratio_after) - 1) { // single support phase double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * default_double_support_ratio_after; hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + (cog - prev_cog) / dt / omega * std::sinh(omega * remain_time) + refzmp; + future_cog(2) = cog(2); for (size_t i = 0; i < all_limbs.size(); i++) { bool is_swing = false; for (size_t j = 0; j < swg_leg_stps.size(); j++) { if (all_limbs[i] == leg_type_map[swg_leg_stps[j].l_r]) { - future_d_ee_pos[i] = (prev_cog - swg_leg_stps[j].worldcoords.pos) - (future_cog - footstep_nodes_list[lcg.get_footstep_index()][j].worldcoords.pos); + future_d_ee_pos[i] = (cog - swg_leg_stps[j].worldcoords.pos) - (future_cog - footstep_nodes_list[lcg.get_footstep_index()][j].worldcoords.pos); is_swing = true; } } - if (!is_swing) future_d_ee_pos[i] = prev_cog - future_cog; + if (!is_swing) future_d_ee_pos[i] = cog - future_cog; } } else if (lcg.get_lcg_count() > static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1) { // double support (before) phase double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * (1.0 - default_double_support_ratio_before); hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * remain_time) + refzmp + (refzmp - prev_refzmp)/dt * remain_time; + future_cog(2) = cog(2); for (size_t i = 0; i < all_limbs.size(); i++) { - future_d_ee_pos[i] = prev_cog - future_cog; + future_d_ee_pos[i] = cog - future_cog; } } else { // double support (after) phase double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt; hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * remain_time) + refzmp + (refzmp - prev_refzmp)/dt * remain_time; + future_cog(2) = cog(2); for (size_t i = 0; i < all_limbs.size(); i++) { - future_d_ee_pos[i] = prev_cog - future_cog; + future_d_ee_pos[i] = cog - future_cog; } } prev_cog = cog; diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 1c3f6b1302c..1bfc64e89fb 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -1272,6 +1272,7 @@ namespace rats void set_stride_limitation_type (const stride_limitation_type _tmp) { default_stride_limitation_type = _tmp; }; void set_toe_check_thre (const double _a) { thtc.set_toe_check_thre(_a); }; void set_heel_check_thre (const double _a) { thtc.set_heel_check_thre(_a); }; + void clear_future_d_ee_pos () { future_d_ee_pos.assign(all_limbs.size(), hrp::Vector3::Zero()); }; /* 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 { From 6280ae6f1fa80202b6030563c39a15b48adb7039 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Fri, 11 Nov 2016 22:02:51 +0900 Subject: [PATCH 3/5] [hrpsys_config.py, AutoBalancer.*, GaitGenerator.*, Stabilizer.*] interpolate root height to avoid limb stretching --- python/hrpsys_config.py | 1 + rtc/AutoBalancer/AutoBalancer.cpp | 18 +++++++++++++++++- rtc/AutoBalancer/AutoBalancer.h | 4 ++++ rtc/AutoBalancer/GaitGenerator.cpp | 18 +++++++++--------- rtc/AutoBalancer/GaitGenerator.h | 4 +++- rtc/Stabilizer/Stabilizer.cpp | 7 +++++++ rtc/Stabilizer/Stabilizer.h | 4 +++- 7 files changed, 44 insertions(+), 12 deletions(-) diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py index ee752060e13..45b0147ad26 100755 --- a/python/hrpsys_config.py +++ b/python/hrpsys_config.py @@ -393,6 +393,7 @@ def connectComps(self): connectPorts(self.abc.port("walkingStates"), self.st.port("walkingStates")) connectPorts(self.abc.port("sbpCogOffset"), self.st.port("sbpCogOffset")) connectPorts(self.abc.port("toeheelRatio"), self.st.port("toeheelRatio")) + connectPorts(self.abc.port("interpolatedRootHeight"), self.st.port("interpolatedRootHeight")) if self.es: connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal")) connectPorts(self.st.port("emergencySignal"), self.abc.port("emergencySignal")) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index b53aa7a42e2..146ee928b41 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -62,6 +62,7 @@ AutoBalancer::AutoBalancer(RTC::Manager* manager) m_walkingStatesOut("walkingStates", m_walkingStates), m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset), m_cogOut("cogOut", m_cog), + m_interpolatedRootHeightOut("interpolatedRootHeight", m_interpolatedRootHeight), m_AutoBalancerServicePort("AutoBalancerService"), // gait_type(BIPED), @@ -106,6 +107,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() addOutPort("cogOut", m_cogOut); addOutPort("walkingStates", m_walkingStatesOut); addOutPort("sbpCogOffset", m_sbpCogOffsetOut); + addOutPort("interpolatedRootHeight", m_interpolatedRootHeightOut); // Set service provider to Ports m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0); @@ -260,6 +262,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator"); leg_names_interpolator_ratio = 1.0; + limb_stretch_avoidance_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); + limb_stretch_avoidance_interpolator->setName(std::string(m_profile.instance_name)+" limb_stretch_avoidance_interpolator"); // setting stride limitations from conf file double stride_fwd_x_limit = 0.15; @@ -357,6 +361,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() rot_ik_thre = (1e-2)*M_PI/180.0; // [rad] ik_error_debug_print_freq = static_cast(0.2/m_dt); // once per 0.2 [s] + prev_d_root_z_pos = 0.0; + hrp::Sensor* sen = m_robot->sensor("gyrometer"); if (sen == NULL) { std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl; @@ -372,6 +378,7 @@ RTC::ReturnCode_t AutoBalancer::onFinalize() delete transition_interpolator; delete adjust_footstep_interpolator; delete leg_names_interpolator; + delete limb_stretch_avoidance_interpolator; return RTC::RTC_OK; } @@ -488,7 +495,8 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) { std::vector future_d_ee_pos = gg->get_future_d_ee_pos(); - double tmp_d_root_z_pos = 0.0; + double tmp_d_root_z_pos = 0.0, tmp_time = gg->get_limb_stretch_remain_time() + m_dt; + static double prev_v; for (size_t i = 0; i < leg_names.size(); i++) { // Check whether inside limb length limitation hrp::Link* parent_link = m_robot->link(ikp[leg_names[i]].parent_name); @@ -500,6 +508,12 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) } } gg->clear_future_d_ee_pos(); + limb_stretch_avoidance_interpolator->set(&prev_d_root_z_pos, &prev_v); + limb_stretch_avoidance_interpolator->setGoal(&tmp_d_root_z_pos, tmp_time, true); + for (size_t i = 0; i < 2; i++) { + limb_stretch_avoidance_interpolator->get(&d_root_z_pos, &prev_v, true); + } + m_interpolatedRootHeight.data = prev_d_root_z_pos = std::min(0.0, d_root_z_pos); } // transition @@ -606,6 +620,8 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) m_walkingStates.data = gg_is_walking; m_walkingStates.tm = m_qRef.tm; m_walkingStatesOut.write(); + m_interpolatedRootHeight.tm = m_qRef.tm; + m_interpolatedRootHeightOut.write(); for (unsigned int i=0; i *> m_ref_forceOut; std::vector m_limbCOPOffset; std::vector *> m_limbCOPOffsetOut; + TimedDouble m_interpolatedRootHeight; + OutPort m_interpolatedRootHeightOut; // for debug OutPort m_cogOut; @@ -242,12 +244,14 @@ class AutoBalancer double m_dt, move_base_gain; hrp::BodyPtr m_robot; coil::Mutex m_mutex; + double d_root_z_pos, prev_d_root_z_pos; double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio; interpolator *zmp_offset_interpolator; interpolator *transition_interpolator; interpolator *adjust_footstep_interpolator; interpolator *leg_names_interpolator; + interpolator *limb_stretch_avoidance_interpolator; hrp::Vector3 input_zmp, input_basePos; hrp::Matrix33 input_baseRot; diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index be04e1f4543..2aaff8c8a5a 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -642,10 +642,10 @@ namespace rats double omega = std::sqrt(gravitational_acceleration / cog(2) - refzmp(2)); future_d_ee_pos.resize(all_limbs.size(), hrp::Vector3::Zero()); std::vector swg_leg_stps = lcg.get_swing_leg_steps(); - if (lcg.get_lcg_count() <= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1 - && lcg.get_lcg_count() > static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * default_double_support_ratio_after) - 1) { // single support phase - double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * default_double_support_ratio_after; - hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + (cog - prev_cog) / dt / omega * std::sinh(omega * remain_time) + refzmp; + if (lcg.get_lcg_count() < static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1 + && lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * default_double_support_ratio_after) - 1) { // single support phase + limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * default_double_support_ratio_after; + hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + (cog - prev_cog) / dt / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp; future_cog(2) = cog(2); for (size_t i = 0; i < all_limbs.size(); i++) { bool is_swing = false; @@ -657,16 +657,16 @@ namespace rats } if (!is_swing) future_d_ee_pos[i] = cog - future_cog; } - } else if (lcg.get_lcg_count() > static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1) { // double support (before) phase - double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * (1.0 - default_double_support_ratio_before); - hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * remain_time) + refzmp + (refzmp - prev_refzmp)/dt * remain_time; + } else if (lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1) { // double support (before) phase + limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * (1.0 - default_double_support_ratio_before); + hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp + (refzmp - prev_refzmp)/dt * limb_stretch_remain_time; future_cog(2) = cog(2); for (size_t i = 0; i < all_limbs.size(); i++) { future_d_ee_pos[i] = cog - future_cog; } } else { // double support (after) phase - double remain_time = static_cast(lcg.get_lcg_count() + 1) * dt; - hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * remain_time) + refzmp + (refzmp - prev_refzmp)/dt * remain_time; + limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt + footstep_nodes_list[lcg.get_footstep_index() + (get_footstep_index() future_d_ee_pos; /* preview controller parameters */ @@ -1101,7 +1102,7 @@ namespace rats dt(_dt), all_limbs(_all_limbs), 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_stride_limitation(false), default_stride_limitation_type(SQUARE), + use_inside_step_limitation(true), use_stride_limitation(false), default_stride_limitation_type(SQUARE), limb_stretch_remain_time(0.0), 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()); @@ -1434,6 +1435,7 @@ namespace rats double get_toe_check_thre () const { return thtc.get_toe_check_thre(); }; double get_heel_check_thre () const { return thtc.get_heel_check_thre(); }; std::vector get_future_d_ee_pos () const { return future_d_ee_pos; }; + double get_limb_stretch_remain_time () const { return limb_stretch_remain_time; }; void print_param (const std::string& print_str = "") const { double stride_fwd_x, stride_y, stride_th, stride_bwd_x; diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index 3dd8c905661..942408300bb 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -55,6 +55,7 @@ Stabilizer::Stabilizer(RTC::Manager* manager) m_qRefSeqIn("qRefSeq", m_qRefSeq), m_walkingStatesIn("walkingStates", m_walkingStates), m_sbpCogOffsetIn("sbpCogOffset", m_sbpCogOffset), + m_interpolatedRootHeightIn("interpolatedRootHeight", m_interpolatedRootHeight), m_qRefOut("q", m_qRef), m_tauOut("tau", m_tau), m_zmpOut("zmp", m_zmp), @@ -115,6 +116,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize() addInPort("qRefSeq", m_qRefSeqIn); addInPort("walkingStates", m_walkingStatesIn); addInPort("sbpCogOffset", m_sbpCogOffsetIn); + addInPort("interpolatedRootHeight", m_interpolatedRootHeightIn); // Set OutPort buffer addOutPort("q", m_qRefOut); @@ -585,6 +587,10 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id) sbp_cog_offset(1) = m_sbpCogOffset.data.y; sbp_cog_offset(2) = m_sbpCogOffset.data.z; } + if (m_interpolatedRootHeightIn.isNew()){ + m_interpolatedRootHeightIn.read(); + interpolated_d_pos_z_root = m_interpolatedRootHeight.data; + } if (is_legged_robot) { getCurrentParameters(); @@ -1601,6 +1607,7 @@ void Stabilizer::limbStretchAvoidanceControl (const std::vector& e } } } + tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, interpolated_d_pos_z_root); // Change root link height depending on limb length double prev_d_pos_z_root = d_pos_z_root; d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const) : tmp_d_pos_z_root; diff --git a/rtc/Stabilizer/Stabilizer.h b/rtc/Stabilizer/Stabilizer.h index 27fe0476fdf..b5da70cd205 100644 --- a/rtc/Stabilizer/Stabilizer.h +++ b/rtc/Stabilizer/Stabilizer.h @@ -171,6 +171,7 @@ class Stabilizer RTC::TimedDoubleSeq m_qRefSeq; RTC::TimedBoolean m_walkingStates; RTC::TimedPoint3D m_sbpCogOffset; + RTC::TimedDouble m_interpolatedRootHeight; // for debug ouput RTC::TimedPoint3D m_originRefZmp, m_originRefCog, m_originRefCogVel, m_originNewZmp; RTC::TimedPoint3D m_originActZmp, m_originActCog, m_originActCogVel; @@ -196,6 +197,7 @@ class Stabilizer RTC::InPort m_qRefSeqIn; RTC::InPort m_walkingStatesIn; RTC::InPort m_sbpCogOffsetIn; + RTC::InPort m_interpolatedRootHeightIn; std::vector m_wrenches; std::vector *> m_wrenchesIn; @@ -293,7 +295,7 @@ class Stabilizer hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset; hrp::Vector3 foot_origin_offset[2]; std::vector prev_act_force_z; - double zmp_origin_off, transition_smooth_gain, d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2]; + double zmp_origin_off, transition_smooth_gain, d_pos_z_root, interpolated_d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2]; boost::shared_ptr > act_cogvel_filter; OpenHRP::StabilizerService::STAlgorithm st_algorithm; SimpleZMPDistributor* szd; From 7a0e23dee2c7401f211468396d20fee59294b1e9 Mon Sep 17 00:00:00 2001 From: YutaKojio Date: Sun, 13 Nov 2016 15:43:01 +0900 Subject: [PATCH 4/5] [AutoBalancer.*, GaitGenerator.*] add function to calculate future cog and interpolate limb strech --- rtc/AutoBalancer/AutoBalancer.cpp | 54 +++++++++++--------- rtc/AutoBalancer/AutoBalancer.h | 1 + rtc/AutoBalancer/GaitGenerator.cpp | 80 ++++++++++++++++-------------- rtc/AutoBalancer/GaitGenerator.h | 1 + 4 files changed, 75 insertions(+), 61 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 146ee928b41..eb7aaa7a82c 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -492,30 +492,6 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) } else { rel_ref_zmp = input_zmp; } - - { - std::vector future_d_ee_pos = gg->get_future_d_ee_pos(); - double tmp_d_root_z_pos = 0.0, tmp_time = gg->get_limb_stretch_remain_time() + m_dt; - static double prev_v; - for (size_t i = 0; i < leg_names.size(); i++) { - // Check whether inside limb length limitation - hrp::Link* parent_link = m_robot->link(ikp[leg_names[i]].parent_name); - hrp::Vector3 future_targetp = ikp[leg_names[i]].target_p0 + future_d_ee_pos[i] - parent_link->p; // position from parent to target link in future (world frame) - double limb_length_limitation = ikp[leg_names[i]].max_limb_length - ikp[leg_names[i]].limb_length_margin; - double tmp = limb_length_limitation * limb_length_limitation - future_targetp(0) * future_targetp(0) - future_targetp(1) * future_targetp(1); - if (future_targetp.norm() > limb_length_limitation && tmp >= 0) { - tmp_d_root_z_pos = std::min(tmp_d_root_z_pos, future_targetp(2) + std::sqrt(tmp)); - } - } - gg->clear_future_d_ee_pos(); - limb_stretch_avoidance_interpolator->set(&prev_d_root_z_pos, &prev_v); - limb_stretch_avoidance_interpolator->setGoal(&tmp_d_root_z_pos, tmp_time, true); - for (size_t i = 0; i < 2; i++) { - limb_stretch_avoidance_interpolator->get(&d_root_z_pos, &prev_v, true); - } - m_interpolatedRootHeight.data = prev_d_root_z_pos = std::min(0.0, d_root_z_pos); - } - // transition if (!is_transition_interpolator_empty) { // transition_interpolator_ratio 0=>1 : IDLE => ABC @@ -537,6 +513,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl; control_mode = MODE_IDLE; } + interpolateLimbStretch(); } if ( m_qRef.data.length() != 0 ) { // initialized if (is_legged_robot) { @@ -997,6 +974,30 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri m_robot->calcForwardKinematics(); } +void AutoBalancer::interpolateLimbStretch () +{ + std::vector future_d_ee_pos = gg->get_future_d_ee_pos(); + double tmp_d_root_z_pos = 0.0, tmp_time = gg->get_limb_stretch_remain_time() + m_dt; + static double prev_v; + for (size_t i = 0; i < leg_names.size(); i++) { + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(ikp[leg_names[i]].parent_name); + hrp::Vector3 future_targetp = ikp[leg_names[i]].target_p0 + future_d_ee_pos[i] - parent_link->p; // position from parent to target link in future (world frame) + double limb_length_limitation = ikp[leg_names[i]].max_limb_length - ikp[leg_names[i]].limb_length_margin; + double tmp = limb_length_limitation * limb_length_limitation - future_targetp(0) * future_targetp(0) - future_targetp(1) * future_targetp(1); + if (future_targetp.norm() > limb_length_limitation && tmp >= 0) { + tmp_d_root_z_pos = std::min(tmp_d_root_z_pos, future_targetp(2) + std::sqrt(tmp)); + } + } + gg->clear_future_d_ee_pos(); + limb_stretch_avoidance_interpolator->set(&prev_d_root_z_pos, &prev_v); + limb_stretch_avoidance_interpolator->setGoal(&tmp_d_root_z_pos, tmp_time, true); + for (size_t i = 0; i < 2; i++) { + limb_stretch_avoidance_interpolator->get(&d_root_z_pos, &prev_v, true); + } + m_interpolatedRootHeight.data = prev_d_root_z_pos = std::min(0.0, d_root_z_pos); +} + bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param, const std::string& limb_name) { param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio); @@ -1687,6 +1688,9 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) { gait_type = GALLOP; } + for (size_t i = 0; i < ikp.size(); i++) { + ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i]; + } for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; @@ -1790,11 +1794,13 @@ bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalanc { i_param.move_base_gain = move_base_gain; i_param.default_zmp_offsets.length(ikp.size()); + i_param.limb_length_margin.length(ikp.size()); for (size_t i = 0; i < ikp.size(); i++) { i_param.default_zmp_offsets[i].length(3); for (size_t j = 0; j < 3; j++) { i_param.default_zmp_offsets[i][j] = default_zmp_offsets[i](j); } + i_param.limb_length_margin[i] = ikp[ee_vec[i]].limb_length_margin; } switch(control_mode) { case MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_IDLE; break; diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index 972abf9ab36..17080ca0ce5 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -211,6 +211,7 @@ class AutoBalancer void waitABCTransition(); hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2); void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot); + void interpolateLimbStretch (); void startWalking (); void stopWalking (); void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs); diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 2aaff8c8a5a..c5f0190543c 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -638,43 +638,7 @@ namespace rats // std::cerr << ")" << std::endl; // } - { - double omega = std::sqrt(gravitational_acceleration / cog(2) - refzmp(2)); - future_d_ee_pos.resize(all_limbs.size(), hrp::Vector3::Zero()); - std::vector swg_leg_stps = lcg.get_swing_leg_steps(); - if (lcg.get_lcg_count() < static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1 - && lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * default_double_support_ratio_after) - 1) { // single support phase - limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * default_double_support_ratio_after; - hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + (cog - prev_cog) / dt / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp; - future_cog(2) = cog(2); - for (size_t i = 0; i < all_limbs.size(); i++) { - bool is_swing = false; - for (size_t j = 0; j < swg_leg_stps.size(); j++) { - if (all_limbs[i] == leg_type_map[swg_leg_stps[j].l_r]) { - future_d_ee_pos[i] = (cog - swg_leg_stps[j].worldcoords.pos) - (future_cog - footstep_nodes_list[lcg.get_footstep_index()][j].worldcoords.pos); - is_swing = true; - } - } - if (!is_swing) future_d_ee_pos[i] = cog - future_cog; - } - } else if (lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1) { // double support (before) phase - limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * (1.0 - default_double_support_ratio_before); - hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp + (refzmp - prev_refzmp)/dt * limb_stretch_remain_time; - future_cog(2) = cog(2); - for (size_t i = 0; i < all_limbs.size(); i++) { - future_d_ee_pos[i] = cog - future_cog; - } - } else { // double support (after) phase - limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt + footstep_nodes_list[lcg.get_footstep_index() + (get_footstep_index() swg_leg_stps = lcg.get_swing_leg_steps(); + if (lcg.get_lcg_count() < static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1 + && lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * default_double_support_ratio_after) - 1) { // single support phase + limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * default_double_support_ratio_after; + hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + (cog - prev_cog) / dt / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp; + future_cog(2) = cog(2); + for (size_t i = 0; i < all_limbs.size(); i++) { + bool is_swing = false; + for (size_t j = 0; j < swg_leg_stps.size(); j++) { + if (all_limbs[i] == leg_type_map[swg_leg_stps[j].l_r]) { + future_d_ee_pos[i] = (cog - swg_leg_stps[j].worldcoords.pos) - (future_cog - footstep_nodes_list[lcg.get_footstep_index()][j].worldcoords.pos); + is_swing = true; + } + } + if (!is_swing) future_d_ee_pos[i] = cog - future_cog; + } + } else if (lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1) { // double support (before) phase + limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt - footstep_nodes_list[lcg.get_footstep_index()][0].step_time * (1.0 - default_double_support_ratio_before); + hrp::Vector3 future_cog = (cog - refzmp) * std::cosh(omega * limb_stretch_remain_time) + ((cog - prev_cog)/dt - (refzmp - prev_refzmp)/dt) / omega * std::sinh(omega * limb_stretch_remain_time) + refzmp + (refzmp - prev_refzmp)/dt * limb_stretch_remain_time; + future_cog(2) = cog(2); + for (size_t i = 0; i < all_limbs.size(); i++) { + future_d_ee_pos[i] = cog - future_cog; + } + } else { // double support (after) phase + limb_stretch_remain_time = static_cast(lcg.get_lcg_count() + 1) * dt + footstep_nodes_list[lcg.get_footstep_index() + (get_footstep_index() x [mm], y [mm], theta [deg] diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 4e47749ee9c..e059a9af3a0 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -1123,6 +1123,7 @@ namespace rats const double delay = 1.6); bool proc_one_tick (); void limit_stride (step_node& cur_fs, const step_node& prev_fs) const; + void calc_future_cog (); void append_footstep_nodes (const std::vector& _legs, const std::vector& _fss) { std::vector tmp_sns; From 702a235597808e70a93bd12028ae895147316fe6 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Mon, 14 Nov 2016 17:31:31 +0900 Subject: [PATCH 5/5] [Stabilizer.cpp] smoothly change root height when switching use_limb_stretch_avoidance --- rtc/Stabilizer/Stabilizer.cpp | 35 +++++++++++++++++++---------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index 942408300bb..876db1c48f7 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -620,6 +620,7 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id) control_mode = MODE_AIR; break; } + if (control_mode != MODE_ST) d_pos_z_root = 0.0; } if ( m_robot->numJoints() == m_qRef.data.length() ) { if (is_legged_robot) { @@ -1510,7 +1511,6 @@ void Stabilizer::calcEEForceMomentControl() { // IK target is link origin pos and rot, not ee pos and rot. std::vector tmpp(stikp.size()); std::vector tmpR(stikp.size()); - double tmp_d_pos_z_root = 0.0; for (size_t i = 0; i < stikp.size(); i++) { if (is_ik_enable[i]) { // Add damping_control compensation to target value @@ -1530,7 +1530,7 @@ void Stabilizer::calcEEForceMomentControl() { } } - if (use_limb_stretch_avoidance) limbStretchAvoidanceControl(tmpp ,tmpR); + limbStretchAvoidanceControl(tmpp ,tmpR); // IK for (size_t i = 0; i < stikp.size(); i++) { @@ -1594,23 +1594,26 @@ void Stabilizer::calcSwingEEModification () void Stabilizer::limbStretchAvoidanceControl (const std::vector& ee_p, const std::vector& ee_R) { - double tmp_d_pos_z_root = 0.0; - for (size_t i = 0; i < stikp.size(); i++) { - if (is_ik_enable[i]) { - // Check whether inside limb length limitation - hrp::Link* parent_link = m_robot->link(stikp[i].parent_name); - hrp::Vector3 targetp = (ee_p[i] - ee_R[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame) - double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin; - double tmp = limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) - targetp(1) * targetp(1); - if (targetp.norm() > limb_length_limitation && tmp >= 0) { - tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, targetp(2) + std::sqrt(tmp)); + double tmp_d_pos_z_root = 0.0, prev_d_pos_z_root = d_pos_z_root; + if (use_limb_stretch_avoidance) { + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(stikp[i].parent_name); + hrp::Vector3 targetp = (ee_p[i] - ee_R[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame) + double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin; + double tmp = limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) - targetp(1) * targetp(1); + if (targetp.norm() > limb_length_limitation && tmp >= 0) { + tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, targetp(2) + std::sqrt(tmp)); + } } } + tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, interpolated_d_pos_z_root); + // Change root link height depending on limb length + d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const) : tmp_d_pos_z_root; + } else { + d_pos_z_root = calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const); } - tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, interpolated_d_pos_z_root); - // Change root link height depending on limb length - double prev_d_pos_z_root = d_pos_z_root; - d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const) : tmp_d_pos_z_root; d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + limb_stretch_avoidance_vlimit[0], prev_d_pos_z_root + limb_stretch_avoidance_vlimit[1]); m_robot->rootLink()->p(2) += d_pos_z_root; }